Commit 258c8ac5 authored by phfriedl's avatar phfriedl
Browse files

added student controller framework called CustomControllerService---hopefully...

added student controller framework called CustomControllerService---hopefully now it actually pushes the file..
parent b9f3c869
// Place for students to implement their controller
// The ROS::service is set to work
//some useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
//the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
//constants
#define PI 3.1415926535
#define RATE_MODE 0
#define ANGLE_MODE 1
#define MOTOR_MODE 2
//constats that you most probably need for your controller to work properly
//see: 2015-08 - Forster - System ID of Crazyflie 2.pdf Chapter 3.3.1: Input Command → Thrust
const std::vector<float> FEEDFORWARD_MOTOR() = {37000, 37000, 37000, 37000};
const std::vector<float> MOTOR_REGRESSION_POLYNOMIAL() = {5.484560e-4, 1.032633e-6, 2.130295e-11};
const float SATURATION_THRUST = MOTOR_REGRESSION_POLYNOMIAL(2) * 12000 * 12000 + MOTOR_REGRESSION_POLYNOMIAL(1) * 12000 + MOTOR_REGRESSION_POLYNOMIAL(0);
//namespacing the package
using namespace d_fall_pps;
// ********* important function that has to be used!! *********
//-est- is an array with the estimated values : x,y,z,vx,vy,vz,roll,pitch,yaw
//-estBody- is an EMPTY array which will then contain the values in the body frame used by the crazyflie
//-yaw_measured- is the value that came from Vicon
void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured) {
float sinYaw = sin(yaw_measured);
float cosYaw = cos(yaw_measured);
state[0] = est[0] * cosYaw + est[1] * sinYaw;
state[1] = -est[0] * sinYaw + est[1] * cosYaw;
state[2] = est[2];
state[3] = est[3] * cosYaw + est[4] * sinYaw;
state[4] = -est[3] * sinYaw + est[4] * cosYaw;
state[5] = est[5];
state[6] = est[6];
state[7] = est[7];
state[8] = est[8];
}
/* --- the data students can work with ---
-request- contains data provided by Vicon. Check d_fall_pps/msg/ViconData.msg what it includes.
-response- is where you have to write your calculated data into.
*/
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
//writing the data from -request- to command line
//might be useful for debugging
ROS_INFO_STREAM("x-coordinates: " << request.crazyflieLocation.x);
ROS_INFO_STREAM("y-coordinates: " << request.crazyflieLocation.y);
ROS_INFO_STREAM("z-coordinates: " << request.crazyflieLocation.z);
ROS_INFO_STREAM("roll: " << request.crazyflieLocation.roll);
ROS_INFO_STREAM("pitch: " << request.crazyflieLocation.pitch);
ROS_INFO_STREAM("yaw: " << request.crazyflieLocation.yaw);
ROS_INFO_STREAM("Delta t: " << request.crazyflieLocation.acquiringTime);
// ********* do your calculations here *********
//Tip: create functions that you call here to keep you code cleaner
//for students to set the newly calculated commands for the controller
response.controlOutput.roll = 0;
response.controlOutput.pitch = 0;
response.controlOutput.yaw = 0;
response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
/*choosing the Crazyflie onBoard controller type.
it can either be Motor, Rate or Angle based */
response.controlOutput.onboardControllerType = MOTOR_MODE;
//response.controlOutput.onboardControllerType = RATE_MODE;
//response.controlOutput.onboardControllerType = ANGLE_MODE;
return true;
}
int main(int argc, char* argv[]) {
//starting the ROS-node
ros::init(argc, argv, "CustomControllerService");
ros::NodeHandle nodeHandle("~");
ros::ServiceServer service = nodeHandle.advertiseService("CustomController", calculateControlOutput);
ROS_INFO("CustomControllerService ready");
ros::spin();
return 0;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment