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

centrlaized_control_cmds was not called in 100Hz

parent 1199a7af
......@@ -405,15 +405,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
y_only_agent_1(i,0) = 0;
}
*/
// compute centralized controller commands
centralized_control_command( request, response );
//display_control_agent1( request, response);
}
// compute centralized controller commands
centralized_control_command( request, response );
//display_control_agent1( request, response);
//float delta_z_load_current = request.otherCrazyflies[3].z - load_setpoint[2];
// gravity_force_copterloadsystem -= 0.001*delta_z_load_current;
// compute agent's lqr command for the setpoint
......@@ -627,16 +624,16 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
outYaw = u(11,0);
}
if (outer_loop_counter % 100 == 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 );
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 % 100 == 0)
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll estimate = " << estimator_state_prev[0] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch estimate = " << estimator_state_prev[1] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw estimate = " << estimator_state_prev[2] );
......@@ -748,8 +745,9 @@ void get_y_from_CFData( Controller::Request &request )
y_to_controller = y_meas - y_eq;
// Debugging
if (outer_loop_counter % 100 == 0)
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent : " << my_agentID);
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );
......@@ -850,8 +848,9 @@ void get_y_from_CFData_withLoad( Controller::Request &request )
y_to_controller = y_meas - y_eq;
// Debugging
if (outer_loop_counter % 100 == 0)
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent : " << my_agentID);
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );
......
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