From 9feb9547ebf24f7e9bf132f86ce818996ed3ac4d Mon Sep 17 00:00:00 2001
From: Jeremiah the Great <jcoulson@ethz.ch>
Date: Wed, 30 Nov 2022 11:39:44 +0100
Subject: [PATCH] Changes to CS1 controller for HS2022 sesssion
---
.../forms/csonecontrollertab.ui | 8 +-
.../flyingAgentGUI/src/csonecontrollertab.cpp | 92 ++++++++++++-------
.../include/nodes/CsoneControllerService.h | 29 ++++--
.../src/nodes/CsoneControllerService.cpp | 44 +++++----
4 files changed, 106 insertions(+), 67 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
index 09e4b6dc..8ec251bd 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
@@ -698,7 +698,7 @@
</font>
</property>
<property name="text">
- <string>0.5</string>
+ <string>0.50</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -1046,7 +1046,7 @@
</font>
</property>
<property name="text">
- <string>0.10</string>
+ <string>0.50</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -1105,7 +1105,7 @@
</font>
</property>
<property name="text">
- <string>0.0160</string>
+ <string>0.1000</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -1132,7 +1132,7 @@
</font>
</property>
<property name="text">
- <string>4.00</string>
+ <string>1.00</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
index 370e192a..9de2c40f 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
@@ -287,7 +287,7 @@ float CsoneControllerTab::validate_and_get_value_from_lineEdit(QLineEdit * lineE
if(!lineEdit->text().isEmpty())
{
return_value = (lineEdit->text()).toFloat();
- // Ensure that it is in the range [2,60]
+ // Ensure that it is in the range [min,max]
if (return_value < min)
return_value = min;
else if (return_value > max)
@@ -336,7 +336,7 @@ void CsoneControllerTab::on_perform_step_button_clicked()
ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
// Update the duration from the field
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,1.0,100.0,0,20.0);
// Set the minimum of the x-axis to agree with the duration
ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
@@ -390,7 +390,7 @@ void CsoneControllerTab::on_log_data_button_clicked()
ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
// Update the duration from the field
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,1.0,100.0,0,20.0);
// Set the minimum of the x-axis to agree with the duration
ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
@@ -508,13 +508,13 @@ void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
// Take the new value if available, otherwise use default
// > For k
- k = validate_and_get_value_from_lineEdit(ui->lineEdit_k,-100.0,100.0,4,0.016);
+ k = validate_and_get_value_from_lineEdit(ui->lineEdit_k,-1000.0,1000.0,4,0.1);
// > For T
- T = validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,2,4.0);
+ T = validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,1000.0,2,1.0);
// > For alpha
- alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.1,1.0,2,0.1);
+ alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.0,1.0,2,0.5);
// Unlock the mutex
m_controller_parameter_mutex.unlock();
@@ -532,10 +532,10 @@ void CsoneControllerTab::on_set_lag_compensator_parameters_button_clicked()
m_lag_controller_parameter_mutex.lock();
// > For T
- T = validate_and_get_value_from_lineEdit(ui->lineEdit_T_lag,0.0,100.0,2,0.0);
+ T = validate_and_get_value_from_lineEdit(ui->lineEdit_T_lag,0.0,1000.0,2,0.0);
// > For alpha
- alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha_lag,1.0,100.0,2,1.0);
+ alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha_lag,0.0,1000.0,2,1.0);
// Unlock the mutex
m_lag_controller_parameter_mutex.unlock();
@@ -547,7 +547,7 @@ void CsoneControllerTab::on_set_lag_compensator_parameters_button_clicked()
void CsoneControllerTab::on_set_time_delay_button_clicked()
{
// Take the new value if available, otherwise use default
- float time_delay_float = validate_and_get_value_from_lineEdit(ui->lineEdit_time_delay,0.0,1000.0,0,0.0);
+ float time_delay_float = validate_and_get_value_from_lineEdit(ui->lineEdit_time_delay,0.0,10000.0,0,0.0);
float time_delay_int = int(time_delay_float);
// Call the function to publish the time delay
@@ -557,7 +557,7 @@ void CsoneControllerTab::on_set_time_delay_button_clicked()
void CsoneControllerTab::on_set_pitch_error_button_clicked()
{
// Take the new value if available, otherwise use default
- float pitch_error = validate_and_get_value_from_lineEdit(ui->lineEdit_pitch_error,-60.0,60.0,3,0.0);
+ float pitch_error = validate_and_get_value_from_lineEdit(ui->lineEdit_pitch_error,-180.0,180.0,3,0.0);
// Call the function to publish the pitch error
publishRequestForPitchErrorChange(pitch_error);
@@ -640,49 +640,49 @@ void CsoneControllerTab::on_lineEdit_step_duration_returnPressed()
void CsoneControllerTab::on_lineEdit_k_editingFinished()
{
m_controller_parameter_mutex.lock();
- validate_and_get_value_from_lineEdit(ui->lineEdit_k,-100.0,100.0,4,0.016);
+ validate_and_get_value_from_lineEdit(ui->lineEdit_k,-1000.0,1000.0,4,0.1);
m_controller_parameter_mutex.unlock();
}
void CsoneControllerTab::on_lineEdit_T_editingFinished()
{
m_controller_parameter_mutex.lock();
- validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,2,4.0);
+ validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,1000.0,2,1.0);
m_controller_parameter_mutex.unlock();
}
void CsoneControllerTab::on_lineEdit_alpha_editingFinished()
{
m_controller_parameter_mutex.lock();
- validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.1,1.0,2,0.1);
+ validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.0,1.0,2,0.5);
m_controller_parameter_mutex.unlock();
}
void CsoneControllerTab::on_lineEdit_T_lag_editingFinished()
{
m_lag_controller_parameter_mutex.lock();
- validate_and_get_value_from_lineEdit(ui->lineEdit_T_lag,0.0,100.0,2,0.0);
+ validate_and_get_value_from_lineEdit(ui->lineEdit_T_lag,0.0,1000.0,2,0.0);
m_lag_controller_parameter_mutex.unlock();
}
void CsoneControllerTab::on_lineEdit_alpha_lag_editingFinished()
{
m_lag_controller_parameter_mutex.lock();
- validate_and_get_value_from_lineEdit(ui->lineEdit_alpha_lag,1.0,100.0,2,1.0);
+ validate_and_get_value_from_lineEdit(ui->lineEdit_alpha_lag,0.0,1000.0,2,1.0);
m_lag_controller_parameter_mutex.unlock();
}
void CsoneControllerTab::on_lineEdit_step_size_editingFinished()
{
m_chart_mutex.lock();
- validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
+ validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,2,0.5);
m_chart_mutex.unlock();
}
void CsoneControllerTab::on_lineEdit_step_duration_editingFinished()
{
m_chart_mutex.lock();
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,1.0,100.0,0,20.0);
m_chart_mutex.unlock();
}
@@ -802,7 +802,7 @@ void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x, float pitc
m_shouldPerformStep = false;
// Extract the current step size
- float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
+ float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,2,0.5);
// Determine the new x setpoint
float new_x = 0.0;
@@ -1421,9 +1421,9 @@ void CsoneControllerTab::simulate_step_response()
float current_setpoint_x = m_current_setpoint_x;
- float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
+ float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,2,0.5);
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,1.0,100.0,0,20.0);
float duration = m_step_response_data_recording_duration;
// Unlock the mutex
@@ -1432,7 +1432,7 @@ void CsoneControllerTab::simulate_step_response()
// Take the new value if available, otherwise use default
- float time_delay_float = validate_and_get_value_from_lineEdit(ui->lineEdit_time_delay,0.0,1000.0,0,0.0);
+ float time_delay_float = validate_and_get_value_from_lineEdit(ui->lineEdit_time_delay,0.0,10000.0,0,0.0);
float time_delay_int = int(time_delay_float);
// Compute the number of milliseconds per time step
@@ -1459,6 +1459,39 @@ void CsoneControllerTab::simulate_step_response()
float control_frequency = 200.0;
float control_deltaT = 0.005;
+ // Model parameters
+ float k_x = 0.2;
+ float m = 0.3;
+ float g = 9.81;
+
+ // Discrete time x subsystem matrices
+ // for k_x / m = 0.15, g = 9.81, T = 0.005:
+ // A =
+ // x1 x2 x3
+ // x1 1 0.004998 0.0001226
+ // x2 0 0.9993 0.04903
+ // x3 0 0 1
+
+ // B =
+ // u1
+ // x1 2.043e-07
+ // x2 0.0001226
+ // x3 0.005
+
+ // in general:
+ float A_x_x = 1.0;
+ float A_x_xdot = control_deltaT * (1 - k_x / m * control_deltaT / 2);
+ float A_x_pitch = g * control_deltaT * control_deltaT / 2;
+ float A_xdot_x = 0.0;
+ float A_xdot_xdot = 1 - k_x / m * control_deltaT;
+ float A_xdot_pitch = g * control_deltaT;
+ float A_pitch_x = 0.0;
+ float A_pitch_xdot = 0.0;
+ float A_pitch_pitch = 1.0;
+ float B_x_pitchRate = g * control_deltaT * control_deltaT * control_deltaT / 6;
+ float B_xdot_pitchRate = g * control_deltaT * control_deltaT / 2;
+ float B_pitch_pitchRate = control_deltaT;
+
float sim_time = 0.0;
int num_time_steps = int( duration / control_deltaT );
@@ -1572,22 +1605,11 @@ void CsoneControllerTab::simulate_step_response()
// > FOURTH: perform the inner controller
float pitch_rate = 5.0*(pitch_ref-current_pitch);
-// A =
-// x1 x2 x3
-// x1 1 0.004998 0.0001226
-// x2 0 0.9993 0.04903
-// x3 0 0 1
-
-// B =
-// u1
-// x1 2.043e-07
-// x2 0.0001226
-// x3 0.005
// Perform the evolution of the state
- current_x = current_x + 0.004998*current_xdot + 0.0001226*current_pitch;
- current_xdot = 0.9993*current_xdot + 0.04903*current_pitch + 0.0001226*pitch_rate;
- current_pitch = current_pitch + 0.005*pitch_rate;
+ current_x = A_x_x * current_x + A_x_xdot * current_xdot + A_x_pitch * current_pitch + B_x_pitchRate * pitch_rate;
+ current_xdot = A_xdot_x * current_x + A_xdot_xdot * current_xdot + A_xdot_pitch * current_pitch + B_xdot_pitchRate * pitch_rate;
+ current_pitch = A_pitch_x * current_x + A_pitch_xdot * current_xdot + A_pitch_pitch * current_pitch + B_pitch_pitchRate * pitch_rate;
// Update the min and max
min_x_value_sim = std::min( min_x_value_sim , current_x );
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index 6192a103..7ed1deaf 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -133,7 +133,7 @@ std::string m_namespace_to_coordinator_parameter_service;
// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
// > the mass of the crazyflie, in [grams]
-float yaml_cf_mass_in_grams = 25.0;
+float yaml_cf_mass_in_grams = 30.0;
// > the frequency at which the controller is running
float yaml_control_frequency = 200.0;
@@ -183,19 +183,30 @@ float m_tau_z_integral = 0.0;
float m_cf_weight_in_newtons = 0.0;
// The state space matrices of lead compensator controller
-float m_A=0.967216;
-float m_B=0.00491758;
-float m_C=-12.0;
-float m_D=2.0;
+// the following corresponds to k=0.2, T=1.5, alpha=0.1
+// this works well
+// float m_A=0.967216;
+// float m_B=0.00491758;
+// float m_C=-12.0;
+// float m_D=2.0;
+
+// the following corresponds to k=0.1, T=1.0, alpha=0.5
+// this is barely stabilizing
+float m_A = 0.99004983;
+float m_B = 0.00497508;
+float m_C = -0.2;
+float m_D = 0.2;
// The state of lead compensator controller
float m_controller_state = 0.0;
// The state space matrices of lag compensator controller
-float m_lag_A=0.0;
-float m_lag_B=0.0;
-float m_lag_C=0.0;
-float m_lag_D=1.0;
+// the following corresponds to T = 0.0, alpha = 1.0
+// (i.e., lag controller inactive)
+float m_lag_A = 0.0;
+float m_lag_B = 0.0;
+float m_lag_C = 0.0;
+float m_lag_D = 1.0;
// The state of lag compensator controller
float m_lag_controller_state = 0.0;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index e6c3b637..265cf91f 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -830,14 +830,21 @@ void requestLagControllerParametersChangeCallback(const SetpointWithHeader& newS
// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
{
- if (alpha > 1){alpha = 1;} else if (alpha<0.1){alpha = 0.1;}
-
- if (T > 100){T = 100;} else if (T<0.1){T = 0.1;}
+ if (alpha > 0.0 && T > 0.0){
+ // valid parameters
+ m_A = pow(2.71828, (-1.0 / (yaml_control_frequency*alpha*T)));
+ m_B = -alpha * T * (m_A - 1.0);
+ m_C = k / (alpha * T) * (1.0 - 1.0 / alpha);
+ m_D = k / alpha;
+ }
+ else{
+ // if invalid parameters, act as a proportional controller with gain k
+ m_A = 0.0;
+ m_B = 0.0;
+ m_C = 0.0;
+ m_D = k;
+ }
- m_A = pow(2.71828,(-1.0/(yaml_control_frequency*alpha*T)));
- m_B = -alpha*T*(m_A-1.0);
- m_C = k/(alpha*T)*(1.0-1.0/alpha);
- m_D = k/alpha;
ROS_INFO_STREAM("[CSONE CONTROLLER] Parameters changed to k="<<k<<", T="<<T<<", alpha="<<alpha);
ROS_INFO_STREAM("[CSONE CONTROLLER] Matrices changed to A="<<m_A<<", B="<<m_B<<", C="<<m_C<<", D="<<m_D);
@@ -848,21 +855,20 @@ void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
// CHANGE LAG CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
void convertLagIntoDiscreteTimeParameters(float T, float alpha)
{
- if (alpha > 100){alpha = 100;} else if (alpha<1){alpha = 1;}
-
- if (T > 100){T = 100;} else if (T<0){T = 0;}
-
- if (T > 0){
- m_lag_A = pow(2.71828,(-1.0/(yaml_control_frequency*alpha*T)));
- m_lag_B = -alpha*T*(m_lag_A-1.0);
- m_lag_C = (alpha-1.0)/(alpha*T);
+ if (alpha > 0.0 && T > 0.0){
+ // valid parameters
+ m_lag_A = pow(2.71828, (-1.0/(yaml_control_frequency*alpha*T)));
+ m_lag_B = -alpha * T * (m_lag_A - 1.0);
+ m_lag_C = (alpha - 1.0) / (alpha * T);
+ m_lag_D = 1.0;
}
else{
- m_lag_A = 0;
- m_lag_B = 0;
- m_lag_C = 0;
+ // if invalid parameters, act as a proportional controller with gain alpha
+ m_lag_A = 0.0;
+ m_lag_B = 0.0;
+ m_lag_C = 0.0;
+ m_lag_D = alpha;
}
- m_lag_D = 1;
ROS_INFO_STREAM("[CSONE CONTROLLER] Lag parameters changed to T="<<T<<", alpha="<<alpha);
ROS_INFO_STREAM("[CSONE CONTROLLER] Lag matrices changed to A="<<m_lag_A<<", B="<<m_lag_B<<", C="<<m_lag_C<<", D="<<m_lag_D);
--
GitLab