To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 66042ebc authored by mastefan's avatar mastefan
Browse files

Minor changes to the FSM, new int pickupmode (yaml) to test chocolate pickup.

parent 64f89283
......@@ -517,6 +517,9 @@ float green_2 = 1.0f;
float blue_2 = 0.0f;
int pickupmode = 0; // 0=land; 1=hover;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
// structure as defined in the file "CrazyflieData.msg" which has the following
......
# Mass of the crazyflie
mass_CF : 28 #32
mass_CF : 31 #28 #32
# Max setpoint change per second
......@@ -28,8 +28,8 @@ trajectory_deltaT_velocity: 0.35
# distance to first point on trajectory
xm1_x_distance: 0.5
# distance from MS to second point on trajectory
xm2_distance_to_ms: 0.6
xm2_height_over_ms: 0.3
xm2_distance_to_ms: 0.2 #0.6
xm2_height_over_ms: 0.4 #0.3
xm1_height: 0.6
......@@ -43,12 +43,13 @@ xm2_distance_to_ms_at_zero_velocity : 0.6 # xm2 distance behind the mothership
xm2_distance_to_ms_scaling_factor : 1 # Scale in direction of the mothership velocity
# tolerances
tol_takeoff: [0.07, 0.07, 0.07]
tol_takeoff: [0.07, 0.07, 0.07] #[0.07, 0.07, 0.07]
tol_approach: [0.3, 0.3, 0.1]
tol_land: [0.01, 0.01, 0.05]
land_height: 0.3
tol_land: [0.05, 0.05, 0.05] #[0.01, 0.01, 0.05]
land_height: 0.03 #0.3
start_height: 0.9
pickupmode: 0 # 0=land directly; 1=hover to pickup chocolate
......
......@@ -562,6 +562,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
// Uncomment this if this part of the FSM should not be below together with the modes.
//motorsOFF(response);
}
break;
......@@ -583,6 +586,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
// Uncomment this if this part of the FSM should not be below together with the modes.
// if(pickupmode == 0){ // land directly/normally
// motorsOFF(response);
// }else{ // hover over mothership to pickup chocolate
// flying_state = DRONEX_STATE_HOVER;
// }
}
break;
......@@ -603,20 +616,43 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// flying_state = DRONEX_STATE_APPROACH;
// ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
//}
// Uncomment this if this part of the FSM should not be below together with the modes.
// if((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.z - land_height - request.otherCrazyflies[0].z) < tol_land[2]) ){
// ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
// flying_state = DRONEX_STATE_ON_MOTHERSHIP;
// previous_flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// }
}
break;
case DRONEX_STATE_LAND_ON_GROUND:
{
if(tookOffFlag){
ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
//ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = 0.0;
tookOffFlag = false;
}
// Uncomment this if this part of the FSM should not be below together with the modes.
// if(request.ownCrazyflie.z < 0.06 ){
// ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
// flying_state = DRONEX_STATE_GROUND;
// previous_flying_state = DRONEX_STATE_LAND_ON_GROUND;
// }
}
break;
......@@ -904,7 +940,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
if(flying_state == DRONEX_STATE_ON_MOTHERSHIP){
motorsOFF(response);
if(pickupmode == 0){ // land directly/normally
motorsOFF(response);
}else{ // hover over mothership to pickup chocolate
flying_state = DRONEX_STATE_HOVER;
}
//ROS_INFO("On Mothership");
}else if(flying_state == DRONEX_STATE_GROUND){
motorsOFF(response);
......@@ -913,6 +953,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
calculateControlOutputDroneX(request, response, stateErrorBody); // chooses controller mode
}
// Uncomment this to have a separated FSM
// if(!(flying_state == DRONEX_STATE_GROUND) && !(flying_state == DRONEX_STATE_ON_MOTHERSHIP)){
// calculateControlOutputDroneX(request, response, stateErrorBody); // chooses controller mode
// }
// PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
if (shouldPublishCurrent_xyz_yaw)
......@@ -2472,6 +2518,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
xm2_distance_to_ms_at_zero_velocity = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms_at_zero_velocity");
xm2_distance_to_ms_scaling_factor = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms_scaling_factor");
pickupmode = getParameterInt(nodeHandle_for_dronexController, "pickupmode");
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[DRONEX CONTROLLER] DEBUGGING: the fetched DroneXController/mass_CF = " << m_mass_CF_grams);
......
Markdown is supported
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