Commit 24c80da3 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

now the take-offis also w.r.t. the loads yaw angle

parent 3d8a86ef
......@@ -10,7 +10,7 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
#take off and landing parameters (in meters and seconds)
takeOffinitial : 0.05
takeOffDistance : 0.6
takeOffDistance : 0.3
landingDistance : 0.1
durationTakeOff : 2.0
durationLanding : 2.0
......
......@@ -187,11 +187,27 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float yaw = request.otherCrazyflies[0].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
load_setpoint[3] = yaw;
agent_setpoint[0] += load_setpoint[0];
agent_setpoint[1] += load_setpoint[1];
agent_setpoint[2] += load_setpoint[2] + takeOffinitial;
agent_setpoint[3] = 0.0;
load_setpoint[3] = yaw;
// convert agent position w.r.t. the load into world frame
std::vector<float> distance_to_load_world;
if (my_agentID == 1)
{
distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P1 , 0.0, 0.0, yaw );
}
else if (my_agentID == 2)
{
distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P2 , 0.0, 0.0, yaw );
}
else if (my_agentID == 3)
{
distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P3 , 0.0, 0.0, yaw );
}
agent_setpoint[0] = load_setpoint[0] + distance_to_load_world[0];
agent_setpoint[1] = load_setpoint[1] + distance_to_load_world[1];
agent_setpoint[2] = load_setpoint[2] + takeOffinitial;
agent_setpoint[3] = 0.0 ;
}
if (outer_loop_counter <= (int)(control_frequency*durationTakeOff) ) {
agent_setpoint[2] += takeOffDistance_increment;
......@@ -307,9 +323,48 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if ( outer_loop_counter == 0 ) {
// Landing was initiated
landingDistance_decrement = delta_z_current / ( control_frequency * durationLanding );
// move triangle formation to current load (x,y) setpoint
agent_setpoint[0] += load_setpoint[0];
agent_setpoint[1] += load_setpoint[1];
// current load position is origin
load_setpoint[0] = request.otherCrazyflies[3].x;
load_setpoint[1] = request.otherCrazyflies[3].y;
load_setpoint[2] = request.otherCrazyflies[3].z;
float yaw = request.otherCrazyflies[0].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
load_setpoint[3] = yaw;
// convert agent position w.r.t. the load into world frame
std::vector<float> distance_to_load_world;
std::vector<float> landing_delta; // for safely flying the copters next to the laod
if (my_agentID == 1)
{
landing_delta = {0.1 , 0.0, 0.0};
landing_delta[0] += attachmentPoint.P1[0];
landing_delta[1] += attachmentPoint.P1[1];
landing_delta[2] += attachmentPoint.P1[2];
distance_to_load_world = transformation_frame_load_to_world(landing_delta , 0.0, 0.0, yaw );
}
else if (my_agentID == 2)
{
landing_delta = {-0.1 , 0.1, 0.0};
landing_delta[0] += attachmentPoint.P2[0];
landing_delta[1] += attachmentPoint.P2[1];
landing_delta[2] += attachmentPoint.P2[2];
distance_to_load_world = transformation_frame_load_to_world(landing_delta, 0.0, 0.0, yaw );
}
else if (my_agentID == 3)
{
landing_delta = {-0.1 , -0.1, 0.0};
landing_delta[0] += attachmentPoint.P3[0];
landing_delta[1] += attachmentPoint.P3[1];
landing_delta[2] += attachmentPoint.P3[2];
distance_to_load_world = transformation_frame_load_to_world(landing_delta, 0.0, 0.0, yaw );
}
agent_setpoint[0] = load_setpoint[0] + distance_to_load_world[0];
agent_setpoint[1] = load_setpoint[1] + distance_to_load_world[1];
agent_setpoint[2] = request.ownCrazyflie.z;
agent_setpoint[3] = 0.0 ;
}
// check if copter is within landing distance
......@@ -996,31 +1051,12 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
// > State 1: copters taking off
case 1:
{
agent_setpoint = {0.0 , 0.0 , 0.0 , 0.0};
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 1 received in controller. ");
ROS_INFO("[STUDENT CONTROLLER] Controller State: copters Taking off. ");
controller_state = custom_button_index;
outer_loop_counter = 0;
if ( my_agentID == 1 ) {
agent_setpoint[0] += triangle_sidelength/sqrt(3.0);
agent_setpoint[1] += 0.0;
ROS_INFO("[STUDENT CONTROLLER] Setpoint for agent1 set to triangle vertex ");
}
else if ( my_agentID == 2 ) {
agent_setpoint[0] -= triangle_sidelength*sqrt(3.0)/6.0;
agent_setpoint[1] += triangle_sidelength/2.0;
}
else if ( my_agentID == 3 ) {
agent_setpoint[0] -= triangle_sidelength*sqrt(3.0)/6.0;
agent_setpoint[1] -= triangle_sidelength/2.0;
}
else {
ROS_INFO("Agent not recognised ");
}
break;
}
// > State 2: enable distributed controller for copters and load
......@@ -1049,34 +1085,12 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
// > State 4: copters landing
case 4:
{
// reset x-y position
agent_setpoint[0] = 0.0;
agent_setpoint[1] = 0.0;
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 4 received in controller.");
ROS_INFO("[STUDENT CONTROLLER] Controller State: landing copters.");
controller_state = custom_button_index;
outer_loop_counter = 0;
if ( my_agentID == 1 ) {
agent_setpoint[0] += triangle_sidelength/sqrt(3.0) + 0.1;
agent_setpoint[1] += 0.0;
ROS_INFO("[STUDENT CONTROLLER] Setpoint for agent1 set to triangle vertex ");
}
else if ( my_agentID == 2 ) {
agent_setpoint[0] -= triangle_sidelength*sqrt(3.0)/6.0 + 0.1;
agent_setpoint[1] += triangle_sidelength/2.0 + 0.1;
}
else if ( my_agentID == 3 ) {
agent_setpoint[0] -= triangle_sidelength*sqrt(3.0)/6.0 + 0.1;
agent_setpoint[1] -= triangle_sidelength/2.0 + 0.1;
}
else {
ROS_INFO("Agent not recognised ");
}
break;
}
// > State 5: copter-load system controller , centralized
......
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