Commit 4fd208ed authored by mastefan's avatar mastefan
Browse files

First landing controller on button click using setpoints working.

parent 9e849ea7
......@@ -196,7 +196,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 = true;
bool m_shouldSmoothSetpointChanges = false;
// Max setpoint change per second
float m_max_setpoint_change_per_second_horizontal;
......@@ -209,9 +209,6 @@ float m_max_setpoint_change_per_second_yaw_radians;
float m_vicon_frequency;
// THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER
......
# Mass of the crazyflie
mass_CF : 32
mass_CF : 28
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.10 # [meters]
max_setpoint_change_per_second_vertical : 0.20 # [meters]
max_setpoint_change_per_second_vertical : 0.50 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz
......
......@@ -19,7 +19,7 @@ defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
landingDistance : -0.05
durationTakeOff : 1.0
durationTakeOff : 0.0
durationLanding : 2.0
# Liner Trayectory following parameters
......
......@@ -197,111 +197,10 @@ void buttonPressed_take_off(){
void buttonPressed_land(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Landing...");
flying_state = DRONEX_STATE_LANDING;
}
// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
/*void buttonPressed_gotoStart()
{
ROS_INFO("[DRONEX CONTROLLER] Goto Start button pressed");
// The drone should move smoothly to the start point:
m_shouldSmoothSetpointChanges = true;
// Set the (x,y) coordinates for the start point:
m_setpoint[0] = m_pickup_coordinates_xy[0];
m_setpoint[1] = m_pickup_coordinates_xy[1];
// Set the z coordinate to be a little more than the
// length of the "dronex string"
m_setpoint[2] = m_dronex_string_length + 0.10;
// Publish the setpoint so that the GUI updates
publishCurrentSetpoint();
}
void buttonPressed_connect()
{
ROS_INFO("[DRONEX CONTROLLER] Connect button pressed");
m_shouldSmoothSetpointChanges = true;
m_setpoint[2] = m_dronex_string_length + m_thickness_of_object_at_pickup;
publishCurrentSetpoint();
}
void buttonPressed_pickup()
{
ROS_INFO("[DRONEX CONTROLLER] Pick up button pressed");
m_shouldSmoothSetpointChanges = true;
m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
m_setpoint[2] = m_dronex_string_length + 0.25;
publishCurrentSetpoint();
}
void buttonPressed_gotoEnd()
{
ROS_INFO("[DRONEX CONTROLLER] Goto End button pressed");
m_shouldSmoothSetpointChanges = true;
m_setpoint[0] = m_dropoff_coordinates_xy_for_E[0];
m_setpoint[1] = m_dropoff_coordinates_xy_for_E[1];
publishCurrentSetpoint();
}
void buttonPressed_putdown()
{
ROS_INFO("[DRONEX CONTROLLER] Put down button pressed");
m_shouldSmoothSetpointChanges = true;
m_setpoint[2] = m_dronex_string_length + m_thickness_of_object_at_putdown;
m_mass_total_grams = m_mass_CF_grams;
publishCurrentSetpoint();
}
void buttonPressed_squat()
{
ROS_INFO("[DRONEX CONTROLLER] Squat button pressed");
flying_state = DRONEX_STATE_LANDING;
m_shouldSmoothSetpointChanges = true;
m_setpoint[2] = m_dronex_string_length - 0.10;
m_mass_total_grams = m_mass_CF_grams;
publishCurrentSetpoint();
// flying_state = DRONEX_STATE_APPROACHING; iff already took off
}
void buttonPressed_disconnect()
{
ROS_INFO("[DRONEX CONTROLLER] Disconnect button pressed");
m_shouldSmoothSetpointChanges = false;
m_setpoint[2] = m_dronex_string_length + 0.10;
m_mass_total_grams = m_mass_CF_grams;
publishCurrentSetpoint();
}
*/
......@@ -419,9 +318,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_time_ticks++;
m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
//ROS_INFO_STREAM("Dronex x: " << request.otherCrazyflies[0].x);
//dronexSetpoint = request.otherCrazyflies[0];
switch(flying_state){
case DRONEX_STATE_APPROACH:
......@@ -430,60 +326,84 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
setpointCallback(dronexSetpoint);
//setpointCallback(dronexSetpoint);
}
break;
case DRONEX_STATE_GROUND:
{
//{
ROS_INFO("DRONEX_STATE_GROUND");
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
/* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;
}
}*/
break;
case DRONEX_STATE_LANDING:
{
ROS_INFO_STREAM("DRONEX_STATE_LANDING" << request.ownCrazyflie.z);
if(request.ownCrazyflie.z > 0.1){
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z - 0.01;
}else{
//ROS_INFO_STREAM("DRONEX_STATE_LANDING: Crazyflie z coordinate " << request.ownCrazyflie.z);
//ROS_INFO_STREAM("DRONEX_STATE_LANDING: Mothership z coordinate " << request.otherCrazyflies[0].z);
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;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
}
//flyingStatePublisher.publish(flying_state_msg);
ROS_INFO_STREAM("DRONEX_MOTORS_OFF..." << request.ownCrazyflie.z - request.otherCrazyflies[0].z);
return true;
}*/
/*
}else{
//if(dronexSetpoint.z>=request.otherCrazyflies[0].z){
float down_per_cycle = 0.003;
if(dronexSetpoint.z < request.otherCrazyflies[0].z+0.5){
down_per_cycle = 0.0005;
}
dronexSetpoint.z -= down_per_cycle;
ROS_INFO_STREAM("DRONEX_L: z" << dronexSetpoint.z);
//}else{
//}
}*/
return true;
}
break;
case DRONEX_STATE_TAKING_OFF:
{
ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
if(request.ownCrazyflie.z < 0.8){
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z + 0.01;
}else{
//flying_state = DRONEX_STATE_APPROACH;
}
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z +0.6;
setpointCallback(dronexSetpoint);
//setpointCallback(dronexSetpoint);
break;
}
}
setpointCallback(dronexSetpoint);
/*dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.3*/
//setpointCallback(dronexSetpoint);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
perControlCycleOperations();
perControlCycleOperations();
// THIS IS THE START OF THE "OUTER" CONTROL LOOP
// > i.e., this is the control loop run on this laptop
// > this function is called at the frequency specified
......@@ -508,8 +428,21 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// CARRY OUT THE CONTROLLER COMPUTATIONS
// Call the function that performs the control computations for this mode
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
if((flying_state == DRONEX_STATE_LANDING && (request.ownCrazyflie.z < 0.1 + request.otherCrazyflies[0].z)) || flying_state == DRONEX_STATE_GROUND){
response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
}else{
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
}
// PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
......@@ -1059,29 +992,9 @@ void dronexCoordinateCallback(const ViconData& viconData){
}
}
/*m_setpoint[0] = droneX.x;
m_setpoint[1] = droneX.y;
m_setpoint[2] = droneX.z+0.2;
m_setpoint[3] = droneX.yaw;*/
}
/*void coordinatesToLocal(CrazyflieData& cf) {
AreaBounds area = context.localArea;
float originX = (area.xmin + area.xmax) / 2.0;
float originY = (area.ymin + area.ymax) / 2.0;
// change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
float originZ = 0.0;
// float originZ = (area.zmin + area.zmax) / 2.0;
cf.x -= originX;
cf.y -= originY;
cf.z -= originZ;
}*/
// ************************************************************************************************
......
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