Commit 9272a68c authored by mastefan's avatar mastefan
Browse files

VM: xm2 on all sides

parent aa97f2c2
...@@ -240,17 +240,6 @@ std::vector<float> tol_land (3, 0.0); ...@@ -240,17 +240,6 @@ std::vector<float> tol_land (3, 0.0);
//TODO we might want to use float64 instead of float
// trajectory generation variables
double trajectory_start_time = 0;
double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// lookahead for trajectory: fetched from .yaml
float trajectory_deltaT_position = 0;
float trajectory_deltaT_velocity = 0;
// Trajectory: xcf0 -> xm1 -> xm2 -> xms // Trajectory: xcf0 -> xm1 -> xm2 -> xms
// xcf0: inital(Button pressed) position of Crazyflie // xcf0: inital(Button pressed) position of Crazyflie
// xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1 // xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1
...@@ -263,14 +252,24 @@ std::vector<float> xms(3, 0.0); ...@@ -263,14 +252,24 @@ std::vector<float> xms(3, 0.0);
std::vector<float> direction_ms_to_cf(3, 0.0); std::vector<float> direction_ms_to_cf(3, 0.0);
std::vector<float> direction_ms_to_cf_ms_bodyframe(3, 0.0); std::vector<float> direction_ms_to_cf_ms_bodyframe(3, 0.0);
float xm2_sign = 1.0; float xm2_sign[2] = {1.0, 0.0}; // sign for (x,y) in {-1, 0, 1}
//TODO we might want to use float64 instead of float
// trajectory generation variables
double trajectory_start_time = 0;
double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// lookahead for trajectory: fetched from .yaml
float trajectory_deltaT_position = 0;
float trajectory_deltaT_velocity = 0;
float trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1 float trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
float trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2 float trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
float trajectory_duration_3 = 0; // Duration of flight between xm2 and xms float trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
float trajectory_t0 = 0; // Time when Button pressed to land on mothership
float trajectory_t1 = 0; // Time at xm1 float trajectory_t1 = 0; // Time at xm1
float trajectory_t2 = 0; // Time at xm2 float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms float trajectory_t3 = 0; // Time at xms
...@@ -297,6 +296,8 @@ float trajectory_velocity_of_CF; // fetched from yaml ...@@ -297,6 +296,8 @@ float trajectory_velocity_of_CF; // fetched from yaml
float trajectory_total_distance = 0; float trajectory_total_distance = 0;
float trajectory_duration = 0; float trajectory_duration = 0;
float acceleration[3] = {0.5, 0.5, 0.1};
// returned trajectory setpoint & velocity // returned trajectory setpoint & velocity
......
...@@ -34,7 +34,7 @@ xm2_height_over_ms: 0.3 ...@@ -34,7 +34,7 @@ xm2_height_over_ms: 0.3
xm1_height: 0.6 xm1_height: 0.6
# velocity of trajctory # velocity of trajctory
trajectory_velocity_of_CF : 3.0 trajectory_velocity_of_CF : 2.50
trajectory_velocity_of_CF_init : 1 trajectory_velocity_of_CF_init : 1
xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms
xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms
......
...@@ -288,7 +288,6 @@ void buttonPressed_integrator_reset(){ ...@@ -288,7 +288,6 @@ void buttonPressed_integrator_reset(){
trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2 trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
trajectory_duration_3 = 0; // Duration of flight between xm2 and xms trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
trajectory_t0 = 0; // Time when Button pressed to land on mothership
trajectory_t1 = 0; // Time at xm1 trajectory_t1 = 0; // Time at xm1
trajectory_t2 = 0; // Time at xm2 trajectory_t2 = 0; // Time at xm2
...@@ -371,8 +370,6 @@ void buttonPressed_follow_trajectory(){ ...@@ -371,8 +370,6 @@ void buttonPressed_follow_trajectory(){
//trajectory_y_radius = 0; //trajectory_y_radius = 0;
first_trajectory_calculation = true; first_trajectory_calculation = true;
trajectory_start_time = ros::Time::now().toSec(); trajectory_start_time = ros::Time::now().toSec();
trajectory_t0 = 0.0;
//trajectory_t0 = ros::Time::now().toSec();
ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time); ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time);
...@@ -556,8 +553,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -556,8 +553,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
tookOffFlag = false; tookOffFlag = false;
approachedFlag = false; approachedFlag = false;
//bool landedFlag = true; //bool landedFlag = true;
taking_off_from_ms_height_gain = false;
integratorFlag == DRONEX_INTEGRATOR_OFF; integratorFlag = DRONEX_INTEGRATOR_OFF;
dronexSetpoint.x = request.ownCrazyflie.x; dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y; dronexSetpoint.y = request.ownCrazyflie.y;
...@@ -576,6 +574,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -576,6 +574,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
tookOffFlag = false; tookOffFlag = false;
approachedFlag = false; approachedFlag = false;
//bool landedFlag = true; //bool landedFlag = true;
taking_off_from_ms_height_gain = true;
integratorFlag = DRONEX_INTEGRATOR_OFF;
dronexSetpoint.x = request.ownCrazyflie.x; dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y; dronexSetpoint.y = request.ownCrazyflie.y;
...@@ -586,7 +587,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -586,7 +587,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_LAND_ON_MOTHERSHIP: case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{ {
integratorFlag == DRONEX_INTEGRATOR_ON; integratorFlag = DRONEX_INTEGRATOR_ON;
//if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] && //if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
// abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] && // abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
// abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){ // abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){
...@@ -594,7 +595,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -594,7 +595,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP"); //ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
dronexSetpoint.x = request.otherCrazyflies[0].x; dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y; dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.03; dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
//}else { //}else {
// //
...@@ -622,10 +623,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -622,10 +623,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{ {
//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF"); //ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
thrust_factor = 1; thrust_factor = 1;
integratorFlag = DRONEX_INTEGRATOR_ON;
//Check where we start from, if from mothership first go straight up to a certain height //Check where we start from, if from mothership first go straight up to a certain height
// before aiming the startCoordinates. // before aiming the startCoordinates.
if(previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP && taking_off_from_ms_height_gain){ if(/*previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP && */taking_off_from_ms_height_gain){
if(!savedStartCoordinatesFromMS) if(!savedStartCoordinatesFromMS)
{ {
startCoordinateFromMS[0] = request.otherCrazyflies[0].x; startCoordinateFromMS[0] = request.otherCrazyflies[0].x;
...@@ -641,95 +643,71 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -641,95 +643,71 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
} }
dronexSetpoint.x = startCoordinateFromMS[0];
dronexSetpoint.y = startCoordinateFromMS[1];
dronexSetpoint.z = startCoordinateFromMS[2] + 0.4;
dronexSetpoint.x = startCoordinateFromMS[0];
dronexSetpoint.y = startCoordinateFromMS[1];
dronexSetpoint.z = startCoordinateFromMS[1] + 0.4;
// For debugging the integrator
// ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
// << request.ownCrazyflie.x-dronexSetpoint.x << ", "
// << request.ownCrazyflie.y-dronexSetpoint.y << ", "
// << request.ownCrazyflie.z-dronexSetpoint.z << ")");
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] && if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) { abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
//tookOffFlag = true; //tookOffFlag = true;
//ROS_INFO("took off");
//ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER"); ROS_INFO_STREAM("Entering from MS-takeoff: ground-takeoff");
//previous_flying_state = flying_state; //previous_flying_state = flying_state;
//flying_state = DRONEX_STATE_HOVER; //flying_state = DRONEX_STATE_HOVER;
//reset_rviz = true; //reset_rviz = true;
taking_off_from_ms_height_gain = false; taking_off_from_ms_height_gain = false;
savedStartCoordinatesFromMS = false;
} }
}else{ // previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP
}else{ // previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP
if(!savedStartCoordinates)
{
startCoordinateX = request.ownCrazyflie.x;
startCoordinateY = request.ownCrazyflie.y;
startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true;
ROS_INFO_STREAM("DRONEX: saved start Coordinates");
ROS_INFO_STREAM("x = " << startCoordinateX);
ROS_INFO_STREAM("y = " << startCoordinateY);
ROS_INFO_STREAM("z = " << startCoordinateZ);
}
if(!savedStartCoordinates)
{
startCoordinateX = request.ownCrazyflie.x;
startCoordinateY = request.ownCrazyflie.y;
startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true;
ROS_INFO_STREAM("DRONEX: saved start Coordinates");
ROS_INFO_STREAM("x = " << startCoordinateX);
ROS_INFO_STREAM("y = " << startCoordinateY);
ROS_INFO_STREAM("z = " << startCoordinateZ);
}
dronexSetpoint.x = startCoordinateX; dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY; dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4; dronexSetpoint.z = startCoordinateZ + 0.4;
// For debugging the integrator
// ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
// << request.ownCrazyflie.x-dronexSetpoint.x << ", "
// << request.ownCrazyflie.y-dronexSetpoint.y << ", "
// << request.ownCrazyflie.z-dronexSetpoint.z << ")");
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
ROS_INFO("took off");
tookOffFlag = true;
ROS_INFO_STREAM("Entering from ground-takeoff: DRONEX_STATE_HOVER");
// For debugging the integrator previous_flying_state = flying_state;
// ROS_INFO_STREAM("TO: (x,y,z) Difference: (" flying_state = DRONEX_STATE_HOVER;
// << request.ownCrazyflie.x-dronexSetpoint.x << ", " taking_off_from_ms_height_gain = false;
// << request.ownCrazyflie.y-dronexSetpoint.y << ", " reset_rviz = true;
// << request.ownCrazyflie.z-dronexSetpoint.z << ")"); }
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
ROS_INFO("took off");
tookOffFlag = true;
ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_HOVER;
taking_off_from_ms_height_gain = false;
reset_rviz = true;
} }
} }
}
break; break;
case DRONEX_STATE_HOVER: case DRONEX_STATE_HOVER:
{ {
integratorFlag = DRONEX_INTEGRATOR_ON;
//ROS_INFO_STREAM("DRONEX_STATE_HOVER"); //ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant // keep setpoint constant
...@@ -744,6 +722,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -744,6 +722,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
break; break;
case DRONEX_STATE_FOLLOWING_TRAJECTORY: case DRONEX_STATE_FOLLOWING_TRAJECTORY:
{
integratorFlag = DRONEX_INTEGRATOR_OFF;
/*if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] && /*if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] && abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] )*/ abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] )*/
...@@ -758,8 +738,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -758,8 +738,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP; flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP"); ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP");
} else{ } else{
ROS_INFO_STREAM("distance to xm2: "<<request.ownCrazyflie.x-xm2[0]<<", "<<request.ownCrazyflie.y-xm2[1]<<", "<<request.ownCrazyflie.z-xm2[2]); //ROS_INFO_STREAM("distance to xm2: "<<request.ownCrazyflie.x-xm2[0]<<", "<<request.ownCrazyflie.y-xm2[1]<<", "<<request.ownCrazyflie.z-xm2[2]);
} }
}
break; break;
...@@ -871,7 +852,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -871,7 +852,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = request.otherCrazyflies[0].x; // setpoint on mothership dronexSetpoint.x = request.otherCrazyflies[0].x; // setpoint on mothership
dronexSetpoint.y = request.otherCrazyflies[0].y; dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.03; dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;
dronexVelocity.x = mothership_vel[0]; // velocity of mothership dronexVelocity.x = mothership_vel[0]; // velocity of mothership
...@@ -915,7 +896,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -915,7 +896,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
} }
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) && else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) && (abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
(abs(request.ownCrazyflie.z - 0.03 - request.otherCrazyflies[0].z) < tol_land[2]) ){ (abs(request.ownCrazyflie.z - 0.05 - request.otherCrazyflies[0].z) < tol_land[2]) ){
ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP"); ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_ON_MOTHERSHIP; flying_state = DRONEX_STATE_ON_MOTHERSHIP;
previous_flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP; previous_flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
...@@ -1082,7 +1063,7 @@ void calculateTrajectory(Controller::Request &request){ ...@@ -1082,7 +1063,7 @@ void calculateTrajectory(Controller::Request &request){
// returns (x,y,z) for a given time t // returns (x,y,z, vx,vy,vz) for a given time t
// Calculate the trajectory from the Crazyflie to the mothership // Calculate the trajectory from the Crazyflie to the mothership
// Trajectory: xcf0 -> xm1 -> xm2 -> xms, All segments are straight lines. (For testing purposes) Later should be curves // Trajectory: xcf0 -> xm1 -> xm2 -> xms, All segments are straight lines. (For testing purposes) Later should be curves
// xcf0: inital(Button pressed) position of Crazyflie // xcf0: inital(Button pressed) position of Crazyflie
...@@ -1090,9 +1071,7 @@ void calculateTrajectory(Controller::Request &request){ ...@@ -1090,9 +1071,7 @@ void calculateTrajectory(Controller::Request &request){
// xm2: Point behind the mothership // xm2: Point behind the mothership
// xms: Point on mothership where we will land // xms: Point on mothership where we will land
// Please use trajectory_t0 as starting time // Input: t in [0, inf)
// Input: t in [trajectory_t0 and trajectory_t3]
// If t > trajectory_t3 then OUTPUT: (-1,-1,-1) -> exception handling
std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// (x,y,z,vx,vy,vz) where abs(vx,vy,vz) = trajectory_velocity_of_CF // (x,y,z,vx,vy,vz) where abs(vx,vy,vz) = trajectory_velocity_of_CF
...@@ -1141,34 +1120,56 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -1141,34 +1120,56 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
//TODO Attention this conversion might be buggy //TODO Attention this conversion might be buggy
direction_ms_to_cf_ms_bodyframe[0] = cosYaw*direction_ms_to_cf[0]; direction_ms_to_cf_ms_bodyframe[0] = cosYaw*direction_ms_to_cf[0] + sinYaw*direction_ms_to_cf[1];
direction_ms_to_cf_ms_bodyframe[1] = sinYaw*direction_ms_to_cf[1]; direction_ms_to_cf_ms_bodyframe[1] = cosYaw*direction_ms_to_cf[1] - sinYaw*direction_ms_to_cf[0];
if(direction_ms_to_cf_ms_bodyframe[0] < 0.0){ // if(direction_ms_to_cf_ms_bodyframe[0] < 0.0){
xm2_sign = 1.0; // Crazyflie can land from behind // xm2_sign = -1.0; // Crazyflie can land from behind
}else{ // }else{
xm2_sign = -1.0; // Crazyflie can land from front // xm2_sign = 1.0; // Crazyflie can land from front
// }
if(direction_ms_to_cf_ms_bodyframe[0] > abs(direction_ms_to_cf_ms_bodyframe[1])){
xm2_sign[0] = 1.0;
xm2_sign[1] = 0.0; // Crazyflie can land from behind
}
else if(direction_ms_to_cf_ms_bodyframe[1] > abs(direction_ms_to_cf_ms_bodyframe[0])){
xm2_sign[0] = 0.0;
xm2_sign[1] = -1.0; // Crazyflie can land from front
}
else if(-direction_ms_to_cf_ms_bodyframe[0] > abs(direction_ms_to_cf_ms_bodyframe[1])){
xm2_sign[0] = -1.0;
xm2_sign[1] = 0.0; // Crazyflie can land from front
} }
else if(-direction_ms_to_cf_ms_bodyframe[1] > abs(direction_ms_to_cf_ms_bodyframe[0])){
xm2_sign[0] = 0.0;
xm2_sign[1] = 1.0; // Crazyflie can land from front
}
// Calculate xm2(the point behind the mothership) // Calculate xm2(the point behind the mothership)
// absolute velocity of MS
float ms_abs_velocity = sqrt(mothership_vel[0] * mothership_vel[0] + mothership_vel[1] * mothership_vel[1] + mothership_vel[2] * mothership_vel[2]); float ms_abs_velocity = sqrt(mothership_vel[0] * mothership_vel[0] + mothership_vel[1] * mothership_vel[1] + mothership_vel[2] * mothership_vel[2]);
float normalized_ms_velocity = ms_abs_velocity/xm2_distance_to_ms_at_zero_velocity; // scale absolute velocity
float normalized_ms_velocity = ms_abs_velocity / xm2_distance_to_ms_at_zero_velocity;
// clip if too big
if(normalized_ms_velocity > xm2_distance_to_ms_at_zero_velocity){ if(normalized_ms_velocity > xm2_distance_to_ms_at_zero_velocity){
normalized_ms_velocity = xm2_distance_to_ms_at_zero_velocity; normalized_ms_velocity = xm2_distance_to_ms_at_zero_velocity;
} }
xm2_distance_to_ms = xm2_distance_to_ms_at_zero_velocity - xm2_distance_to_ms_scaling_factor * normalized_ms_velocity; xm2_distance_to_ms = xm2_distance_to_ms_at_zero_velocity - xm2_distance_to_ms_scaling_factor * normalized_ms_velocity;
xm2[0] = request.otherCrazyflies[0].x - xm2_sign*cosYaw*xm2_distance_to_ms; xm2[0] = request.otherCrazyflies[0].x + (xm2_sign[0] * cosYaw + xm2_sign[1] * sinYaw) * xm2_distance_to_ms;
xm2[1] = request.otherCrazyflies[0].y - xm2_sign*sinYaw*xm2_distance_to_ms; xm2[1] = request.otherCrazyflies[0].y + (-xm2_sign[1] * cosYaw + xm2_sign[0] * sinYaw) * xm2_distance_to_ms;
xm2[2] = request.otherCrazyflies[0].z + xm2_height_over_ms; xm2[2] = request.otherCrazyflies[0].z + xm2_height_over_ms;
xms[0] = request.otherCrazyflies[0].x; xms[0] = request.otherCrazyflies[0].x;
xms[1] = request.otherCrazyflies[0].y; xms[1] = request.otherCrazyflies[0].y;
xms[2] = request.otherCrazyflies[0].z+0.2; // TODO add a certain height to not collide with mothership xms[2] = request.otherCrazyflies[0].z+0.2; // a bit over the MS not to collide
// Calculate the whole trajectory distance // Calculate the whole trajectory distance
...@@ -1188,7 +1189,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -1188,7 +1189,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
trajectory_duration_3 = calculate_distance_in_xyz(xm2,xms)/trajectory_velocity_of_CF; trajectory_duration_3 = calculate_distance_in_xyz(xm2,xms)/trajectory_velocity_of_CF;
// Time when at xm1 // Time when at xm1
trajectory_t1 = trajectory_t0 + trajectory_duration_1; trajectory_t1 = trajectory_duration_1;
// Time when at xm2 // Time when at xm2
trajectory_t2 = trajectory_t1 + trajectory_duration_2; trajectory_t2 = trajectory_t1 + trajectory_duration_2;
// Time when at xms // Time when at xms
...@@ -1198,7 +1199,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -1198,7 +1199,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// Calculate the x,y,z position depending on where the CF should be at a certain time t // Calculate the x,y,z position depending on where the CF should be at a certain time t
if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position & velocity on line between xcf0 and xm1 if(0.0 <= t && t <= trajectory_t1){ // Calculate position & velocity on line between xcf0 and xm1
for(int i = 0; i < 3; i++){ for(int i = 0; i < 3; i++){
trajectory_return[i] = xcf0[i] + t*(xm1[i] - xcf0[i])/trajectory_t1; trajectory_return[i] = xcf0[i] + t*(xm1[i] - xcf0[i])/trajectory_t1;
trajectory_return[i+3] = (xm1[i]-xcf0[i])/calculate_distance_in_xyz(xcf0, xm1) * trajectory_velocity_of_CF_init; trajectory_return[i+3] = (xm1[i]-xcf0[i])/calculate_distance_in_xyz(xcf0, xm1) * trajectory_velocity_of_CF_init;
...@@ -1212,8 +1213,14 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -1212,8 +1213,14 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
for(int i = 0; i < 3; i++){ for(int i = 0; i < 3; i++){
trajectory_return[i] = xm2[i] + (t - trajectory_t2)*(xms[i] - xm2[i])/(trajectory_t3 - trajectory_t2); trajectory_return[i] = xm2[i] + (t - trajectory_t2)*(xms[i] - xm2[i])/(trajectory_t3 - trajectory_t2);
trajectory_return[i+3] = (xms[i]-xm2[i])/calculate_distance_in_xyz(xms, xm2) * trajectory_velocity_of_CF; trajectory_return[i+3] = (xms[i]-xm2[i])/calculate_distance_in_xyz(xms, xm2) * trajectory_velocity_of_CF;
//float acceleration = (mothership_vel[i] - trajectory_return[i+3]) / (trajectory_t3 - trajectory_t2);
// smooth velocity change between xm2 and xms
//trajectory_return[i+3] = trajectory_return[i+3] + (t - trajectory_t2) * (mothership_vel[i] - trajectory_return[i+3]) / (trajectory_t3 - trajectory_t2);
} }
}else{ }else{
trajectory_return[0] = xms[0]; trajectory_return[0] = xms[0];
trajectory_return[1] = xms[1]; trajectory_return[1] = xms[1];
trajectory_return[2] = xms[2]; trajectory_return[2] = xms[2];
...@@ -1285,7 +1292,6 @@ void motorsOFFSmooth(Controller::Response &response){ ...@@ -1285,7 +1292,6 @@ void motorsOFFSmooth(Controller::Response &response){
} }
void motorsOFF(Controller::Response &response){ void motorsOFF(Controller::Response &response){
float feed_forward_thrust_per_motor = m_mass_total_grams * 9.81/(1000*4);
// > Put in the per motor commands // > Put in the per motor commands
response.controlOutput.motorCmd1 = 0; response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0; response.controlOutput.motorCmd2 = 0;
...@@ -1821,6 +1827,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle ...@@ -1821,6 +1827,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_points_trajectory_publisher.publish(rviz_mothership); rviz_points_trajectory_publisher.publish(rviz_mothership);
//if (ros::ok()){ //if (ros::ok()){
//ROS_INFO("Init RVIZ STUFF");