Commit 36c16643 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Finished implmementing and testing the Tuning contorller

parent 2cb81cfc
......@@ -2833,7 +2833,7 @@
<item row="4" column="0">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Vertical Controller Gain</string>
<string>Vertical Controller Gain (Vertikal Regler Verstaekung)</string>
</property>
</widget>
</item>
......@@ -2905,7 +2905,7 @@
<item row="8" column="0">
<widget class="QLabel" name="label_46">
<property name="text">
<string>Heading Controller Gain</string>
<string>Heading Controller Gain (Orientierung Regler Verstaekung)</string>
</property>
</widget>
</item>
......@@ -2998,7 +2998,7 @@
<item row="0" column="0">
<widget class="QLabel" name="label_37">
<property name="text">
<string>Horizontal Controller Gain</string>
<string>Horizontal Controller Gain (Horizontal Regler Verstaekung)</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
......
......@@ -202,6 +202,19 @@ int test_all_currentpoint = 1;
std::vector<float> test_all_setpoint1 (4,0.0);
std::vector<float> test_all_setpoint2 (4,0.0);
// Multipliers for the HORIZONTAL contorller
float multiplier_horizontal = 1.0;
float multiplier_horizontal_min = 0.9;
float multiplier_horizontal_max = 1.1;
// Multipliers for the VERTICAL contorller
float multiplier_vertical = 1.0;
float multiplier_vertical_min = 0.9;
float multiplier_vertical_max = 1.1;
// Multipliers for the HEADING contorller
float multiplier_heading = 1.0;
float multiplier_heading_min = 0.9;
float multiplier_heading_max = 1.1;
// LQR Gain matrix for tracking the angle commands from the Crazyflie
std::vector<float> gainMatrixRollRate_forRemoteControl (3,0.0);
......@@ -270,6 +283,30 @@ float lqr_angleRateNested_prev_pitchAngle = 0.0;
float lqr_angleRateNested_prev_yawAngle = 0.0;
// // SAME GAINS AGAIN BUT INSTEAD AS DEFAULTS
// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
// std::vector<float> gainMatrixThrust_NineStateVector_default (9,0.0);
// std::vector<float> gainMatrixRollRate_default (9,0.0);
// std::vector<float> gainMatrixPitchRate_default (9,0.0);
// std::vector<float> gainMatrixYawRate_default (9,0.0);
// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
// std::vector<float> gainMatrixThrust_SixStateVector_default (6,0.0);
// std::vector<float> gainMatrixRollAngle_default (6,0.0);
// std::vector<float> gainMatrixPitchAngle_default (6,0.0);
// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
// std::vector<float> gainMatrixThrust_SixStateVector_50Hz_default (6,0.0);
// std::vector<float> gainMatrixRollAngle_50Hz_default (6,0.0);
// std::vector<float> gainMatrixPitchAngle_50Hz_default (6,0.0);
// std::vector<float> gainMatrixRollRate_Nested_default (3,0.0);
// std::vector<float> gainMatrixPitchRate_Nested_default (3,0.0);
// std::vector<float> gainMatrixYawRate_Nested_default (3,0.0);
// The 16-bit command limits
float cmd_sixteenbit_min;
float cmd_sixteenbit_max;
......
......@@ -74,6 +74,15 @@
>
</node>
<!-- TUNING CONTROLLER -->
<node
pkg = "d_fall_pps"
name = "TuningControllerService"
output = "screen"
type = "TuningControllerService"
>
</node>
<!-- PARAMETER SERVICE -->
<node
pkg = "d_fall_pps"
......@@ -113,6 +122,11 @@
file = "$(find d_fall_pps)/param/RemoteController.yaml"
ns = "RemoteController"
/>
<rosparam
command = "load"
file = "$(find d_fall_pps)/param/TuningController.yaml"
ns = "TuningController"
/>
</node>
<!-- AGENT GUI (aka. the "student GUI") -->
......
......@@ -5,8 +5,8 @@ mpcController: "MpcControllerService/MpcController"
remoteController: "RemoteControllerService/RemoteController"
tuningController: "TuningControllerService/TuningController"
strictSafety: false
angleMargin: 0.6
strictSafety: true
angleMargin: 0.8
battery_threshold_while_flying: 2.8 # in V
battery_threshold_while_motors_off: 3.30 # in V
......
......@@ -2,20 +2,31 @@
# PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
# Setpoint for the HORIZONTAL test
test_horizontal_setpoint1 : [0.3, 0.0, 0.4, 0.0];
test_horizontal_setpoint2 : [-0.3, 0.0, 0.4, 0.0];
test_horizontal_setpoint1 : [0.3, 0.0, 0.4, 0.0]
test_horizontal_setpoint2 : [-0.3, 0.0, 0.4, 0.0]
# Setpoint for the VERTICAL test
test_vertical_setpoint1 : [0.0, 0.0, 0.4, 0.0];
test_vertical_setpoint2 : [0.0, 0.0, 0.8, 0.0];
test_vertical_setpoint1 : [0.0, 0.0, 0.4, 0.0]
test_vertical_setpoint2 : [0.0, 0.0, 0.8, 0.0]
# Setpoint for the HEADING test
test_heading_setpoint1 : [0.0, 0.0, 0.4, 0.0];
test_heading_setpoint2 : [0.0, 0.0, 0.4, 1.5];
test_heading_setpoint1 : [0.0, 0.0, 0.4, 0.0]
test_heading_setpoint2 : [0.0, 0.0, 0.4, 3.14]
# Setpoint for the ALL test
test_all_setpoint1 : [0.3, 0.0, 0.4, 0.0];
test_all_setpoint2 : [-0.3, 0.0, 0.8, 1.5];
test_all_setpoint1 : [0.3, 0.0, 0.4, 0.0]
test_all_setpoint2 : [-0.3, 0.0, 0.8, 3.14]
# Multipliers for the HORIZONTAL contorller
multiplier_horizontal_min : 0.1
multiplier_horizontal_max : 2.2
# Multipliers for the VERTICAL contorller
multiplier_vertical_min : 0.1
multiplier_vertical_max : 1.8
# Multipliers for the HEADING contorller
multiplier_heading_min : 0.05
multiplier_heading_max : 3.0
# ***************************************************************************** #
......
......@@ -232,6 +232,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test the HORIZONTAL controller
case 1:
{
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform HORIZONTAL test");
switch (test_horizontal_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
......@@ -269,6 +272,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test the VERTICAL controller
case 2:
{
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform VERTICAL test");
switch (test_vertical_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
......@@ -306,6 +312,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test the HEADING controller
case 3:
{
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform HEADING test");
switch (test_heading_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
......@@ -343,6 +352,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test ALL the controllers
case 4:
{
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform ALL test");
switch (test_all_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
......@@ -390,19 +402,40 @@ void activateTestCallback(const std_msgs::Int32& msg)
// CHANGE THE GAIN FOR THE HORIZONTAL CONTROLLER
void horizontalGainCallback(const std_msgs::Int32& msg)
{
// Get the value from the message
float value = float(msg.data) / 1000.0;
// Compute the new multiplier value
multiplier_horizontal = multiplier_horizontal_min + value * (multiplier_horizontal_max-multiplier_horizontal_min);
// Display the value received
ROS_INFO_STREAM("[TUNING CONTROLLER] Received new HORIZONTAL gain with value " << value << ", the multiplier now = " << multiplier_horizontal );
}
// CHANGE THE GAIN FOR THE VERTICAL CONTROLLER
void verticalGainCallback(const std_msgs::Int32& msg)
{
// Get the value from the message
float value = float(msg.data) / 1000.0;
// Compute the new multiplier value
multiplier_vertical = multiplier_vertical_min + value * (multiplier_vertical_max-multiplier_vertical_min);
// Display the value received
ROS_INFO_STREAM("[TUNING CONTROLLER] Received new VERTICAL gain with value " << value << ", the multiplier now = " << multiplier_vertical );
}
// CHANGE THE GAIN FOR THE HEADING CONTROLLER
void headingGainCallback(const std_msgs::Int32& msg)
{
// Get the value from the message
float value = float(msg.data) / 1000.0;
// Compute the new multiplier value
multiplier_heading = multiplier_heading_min + value * (multiplier_heading_max-multiplier_heading_min);
// Display the value received
ROS_INFO_STREAM("[TUNING CONTROLLER] Received new HEADING gain with value " << value << ", the multiplier now = " << multiplier_heading );
}
......@@ -633,13 +666,13 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
for(int i = 0; i < 9; ++i)
{
// BODY FRAME Y CONTROLLER
rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME YAW CONTROLLER
yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i] * multiplier_heading;
// > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i] * multiplier_vertical;
}
......@@ -722,11 +755,11 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i] * multiplier_horizontal;
// > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i] * multiplier_vertical;
}
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
......@@ -789,11 +822,11 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// > ALITUDE CONTROLLER (i.e., z-controller):
lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i] * multiplier_vertical;
}
// BODY FRAME Z CONTROLLER
......@@ -830,7 +863,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// BODY FRAME X CONTROLLER
pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
// BODY FRAME Z CONTROLLER
yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i] * multiplier_heading;
}
......@@ -1252,8 +1285,19 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint1", test_all_setpoint1, 4);
getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint2", test_all_setpoint2, 4);
// Multipliers for the HORIZONTAL contorller
multiplier_horizontal_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_horizontal_min");
multiplier_horizontal_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_horizontal_max");
// Multipliers for the VERTICAL contorller
multiplier_vertical_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_vertical_min");
multiplier_vertical_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_vertical_max");
// Multipliers for the HEADING contorller
multiplier_heading_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_heading_min");
multiplier_heading_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_heading_max");
// // DEBUGGING: Print out one of the parameters that was loaded
// ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/default_viconObjectName_forRemote = " << default_viconObjectName_forRemote);
ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: the fetched TuningController/multiplier_horizontal_min = " << multiplier_horizontal_min);
// ******************************************************************************* //
......@@ -1355,21 +1399,8 @@ void processFetchedParameters()
// Set that the estimator frequency is the same as the control frequency
estimator_frequency = vicon_frequency;
// // Roll and pitch limit (in degrees for angles)
// remoteControlLimit_roll = remoteControlLimit_roll_degrees * PI / 180.0;
// remoteControlLimit_pitch = remoteControlLimit_pitch_degrees * PI / 180.0;
// // Use the Remote name if the variable is currently empty
// if (viconObjectName_forRemote == "empty")
// {
// viconObjectName_forRemote = default_viconObjectName_forRemote;
// }
// DEBUGGING: Print out one of the computed quantities
//ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: after processing the viconObjectName_forRemote = " << viconObjectName_forRemote);
ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: after processing the gravity_force_quarter = " << gravity_force_quarter);
}
......
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