Commit d93b0161 authored by mastefan's avatar mastefan
Browse files

flying sequence now working

parent f5a97334
...@@ -118,6 +118,7 @@ ...@@ -118,6 +118,7 @@
// These constants define the modes that can be used for controller the Crazyflie 2.0, // These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the // the constants defined here need to be in agreement with those defined in the
// firmware running on the Crazyflie 2.0. // firmware running on the Crazyflie 2.0.
...@@ -209,11 +210,13 @@ bool landedFlag = false; ...@@ -209,11 +210,13 @@ bool landedFlag = false;
// Flag for saving starting coordinates // Flag for saving starting coordinates
bool savedStartCoordinates = false; bool savedStartCoordinates = false;
int startCoordinateX; float startCoordinateX;
int startCoordinateY; float startCoordinateY;
int startCoordinateZ; float startCoordinateZ;
int eps_height = 0.05; // tolerances (x,y,z)
float tol_eps[3] = {0.5, 0.5, 0.5};
float eps_height = 0.05;
...@@ -234,7 +237,7 @@ float m_xAdjustment = 0.0f; ...@@ -234,7 +237,7 @@ float m_xAdjustment = 0.0f;
float m_yAdjustment = 0.0f; float m_yAdjustment = 0.0f;
// Boolean for whether to limit rate of change of the setpoint // Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = false; bool m_shouldSmoothSetpointChanges = true;
// Max setpoint change per second // Max setpoint change per second
float m_max_setpoint_change_per_second_horizontal; float m_max_setpoint_change_per_second_horizontal;
......
...@@ -3,8 +3,8 @@ mass_CF : 28 ...@@ -3,8 +3,8 @@ mass_CF : 28
# Max setpoint change per second # Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.10 # [meters] max_setpoint_change_per_second_horizontal : 1.00 # [meters]
max_setpoint_change_per_second_vertical : 0.50 # [meters] max_setpoint_change_per_second_vertical : 0.40 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees] max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz # Frequency of the controller, in hertz
...@@ -84,4 +84,4 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342] ...@@ -84,4 +84,4 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648] #PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# for our integrator (so far just random values) # for our integrator (so far just random values)
gainIntegrator : [0.01, 0.01, 0.01] gainIntegrator : [1, 1, 1]
...@@ -211,6 +211,9 @@ void buttonPressed_land(){ ...@@ -211,6 +211,9 @@ void buttonPressed_land(){
void buttonPressed_abort(){ void buttonPressed_abort(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Abort Mission!"); ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Abort Mission!");
flying_state = DRONEX_STATE_LAND_ON_GROUND; flying_state = DRONEX_STATE_LAND_ON_GROUND;
// reset start position
savedStartCoordinates = false;
} }
void buttonPressed_integrator_on(){ void buttonPressed_integrator_on(){
...@@ -350,17 +353,35 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -350,17 +353,35 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
switch(flying_state){ switch(flying_state){
case DRONEX_STATE_APPROACH: case DRONEX_STATE_APPROACH:
{ {
ROS_INFO("DRONEX_STATE_APPROACH"); //ROS_INFO("DRONEX_STATE_APPROACH");
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.2; dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_eps[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_eps[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_eps[2] ){
approachedFlag = true;
ROS_INFO("approached");
}
//setpointCallback(dronexSetpoint); //setpointCallback(dronexSetpoint);
} }
break; break;
case DRONEX_STATE_GROUND: case DRONEX_STATE_GROUND:
{ {
ROS_INFO("DRONEX_STATE_GROUND"); //ROS_INFO("DRONEX_STATE_GROUND");
// Variable for choosing flight sequence off
flightSequence = 0;
// Flags of landing sequence reset
tookOffFlag = false;
approachedFlag = false;
//bool landedFlag = true;
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
/* std_msgs::Int32 flying_state_msg; /* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF; flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg); flyingStatePublisher.publish(flying_state_msg);
...@@ -368,6 +389,37 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -368,6 +389,37 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
} }
break; break;
case DRONEX_STATE_ON_MOTHERSHIP:
{
//ROS_INFO("DRONEX_STATE_ON_MOTHERSHIP");
// Variable for choosing flight sequence off
flightSequence = 0;
// Flags of landing sequence reset
tookOffFlag = false;
approachedFlag = false;
//bool landedFlag = true;
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
/* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;*/
}
break;
case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{
//to land on mothership (17.10. vm)
dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
}
break;
case DRONEX_STATE_LAND_ON_GROUND: case DRONEX_STATE_LAND_ON_GROUND:
{ {
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Crazyflie z coordinate " << request.ownCrazyflie.z); //ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Crazyflie z coordinate " << request.ownCrazyflie.z);
...@@ -379,8 +431,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -379,8 +431,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//dronexSetpoint.z = request.otherCrazyflies[0].z+0.05; //dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
// to land on ground (17.10. vm) // to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie[0].x; dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie[0].y; dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = 0.05; dronexSetpoint.z = 0.05;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1 /*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
...@@ -415,16 +467,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -415,16 +467,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_TAKING_OFF: case DRONEX_STATE_TAKING_OFF:
{ {
ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF"); //ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
if(!savedStartCoordinates) if(!savedStartCoordinates)
{ {
startCoordinateY = request.ownCrazyflie.x; // Nöd sicher öbs das brucht. Idee: Afangscoordinate abspeichere zum in Hover state z ende. startCoordinateX = request.ownCrazyflie.x; // Nöd sicher öbs das brucht. Idee: Afangscoordinate abspeichere zum in Hover state z ende.
startCoordinateX = request.ownCrazyflie.y; startCoordinateY = request.ownCrazyflie.y;
startCoordinateZ = request.ownCrazyflie.z; startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true; savedStartCoordinates = true;
//setpointCallback(dronexSetpoint); //setpointCallback(dronexSetpoint);
ROS_INFO_STREAM("DRONEX: saved start Coordinates");
ROS_INFO_STREAM("x = " << startCoordinateX);
ROS_INFO_STREAM("y = " << startCoordinateY);
ROS_INFO_STREAM("z = " << startCoordinateZ);
} }
...@@ -433,21 +489,26 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -433,21 +489,26 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.z = startCoordinateZ + 0.6; dronexSetpoint.z = startCoordinateZ + 0.6;
if((request.ownCrazyflie.z > startCoordinateZ + 0.6 - eps_height) || (request.ownCrazyflie.z < startCoordinateZ + 0.6 + eps_height)){ if(abs(request.ownCrazyflie.z - startCoordinateZ - 0.6) < eps_height){
tookOffFlag == true; ROS_INFO("took off");
// -> flyingState: Hover tookOffFlag = true;
flying_state = DRONEX_STATE_HOVER;
} }
} }
break; break;
case DRONEX_STATE_LAND_ON_MOTHERSHIP: case DRONEX_STATE_HOVER:
{ {
//to land on mothership (17.10. vm) // keep setpoint constant
dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates /*
dronexSetpoint.y = request.otherCrazyflies[0].y; dronexSetpoint.x = dronexSetpoint.x;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.05; dronexSetpoint.y = dronexSetpoint.y;
dronexSetpoint.z = dronexSetpoint.z;
*/
} }
break; break;
} // END switch case } // END switch case
...@@ -455,13 +516,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -455,13 +516,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (flightSequence == 1){ if (flightSequence == 1){
flying_state = DRONEX_TAKE_OFF; flying_state = DRONEX_STATE_TAKING_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership"); //ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){ if(tookOffFlag){
flying_state = DRONEX_STATE_APPROACH; flying_state = DRONEX_STATE_APPROACH;
if(approachedFlag){ if(approachedFlag){
flying_state = DRONEX_LAND; // @ Stefano: Was macht DRONEX_LAND? müessti das nid DRONEX_LAND_ON_MOTHERSHIP si? flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP; // @ Stefano: Was macht DRONEX_LAND? müessti das nid DRONEX_STATE_LAND_ON_MOTHERSHIP si?
// Valentin // Valentin
} }
...@@ -508,12 +569,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -508,12 +569,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Turn motors off if wanted or do LQR-control // Turn motors off if wanted or do LQR-control
if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.1 )){ if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.1 )){
motorsOFF(response); ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
flying_state = DRONEX_STATE_GROUND;
} }
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (request.ownCrazyflie.z < 0.1 + request.otherCrazyflies[0].z)){ else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (request.ownCrazyflie.z < 0.1 + request.otherCrazyflies[0].z)){
motorsOFF(response); ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_ON_MOTHERSHIP;
} }
else if(flying_state == DRONEX_STATE_GROUND){
if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
motorsOFF(response); motorsOFF(response);
} }
else{ else{
......
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