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 545d1325 authored by beuchatp's avatar beuchatp
Browse files

minor Info-stream changes

parent e2544b0d
......@@ -515,6 +515,94 @@ void distributed_control_command( Controller::Request &request, Controller::Resp
}
void centralized_control_command( Controller::Request &request, Controller::Response &response)
{
// Control output
Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev;
// Estimator update
estimator_state_prev = centralized_controller.Phi * estimator_state_prev + centralized_controller.Gamma * y_to_controller;
// extract motor command from u:
float thrustSum = 0.0;
float outRoll = 0.0;
float outPitch = 0.0;
float outYaw = 0.0;
//if ( request.ownCrazyflie.crazyflieName == request.otherCrazyflies[0].crazyflieName )
if (my_agentID == 1 )
{
// Controller for crazyflie 01
thrustSum = u(0,0);
outRoll = u(1,0);
outPitch = u(2,0);
outYaw = u(3,0);
}
//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[1].crazyflieName )
else if (my_agentID == 2 )
{
// Controller for crazyflie 02
thrustSum = u(4,0);
outRoll = u(5,0);
outPitch = u(6,0);
outYaw = u(7,0);
}
//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[2].crazyflieName )
else if (my_agentID == 3 )
{
// Controller for crazyflie 03
thrustSum = u(8,0);
outRoll = u(9,0);
outPitch = u(10,0);
outYaw = u(11,0);
}
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] COMMANDS FROM LQG:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " << outYaw );
float est_error_roll = overlapping_estimator_state_prev(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = overlapping_estimator_state_prev(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = overlapping_estimator_state_prev(2)-request.otherCrazyflies[0].yaw;
float est_error_load_x = overlapping_estimator_state_prev(21)-request.otherCrazyflies[3].x;
float est_error_load_y = overlapping_estimator_state_prev(22)-request.otherCrazyflies[3].y;
float est_error_load_z = overlapping_estimator_state_prev(23)-request.otherCrazyflies[3].z;
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATION ERRORS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll = " << est_error_roll);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch = " << est_error_pitch);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw = " << est_error_yaw);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: x = " << est_error_load_x);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: y = " << est_error_load_y);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: z = " << est_error_load_z);
ROS_INFO_STREAM("[STUDENT CONTROLLER] CABLE ANGLE ESTIMATES:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << estimator_state_prev(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << estimator_state_prev(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << estimator_state_prev(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev(6) );
}
// PREPARE AND RETURN THE VARIABLE "response"
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
// choosing the Crazyflie onBoard controller type.
// it can either be Motor, Rate or Angle based
// response.controlOutput.onboardControllerType = MOTOR_MODE;
response.controlOutput.onboardControllerType = RATE_MODE;
// response.controlOutput.onboardControllerType = ANGLE_MODE;
}
void display_control_agent1( Controller::Request &request, Controller::Response &response)
{
......@@ -613,94 +701,6 @@ void display_control_agent1( Controller::Request &request, Controller::Response
// response.controlOutput.onboardControllerType = ANGLE_MODE;
}
void centralized_control_command( Controller::Request &request, Controller::Response &response)
{
// Control output
Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev;
// Estimator update
estimator_state_prev = centralized_controller.Phi * estimator_state_prev + centralized_controller.Gamma * y_to_controller;
// extract motor command from u:
float thrustSum = 0.0;
float outRoll = 0.0;
float outPitch = 0.0;
float outYaw = 0.0;
//if ( request.ownCrazyflie.crazyflieName == request.otherCrazyflies[0].crazyflieName )
if (my_agentID == 1 )
{
// Controller for crazyflie 01
thrustSum = u(0,0);
outRoll = u(1,0);
outPitch = u(2,0);
outYaw = u(3,0);
}
//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[1].crazyflieName )
else if (my_agentID == 2 )
{
// Controller for crazyflie 02
thrustSum = u(4,0);
outRoll = u(5,0);
outPitch = u(6,0);
outYaw = u(7,0);
}
//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[2].crazyflieName )
else if (my_agentID == 3 )
{
// Controller for crazyflie 03
thrustSum = u(8,0);
outRoll = u(9,0);
outPitch = u(10,0);
outYaw = u(11,0);
}
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " << outYaw );
}
if (outer_loop_counter % 1 == 0)
{
float est_error_roll = overlapping_estimator_state_prev(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = overlapping_estimator_state_prev(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = overlapping_estimator_state_prev(2)-request.otherCrazyflies[0].yaw;
float est_error_load_x = overlapping_estimator_state_prev(21)-request.otherCrazyflies[3].x;
float est_error_load_y = overlapping_estimator_state_prev(22)-request.otherCrazyflies[3].y;
float est_error_load_z = overlapping_estimator_state_prev(23)-request.otherCrazyflies[3].z;
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATION ERRORS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll = " << est_error_roll);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch = " << est_error_pitch);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw = " << est_error_yaw);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: x = " << est_error_load_x);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: y = " << est_error_load_y);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: z = " << est_error_load_z);
ROS_INFO_STREAM("[STUDENT CONTROLLER] CABLE ANGLE ESTIMATES:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << estimator_state_prev(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << estimator_state_prev(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << estimator_state_prev(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev(6) );
}
// PREPARE AND RETURN THE VARIABLE "response"
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
// choosing the Crazyflie onBoard controller type.
// it can either be Motor, Rate or Angle based
// response.controlOutput.onboardControllerType = MOTOR_MODE;
response.controlOutput.onboardControllerType = RATE_MODE;
// response.controlOutput.onboardControllerType = ANGLE_MODE;
}
void get_y_from_CFData( Controller::Request &request )
{
// get load orientation for transformation to world frame
......
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