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 12146270 authored by josephp's avatar josephp
Browse files

integral controller for x and y

parent 2b720c43
......@@ -22,4 +22,6 @@ KX3 : 4.5499
KY1 : -1.6991
KY2 : -1.0195
KY3 : 4.5499
\ No newline at end of file
KY3 : 4.5499
gain_integrator : 0.1
\ No newline at end of file
......@@ -62,6 +62,9 @@ float yaw_int_error = 0;
float yaw_dif_error;
float yaw_stateErrorBody_old = 0;
float x_int_err = 0;
float y_int_err = 0;
float yawRate_forResponse;
float time_forward = 1;
......@@ -70,6 +73,8 @@ int time_direction = 2;
bool initial_setpoint_set = false;
bool turn = false;
float yaml_gain_integrator;
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
......@@ -332,12 +337,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
float feed_forward_thrust_per_motor = cf_weight_in_newtons / 4.0;
x_int_err += stateErrorBody[0]*0.005;
y_int_err += stateErrorBody[1]*0.005;
float gain_integrator = yaml_gain_integrator;
float trq_x = gain_integrator * x_int_err;
float trq_y = gain_integrator * y_int_err;
// > NOTE: the function "computeMotorPolyBackward" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor + trq_x - trq_y);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor - trq_x - trq_y);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor - trq_x + trq_y);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor + trq_x + trq_y);
......@@ -360,6 +373,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
// }
pitchRate_forResponse = -Kx1 * stateErrorBody[0] - Kx2 * stateErrorBody[3] - Kx3 * stateErrorBody[7];
// Put the computed pitch rate into the "response" variable
......@@ -826,6 +841,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// load TD
Ky3 = getParameterFloat(nodeHandle_for_studentController , "KY3");
yaml_gain_integrator = getParameterFloat(nodeHandle_for_studentController , "gain_integrator");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
getParameterFloatVector(nodeHandle_for_studentController, "motorPoly", motorPoly, 3);
......
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