Commit d93b0161 authored by mastefan's avatar mastefan
Browse files

flying sequence now working

parent f5a97334
......@@ -118,6 +118,7 @@
// 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
// firmware running on the Crazyflie 2.0.
......@@ -209,11 +210,13 @@ bool landedFlag = false;
// Flag for saving starting coordinates
bool savedStartCoordinates = false;
int startCoordinateX;
int startCoordinateY;
int startCoordinateZ;
float startCoordinateX;
float startCoordinateY;
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;
float m_yAdjustment = 0.0f;
// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = false;
bool m_shouldSmoothSetpointChanges = true;
// Max setpoint change per second
float m_max_setpoint_change_per_second_horizontal;
......
......@@ -3,8 +3,8 @@ mass_CF : 28
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.10 # [meters]
max_setpoint_change_per_second_vertical : 0.50 # [meters]
max_setpoint_change_per_second_horizontal : 1.00 # [meters]
max_setpoint_change_per_second_vertical : 0.40 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz
......@@ -84,4 +84,4 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# 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(){
void buttonPressed_abort(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Abort Mission!");
flying_state = DRONEX_STATE_LAND_ON_GROUND;
// reset start position
savedStartCoordinates = false;
}
void buttonPressed_integrator_on(){
......@@ -350,17 +353,35 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
switch(flying_state){
case DRONEX_STATE_APPROACH:
{
ROS_INFO("DRONEX_STATE_APPROACH");
//ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
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);
}
break;
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;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
......@@ -368,6 +389,37 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
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:
{
//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 &
//dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
// to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie[0].x;
dronexSetpoint.y = request.ownCrazyflie[0].y;
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = 0.05;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
......@@ -415,16 +467,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_TAKING_OFF:
{
ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
if(!savedStartCoordinates)
{
startCoordinateY = request.ownCrazyflie.x; // Nöd sicher öbs das brucht. Idee: Afangscoordinate abspeichere zum in Hover state z ende.
startCoordinateX = request.ownCrazyflie.y;
startCoordinateX = request.ownCrazyflie.x; // Nöd sicher öbs das brucht. Idee: Afangscoordinate abspeichere zum in Hover state z ende.
startCoordinateY = request.ownCrazyflie.y;
startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true;
//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 &
dronexSetpoint.z = startCoordinateZ + 0.6;
if((request.ownCrazyflie.z > startCoordinateZ + 0.6 - eps_height) || (request.ownCrazyflie.z < startCoordinateZ + 0.6 + eps_height)){
tookOffFlag == true;
// -> flyingState: Hover
if(abs(request.ownCrazyflie.z - startCoordinateZ - 0.6) < eps_height){
ROS_INFO("took off");
tookOffFlag = true;
flying_state = DRONEX_STATE_HOVER;
}
}
break;
case DRONEX_STATE_LAND_ON_MOTHERSHIP:
case DRONEX_STATE_HOVER:
{
//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;
// keep setpoint constant
/*
dronexSetpoint.x = dronexSetpoint.x;
dronexSetpoint.y = dronexSetpoint.y;
dronexSetpoint.z = dronexSetpoint.z;
*/
}
break;
} // END switch case
......@@ -455,13 +516,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (flightSequence == 1){
flying_state = DRONEX_TAKE_OFF;
flying_state = DRONEX_STATE_TAKING_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){
flying_state = DRONEX_STATE_APPROACH;
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
}
......@@ -508,12 +569,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Turn motors off if wanted or do LQR-control
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)){
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);
}
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