Commit f8899d57 authored by mastefan's avatar mastefan
Browse files

VM: trajectory kind of working

parent a1e3233b
......@@ -233,6 +233,7 @@ float tol_land[3] = {0.03, 0.03, 0.03}; // *
// Origin of the Flying zone
float trajectory_setpoint[4]; // x,y,z,yaw
float trajectory_velocity[3]; // x,y,z
float originX = 0;
float originY = 0;
d_fall_pps::AreaBounds area;
......@@ -240,7 +241,7 @@ double trajectory_start_time = 0;
double total_time_since_start = 0;
float trajectory_x_radius = 0;
float trajectory_y_radius = 0;
float trajectory_period = 10; // 1 round in 10 sec
float trajectory_period = 5; // 1 round in 5 sec
......
......@@ -240,6 +240,7 @@ void buttonPressed_follow_trajectory(){
trajectory_x_radius = 0;
trajectory_y_radius = 0;
trajectory_start_time = ros::Time::now().toSec();
ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time);
}
......@@ -652,6 +653,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
construct_and_publish_debug_message(request,response);
}
// RETURN "true" TO INDICATE THAT THE COMPUTATIONS WERE SUCCESSFUL
return true;
}
......@@ -919,6 +921,10 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/
// Load Mothership velocity
// x dot, y dot, z dot
float ms_velocity[3];
if(flying_state == DRONEX_FOLLOW_TRAJECTORY){
calculateTrajectory();
......@@ -927,20 +933,27 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
ms_coordinates[1] = trajectory_setpoint[1];
ms_coordinates[2] = trajectory_setpoint[2];
ms_coordinates[3] = trajectory_setpoint[3];
ms_velocity[0] = trajectory_velocity[0];
ms_velocity[1] = trajectory_velocity[1];
ms_velocity[2] = trajectory_velocity[2];
ms_velocity[0] = 0;
ms_velocity[1] = 0;
ms_velocity[2] = 0;
}else{
ms_coordinates[0] = m_setpoint_for_controller[0];
ms_coordinates[1] = m_setpoint_for_controller[1];
ms_coordinates[2] = m_setpoint_for_controller[2];
ms_coordinates[3] = request.otherCrazyflies[0].yaw;
}
// Load Mothership velocity
// x dot, y dot, z dot
float ms_velocity[3];
ms_velocity[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2];
ms_velocity[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2];
}
......@@ -1068,7 +1081,14 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// to see setpoint in lqr plots
Setpoint msg_setpoint;
msg_setpoint.x = ms_coordinates[0];
msg_setpoint.y = ms_coordinates[1];
msg_setpoint.z = ms_coordinates[2];
msg_setpoint.yaw = ms_coordinates[3];
dronexSetpointToGUIPublisher.publish(msg_setpoint);
......@@ -1105,14 +1125,21 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
void calculateTrajectory(){
trajectory_x_radius = (area.xmax - area.xmin)/2.0;
trajectory_y_radius = (area.ymax - area.ymin)/2.0;
total_time_since_start = trajectory_start_time - ros::Time::now().toSec();
trajectory_x_radius = 0.2;//(area.xmax - area.xmin)/2.0;
ROS_INFO_STREAM("area_x (max,min) " << area.xmax << ", " << area.xmin);
trajectory_y_radius = 0.4;//(area.ymax - area.ymin)/2.0;
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
ROS_INFO_STREAM("radius (x,y) " << trajectory_x_radius << ", " << trajectory_y_radius);
// Nicht ganz an den Rand -> - 0.3
trajectory_setpoint[0] = originX + cos(2*PI/trajectory_period * total_time_since_start)*trajectory_x_radius-0.3;
trajectory_setpoint[1] = originY + sin(2*PI/trajectory_period * total_time_since_start)*trajectory_y_radius-0.3;
trajectory_setpoint[0] = originX + cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius);
trajectory_setpoint[1] = originY + sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
trajectory_setpoint[2] = 0.5;
trajectory_setpoint[3] = 0;
trajectory_velocity[0] = -2*PI/trajectory_period * sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius);
trajectory_velocity[1] = 2*PI/trajectory_period * cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
trajectory_velocity[2] = 0;
}
......@@ -1124,10 +1151,12 @@ void calculateTrajectory(){
// Set motors Output to 0
void motorsOFF(Controller::Response &response){
response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
float feed_forward_thrust_per_motor = m_mass_total_grams * 9.81/(1000*4);
// > Put in the per motor commands
response.controlOutput.motorCmd1 = /*0.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = /*0.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = /*0.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = /*0.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
......@@ -2070,7 +2099,7 @@ void integrator_XYZ(float (&stateErrorBody)[12])
{
if(integratorFlag == DRONEX_INTEGRATOR_ON)
{
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
//ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
for(int i=0; i < 3; i++)
{
integrator_sum_XYZ[i] += stateErrorBody[i]*(1.0/control_frequency);
......
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