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 200aa149 authored by elokdae's avatar elokdae
Browse files

Implemented Figure 8 in LQR

parent f69dd556
......@@ -891,7 +891,7 @@
</sizepolicy>
</property>
<property name="text">
<string>Spare</string>
<string>Changing Reference</string>
</property>
</widget>
</item>
......
......@@ -292,6 +292,16 @@ bool s_yaml_opt_steady_state = false;
bool s_yaml_grb_LogToFile = false;
bool s_yaml_grb_presolve_at_setup = false;
// Changing reference parameters
// Figure 8 amplitude, in m
float yaml_figure_8_amplitude = 0.5;
// Figure 8 frequency, in Hz
float yaml_figure_8_frequency = 0.1;
// z sine amplitude, in m
float yaml_z_sine_amplitude = 0.2;
// z sine frequency, in Hz
float yaml_z_sine_frequency = 0.1;
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
......@@ -357,6 +367,12 @@ MatrixXf m_y_data_Deepc = MatrixXf::Zero(0,0);
MatrixXf m_r_data_Deepc = MatrixXf::Zero(0,0);
int m_dataIndex_Deepc = 0;
// Variables used for changing reference
bool m_changing_ref_enable = false;
float m_figure_8_frequency_rad;
float m_z_sine_frequency_rad;
// Variables shared between main and Deepc thread
float s_cf_weight_in_newtons = m_cf_weight_in_newtons;
MatrixXf s_setpoint = MatrixXf::Zero(4, 1);
......@@ -620,6 +636,7 @@ void processCustomButton1(float float_data, int int_data, bool* bool_data);
void processCustomButton2(float float_data, int int_data, bool* bool_data);
void processCustomButton3(float float_data, int int_data, bool* bool_data);
void processCustomButton4(float float_data, int int_data, bool* bool_data);
void processCustomButton5(float float_data, int int_data, bool* bool_data);
// FOR LOADING THE YAML PARAMETERS
void isReadyDeepcControllerYamlCallback(const IntWithHeader& msg);
......
......@@ -58,6 +58,9 @@ gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0
#gainMatrixPitchRate : [ 2.97, 0.00, 0.00, 1.92, 0.00, 0.00, 0.00, 6.09, 0.00]
#gainMatrixYawRate : [ -0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 3.06]
# ------------------------------------------------------
# PARAMTERS FOR DEEPC
# Data collection max time, in minutes
data_collection_max_time : 10
......@@ -132,4 +135,17 @@ opt_steady_state : false # Available in sparse formulation only, ignored otherw
# Parameters specific to Gurobi
grb_LogToFile : false
grb_presolve_at_setup : true
\ No newline at end of file
grb_presolve_at_setup : true
# ------------------------------------------------------
# PARAMTERS FOR CHANGING REFERENCE
# This implements a 'Figure 8' in x and y and a sine in z
# Figure 8 amplitude, in m
figure_8_amplitude : 0.5
# Figure 8 frequency, in Hz
figure_8_frequency : 0.1
# z sine amplitude, in m
z_sine_amplitude : 0.2
# z sine frequency, in Hz
z_sine_frequency : 0.1
\ No newline at end of file
......@@ -2119,6 +2119,17 @@ void computeResponse_for_LQR(Controller::Request &request, Controller::Response
m_setpoint_for_controller[2] = m_setpoint[2];
m_setpoint_for_controller[3] = m_setpoint[3];
// Add 'Figure 8' (found here: "https://gamedev.stackexchange.com/questions/43691/how-can-i-move-an-object-in-an-infinity-or-figure-8-trajectory", as "Lemniscate of Bernoulli")
if (m_changing_ref_enable)
{
float scale = 2 / (3 - cos(2 * m_figure_8_frequency_rad * (m_time_in_seconds - PI/2))) * yaml_figure_8_amplitude;
m_setpoint_for_controller[0] += scale * cos(m_figure_8_frequency_rad * (m_time_in_seconds - PI/2));
m_setpoint_for_controller[1] += scale * sin(2 * m_figure_8_frequency_rad * (m_time_in_seconds - PI/2)) / 2;
m_setpoint_for_controller[2] += yaml_z_sine_amplitude * sin(m_z_sine_frequency_rad * m_time_in_seconds);
m_time_in_seconds += m_control_deltaT;
}
// Call the LQR control function
control_output output;
calculateControlOutput_viaLQR(request, output);
......@@ -3420,11 +3431,12 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
break;
// > FOR CUSTOM BUTTON 5 - SPARE
// > FOR CUSTOM BUTTON 5 - CHANGING REFERENCE
case 5:
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[DEEPC CONTROLLER] Button 5 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 5
processCustomButton5(float_data, int_data, bool_data);
break;
......@@ -3688,6 +3700,42 @@ void processCustomButton4(float float_data, int int_data, bool* bool_data)
}
}
// CUSTOM BUTTON 5 - CHANGING REFERENCE
void processCustomButton5(float float_data, int int_data, bool* bool_data)
{
// If already following changing reference, disable it and return
if (m_changing_ref_enable)
{
m_changing_ref_enable = false;
return;
}
// Switch between the possible states
switch (m_current_state)
{
case DEEPC_CONTROLLER_STATE_LQR:
// Inform the user
ROS_INFO("[DEEPC CONTROLLER] Received request to follow changing reference while in LQR");
// Reset time
m_time_in_seconds = 0.0;
// Set the flag
m_changing_ref_enable = true;
break;
case DEEPC_CONTROLLER_STATE_DEEPC:
case DEEPC_CONTROLLER_STATE_EXCITATION_LQR:
case DEEPC_CONTROLLER_STATE_EXCITATION_DEEPC:
case DEEPC_CONTROLLER_STATE_LANDING_MOVE_DOWN:
case DEEPC_CONTROLLER_STATE_LANDING_SPIN_MOTORS:
case DEEPC_CONTROLLER_STATE_STANDBY:
default:
// Inform the user
ROS_INFO("[DEEPC CONTROLLER] Received request to follow changing reference in invalid state. Request ignored");
break;
}
}
// ----------------------------------------------------------------------------------
// L OOO A DDDD
// L O O A A D D
......@@ -3849,6 +3897,19 @@ void fetchDeepcControllerYamlParameters(ros::NodeHandle& nodeHandle)
// Deepc prediction horizon
yaml_N = getParameterInt(nodeHandle_for_paramaters, "N");
// Changing reference parameters
// Figure 8 amplitude, in m
yaml_figure_8_amplitude = getParameterFloat(nodeHandle_for_paramaters, "figure_8_amplitude");
// Figure 8 frequency, in Hz
yaml_figure_8_frequency = getParameterFloat(nodeHandle_for_paramaters, "figure_8_frequency");
// z sine amplitude, in m
yaml_z_sine_amplitude = getParameterFloat(nodeHandle_for_paramaters, "z_sine_amplitude");
// z sine frequency, in Hz
yaml_z_sine_frequency = getParameterFloat(nodeHandle_for_paramaters, "z_sine_frequency");
// PARAMETERS ACCESSED BY DEEPC THREAD
s_Deepc_mutex.lock();
// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 2352");
......@@ -3949,6 +4010,12 @@ void fetchDeepcControllerYamlParameters(ros::NodeHandle& nodeHandle)
if (m_yawRateExcSignal.size() <= 0)
ROS_INFO("[DEEPC CONTROLLER] Failed to read yaw rate excitation signal file");
// > Compute the Figure 8 frequency in units of rad/s
m_figure_8_frequency_rad = 2 * PI * yaml_figure_8_frequency;
// > Compute the z sine frequency in units of rad/s
m_z_sine_frequency_rad = 2 * PI * yaml_z_sine_frequency;
// PARAMETERS ACCESSED BY DEEPC THREAD
s_Deepc_mutex.lock();
// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 2432");
......
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