From f6c0ce237877a06a174c20afce97472727dac158 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Fri, 6 Dec 2019 09:17:15 +0100
Subject: [PATCH] Added first draft of simulation to CS1 GUI
---
.../forms/csonecontrollertab.ui | 123 +++-
.../include/csonecontrollertab.h | 44 +-
.../flyingAgentGUI/src/csonecontrollertab.cpp | 640 ++++++++++++++----
3 files changed, 667 insertions(+), 140 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 2e7d1709..b9db68b0 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
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>2417</width>
+ <width>3197</width>
<height>1336</height>
</rect>
</property>
@@ -421,6 +421,22 @@
<property name="rightMargin">
<number>0</number>
</property>
+ <item row="0" column="8">
+ <widget class="QLabel" name="label_simulation">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Simulation</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
<item row="0" column="2">
<widget class="QLabel" name="label_time_delay">
<property name="font">
@@ -437,7 +453,7 @@
</property>
</widget>
</item>
- <item row="1" column="8">
+ <item row="1" column="10">
<layout class="QGridLayout" name="gridLayout_8">
<item row="2" column="0">
<widget class="QPushButton" name="reset_integrator_button">
@@ -495,7 +511,7 @@
</item>
</layout>
</item>
- <item row="0" column="7">
+ <item row="0" column="9">
<spacer name="horizontalSpacer_7">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -543,7 +559,7 @@
</property>
</widget>
</item>
- <item row="0" column="10">
+ <item row="0" column="12">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -592,7 +608,7 @@
</font>
</property>
<property name="text">
- <string>0.1</string>
+ <string>0.10</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -641,7 +657,7 @@
</property>
<property name="maximumSize">
<size>
- <width>180</width>
+ <width>240</width>
<height>60</height>
</size>
</property>
@@ -651,7 +667,7 @@
</font>
</property>
<property name="text">
- <string>0.016</string>
+ <string>0.0160</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -922,7 +938,7 @@
</font>
</property>
<property name="text">
- <string>0.2</string>
+ <string>0.5</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -1028,7 +1044,7 @@
</property>
</widget>
</item>
- <item row="0" column="8">
+ <item row="0" column="10">
<widget class="QLabel" name="label_integrator">
<property name="font">
<font>
@@ -1044,7 +1060,7 @@
</property>
</widget>
</item>
- <item row="0" column="9">
+ <item row="0" column="11">
<spacer name="horizontalSpacer_8">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -1123,6 +1139,80 @@
</item>
</layout>
</item>
+ <item row="0" column="7">
+ <spacer name="horizontalSpacer_simulation">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>100</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="1" column="8">
+ <layout class="QGridLayout" name="gridLayout_10">
+ <item row="2" column="0">
+ <widget class="QPushButton" name="clear_simulation_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Clear</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="simulate_step_response_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Perform</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_simulation_blank">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
</layout>
</item>
<item row="7" column="0">
@@ -1167,15 +1257,28 @@
<tabstop>lineEdit_k</tabstop>
<tabstop>lineEdit_T</tabstop>
<tabstop>lineEdit_alpha</tabstop>
+ <tabstop>lineEdit_time_delay</tabstop>
<tabstop>lineEdit_kp</tabstop>
<tabstop>lineEdit_kd</tabstop>
<tabstop>lineEdit_step_size</tabstop>
<tabstop>lineEdit_step_duration</tabstop>
<tabstop>set_lead_compensator_parameters_button</tabstop>
+ <tabstop>set_time_delay_button</tabstop>
<tabstop>set_pd_controller_parameters_button</tabstop>
<tabstop>perform_step_button</tabstop>
<tabstop>log_data_button</tabstop>
+ <tabstop>simulate_step_response_button</tabstop>
+ <tabstop>clear_simulation_button</tabstop>
+ <tabstop>toggle_integrator_button</tabstop>
+ <tabstop>reset_integrator_button</tabstop>
+ <tabstop>lineEdit_rise_time</tabstop>
+ <tabstop>lineEdit_settling_time</tabstop>
+ <tabstop>lineEdit_step_end</tabstop>
+ <tabstop>lineEdit_step_start</tabstop>
<tabstop>chartView_for_x</tabstop>
+ <tabstop>chartView_for_pitch</tabstop>
+ <tabstop>lineEdit_overshoot_value</tabstop>
+ <tabstop>lineEdit_overshoot_percent</tabstop>
</tabstops>
<resources/>
<connections/>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
index e45d064c..5ba4e055 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
@@ -59,6 +59,8 @@
// For the chart:
#include <QChart>
#include <QLineSeries>
+#include <QList>
+#include <QPointF>
using namespace QtCharts;
#ifdef CATKIN_MAKE
@@ -118,6 +120,9 @@ public slots:
private slots:
+
+ float validate_and_get_value_from_lineEdit(QLineEdit * lineEdit, float min, float max, int decimals, float default_value);
+
void newDataForPerformingStepAndPlotting(float x, float pitch);
void analyseStepResponse();
void updateStepAnalysisLineEdit(float value, QLineEdit * lineEdit, int num_dec_places);
@@ -126,6 +131,9 @@ private slots:
void on_perform_step_button_clicked();
void on_log_data_button_clicked();
+ void on_simulate_step_response_button_clicked();
+ void on_clear_simulation_button_clicked();
+
void on_set_lead_compensator_parameters_button_clicked();
void on_set_time_delay_button_clicked();
@@ -142,10 +150,17 @@ private slots:
void on_lineEdit_step_size_returnPressed();
void on_lineEdit_step_duration_returnPressed();
+ void on_lineEdit_k_editingFinished();
+ void on_lineEdit_T_editingFinished();
+ void on_lineEdit_alpha_editingFinished();
+ void on_lineEdit_step_size_editingFinished();
+ void on_lineEdit_step_duration_editingFinished();
+ void simulate_step_response();
+
private:
Ui::CsoneControllerTab *ui;
@@ -171,6 +186,9 @@ private:
float m_current_setpoint_z = 0.4;
float m_current_setpoint_yaw = 0.0;
+ // > Mutex for serialising acess to the controller variables
+ QMutex m_controller_parameter_mutex;
+
// FOR PLOTTING DATA ON THE CHART
// > Mutex for serialising acess to any charting variable
QMutex m_chart_mutex;
@@ -183,7 +201,7 @@ private:
// > Time (as a float) for the horizontal axis
float m_time_for_step = 0.0f;
// > The duration for which to record the step
- float m_step_response_data_recording_duration = 20.0f;
+ float m_step_response_data_recording_duration = 10.0f;
// > Line Series for the x position data
QLineSeries *m_lineSeries_for_setpoint_x;
QLineSeries *m_lineSeries_for_measured_x;
@@ -192,6 +210,28 @@ private:
QLineSeries *m_lineSeries_for_measured_pitch;
+ // FOR SIMULATING THE CONTROLLER
+ // > Mutex for serialising acess to any simulation variables
+ QMutex m_simulation_mutex;
+
+ // Flag for whether a simulation is under way
+ bool m_simulationIsInProgress = false;
+
+ // The state space matrices of lead compensator controller
+ float m_lead_compensator_A=0.987578;
+ float m_lead_compensator_B=0.00496888;
+ float m_lead_compensator_C=-0.36;
+ float m_lead_compensator_D=0.16;
+
+ // The state of lead compensator controller
+ float m_lead_compensator_state = 0.0;
+
+ // Line Series for plotting the simulation results
+ QLineSeries *m_lineSeries_for_sim_setpoint_x;
+ QLineSeries *m_lineSeries_for_sim_measured_x;
+ QLineSeries *m_lineSeries_for_sim_measured_pitch;
+
+
#ifdef CATKIN_MAKE
// PUBLISHER
@@ -248,6 +288,8 @@ private:
void publishRequestForTimeDelayChange(int time_delay_to_publish);
+ void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
+
};
#endif // CSONECONTROLLERTAB_H
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 a2114441..401093fb 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
@@ -56,6 +56,21 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
ui->lineEdit_kd->hide();
ui->set_pd_controller_parameters_button->hide();
+
+ // ------------------------------------------------------------- //
+ // HIDE THE SIMULATION BUTTONS
+ //ui->label_simulation->hide();
+ //ui->label_simulation_blank->hide();
+ //ui->simulate_step_response_button->hide();
+ //ui->clear_simulation_button->hide();
+
+
+ // ------------------------------------------------------------- //
+ // SET VALIDATORS FOR THE LINE EDITS
+ // > NOT USED BECAUSE DO NOT AUTOMATICALLY CLIP TO THE MIN AND MAX
+ //ui->lineEdit_k->setValidator(new QDoubleValidator(-1.0,1.0,2,ui->lineEdit_k) );
+
+
// ------------------------------------------------------------- //
// SETUP THE CHART VIEW FOR THE X POSITION
//
@@ -82,14 +97,26 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
m_lineSeries_for_setpoint_x = new QLineSeries();
m_lineSeries_for_measured_x = new QLineSeries();
+ m_lineSeries_for_sim_setpoint_x = new QLineSeries();
+ m_lineSeries_for_sim_measured_x = new QLineSeries();
+
// Add the line series to the chart
ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_setpoint_x);
ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_measured_x);
+ ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_sim_setpoint_x);
+ ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_sim_measured_x);
+
// Set the style of the series
m_lineSeries_for_setpoint_x->setPen(QPen( QBrush("blue") , 5.0 ));
m_lineSeries_for_measured_x->setPen(QPen( QBrush("red") , 5.0 ));
+ m_lineSeries_for_sim_setpoint_x->setPen(QPen( QBrush("blue") , 5.0 , Qt::DotLine ));
+ m_lineSeries_for_sim_measured_x->setPen(QPen( QBrush("red") , 5.0 , Qt::DotLine ));
+
+ // OPTIONS FOR THE LINE STYLE:
+ // { Qt::SolidLine , Qt::DashLine , Qt::DotLine , Qt::DashDotLine , Qt::DashDotDotLine }
+
// Set the initial axes limits
ui->chartView_for_x->chart()->createDefaultAxes();
ui->chartView_for_x->chart()->axisX()->setMin(-1.0);
@@ -117,20 +144,23 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
// Initialise the line series to be used for plotting
//m_lineSeries_for_setpoint_pitch = new QLineSeries();
m_lineSeries_for_measured_pitch = new QLineSeries();
+ m_lineSeries_for_sim_measured_pitch = new QLineSeries();
// Add the line series to the chart
//ui->chartView_for_pitch->chart()->addSeries(m_lineSeries_for_setpoint_pitch);
ui->chartView_for_pitch->chart()->addSeries(m_lineSeries_for_measured_pitch);
+ ui->chartView_for_pitch->chart()->addSeries(m_lineSeries_for_sim_measured_pitch);
// Set the style of the series
//m_lineSeries_for_setpoint_pitch->setPen(QPen( QBrush("blue") , 5.0 ));
m_lineSeries_for_measured_pitch->setPen(QPen( QBrush("red") , 5.0 ));
+ m_lineSeries_for_sim_measured_pitch->setPen(QPen( QBrush("red") , 5.0 , Qt::DotLine ));
// Set the initial axes limits
ui->chartView_for_pitch->chart()->createDefaultAxes();
ui->chartView_for_pitch->chart()->axisX()->setMin(-1.0);
ui->chartView_for_pitch->chart()->axisX()->setMax(m_step_response_data_recording_duration);
- ui->chartView_for_pitch->chart()->axisY()->setMin(20.0);
+ ui->chartView_for_pitch->chart()->axisY()->setMin(-20.0);
ui->chartView_for_pitch->chart()->axisY()->setMax(20.0);
@@ -233,6 +263,37 @@ CsoneControllerTab::~CsoneControllerTab()
+
+
+float CsoneControllerTab::validate_and_get_value_from_lineEdit(QLineEdit * lineEdit, float min, float max, int decimals, float default_value)
+{
+ // Initialise the value to the default
+ float return_value = default_value;
+
+ // Update the duration from the field
+ if(!lineEdit->text().isEmpty())
+ {
+ return_value = (lineEdit->text()).toFloat();
+ // Ensure that it is in the range [2,60]
+ if (return_value < min)
+ return_value = min;
+ else if (return_value > max)
+ return_value = max;
+ }
+
+ // Clip the value to the specified decimal places
+
+
+ // Put the value back into the line edit
+ lineEdit->setText(QString::number( return_value, 'f', decimals));
+
+ // Return the value
+ return return_value;
+}
+
+
+
+
// ----------------------------------------------------------------------------------
// BBBB U U TTTTT TTTTT OOO N N SSSS
// B B U U T T O O NN N S
@@ -262,29 +323,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
- if(!ui->lineEdit_step_duration->text().isEmpty())
- {
- float temp_duration = (ui->lineEdit_step_duration->text()).toFloat();
- // Ensure that it is in the range [2,60]
- if (temp_duration < 2.0)
- {
- temp_duration = 2.0;
- ui->lineEdit_step_duration->setText(QString::number( temp_duration, 'f', 0));
- }
- else if (temp_duration > 20.0)
- {
- temp_duration = 20.0;
- ui->lineEdit_step_duration->setText(QString::number( temp_duration, 'f', 0));
- }
- // Update the global variable
- m_step_response_data_recording_duration = temp_duration;
- }
- else
- {
- // Default the duration to 10
- m_step_response_data_recording_duration = 10.0;
- ui->lineEdit_step_duration->setText(QString::number( m_step_response_data_recording_duration, 'f', 0));
- }
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.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);
@@ -338,29 +377,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
- if(!ui->lineEdit_step_duration->text().isEmpty())
- {
- float temp_duration = (ui->lineEdit_step_duration->text()).toFloat();
- // Ensure that it is in the range [2,60]
- if (temp_duration < 2.0)
- {
- temp_duration = 2.0;
- ui->lineEdit_step_duration->setText(QString::number( temp_duration, 'f', 0));
- }
- else if (temp_duration > 60.0)
- {
- temp_duration = 60.0;
- ui->lineEdit_step_duration->setText(QString::number( temp_duration, 'f', 0));
- }
- // Update the global variable
- m_step_response_data_recording_duration = temp_duration;
- }
- else
- {
- // Default the duration to 10
- m_step_response_data_recording_duration = 10.0;
- ui->lineEdit_step_duration->setText(QString::number( m_step_response_data_recording_duration, 'f', 0));
- }
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.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);
@@ -395,77 +412,99 @@ void CsoneControllerTab::on_log_data_button_clicked()
}
-void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
-{
- // Initialise local variable for each of (x,y,z,yaw)
- float k = 1.0f, T = 1.0f, alpha = 1.0f;
- // Initialise a string variable for adding the "+"
- QString qstr = "";
- // Take the new value if available, otherwise use default
- // > For k
- if(!ui->lineEdit_k->text().isEmpty())
+void CsoneControllerTab::on_simulate_step_response_button_clicked()
+{
+ // Initialise a flag for whether to start a simulation or not
+ bool shouldStartSimulation = true;
+
+ // Set the flag that a simulation is being performed
+ m_simulation_mutex.lock();
+ if (m_simulationIsInProgress)
{
- k = (ui->lineEdit_k->text()).toFloat();
- // Ensure that it is in the range [0,100]
- if (k < 0.0)
- {
- k = 0.0;
- ui->lineEdit_k->setText(qstr + QString::number( k, 'f', 4));
- }
- else if (k > 100.0)
- {
- k = 100.0;
- ui->lineEdit_k->setText(qstr + QString::number( k, 'f', 4));
- }
+ shouldStartSimulation = false;
}
else
{
- ui->lineEdit_k->setText(qstr + QString::number( k, 'f', 4));
+ m_simulationIsInProgress = true;
+ // Set the button to not be avaialble
+ ui->simulate_step_response_button->setEnabled(false);
+ ui->clear_simulation_button->setEnabled(false);
}
+ m_simulation_mutex.unlock();
- // > For T
- if(!ui->lineEdit_T->text().isEmpty())
- {
- T = (ui->lineEdit_T->text()).toFloat();
- // Ensure that it is in the range [0,100]
- if (T < 0.0)
- {
- T = 0.0;
- ui->lineEdit_T->setText(qstr + QString::number( T, 'f', 1));
- }
- else if (T > 100.0)
- {
- T = 100.0;
- ui->lineEdit_T->setText(qstr + QString::number( T, 'f', 1));
- }
- }
- else
+ // Call the function that performs the simulation
+ if (shouldStartSimulation)
{
- ui->lineEdit_T->setText(qstr + QString::number( T, 'f', 1));
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Clear the line series
+ m_lineSeries_for_sim_setpoint_x->removePoints(0,m_lineSeries_for_sim_setpoint_x->count());
+ m_lineSeries_for_sim_measured_x->removePoints(0,m_lineSeries_for_sim_measured_x->count());
+ m_lineSeries_for_sim_measured_pitch->removePoints(0,m_lineSeries_for_sim_measured_pitch->count());
+
+ // > For x position
+ m_lineSeries_for_setpoint_x->removePoints(0,m_lineSeries_for_setpoint_x->count());
+ m_lineSeries_for_measured_x->removePoints(0,m_lineSeries_for_measured_x->count());
+ // > For pitch angles
+ //m_lineSeries_for_setpoint_pitch->removePoints(0,m_lineSeries_for_setpoint_pitch->count());
+ m_lineSeries_for_measured_pitch->removePoints(0,m_lineSeries_for_measured_pitch->count());
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ simulate_step_response();
+ //QTimer::singleShot(50, this, SLOT(simulate_step_response()));
}
+}
+
+
+void CsoneControllerTab::on_clear_simulation_button_clicked()
+{
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // First clear the line series
+ m_lineSeries_for_sim_setpoint_x->removePoints(0,m_lineSeries_for_sim_setpoint_x->count());
+ m_lineSeries_for_sim_measured_x->removePoints(0,m_lineSeries_for_sim_measured_x->count());
+ m_lineSeries_for_sim_measured_pitch->removePoints(0,m_lineSeries_for_sim_measured_pitch->count());
+
+ // > For x position
+ m_lineSeries_for_setpoint_x->removePoints(0,m_lineSeries_for_setpoint_x->count());
+ m_lineSeries_for_measured_x->removePoints(0,m_lineSeries_for_measured_x->count());
+ // > For pitch angles
+ //m_lineSeries_for_setpoint_pitch->removePoints(0,m_lineSeries_for_setpoint_pitch->count());
+ m_lineSeries_for_measured_pitch->removePoints(0,m_lineSeries_for_measured_pitch->count());
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+}
+
+
+
+
+void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
+{
+ // Initialise local variable for each of (x,y,z,yaw)
+ float k = 1.0f, T = 1.0f, alpha = 1.0f;
+
+ // Lock the mutex
+ m_controller_parameter_mutex.lock();
+
+ // 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);
// > For T
- if(!ui->lineEdit_alpha->text().isEmpty())
- {
- alpha = (ui->lineEdit_alpha->text()).toFloat();
- // Ensure that it is in the range [0,100]
- if (alpha < 0.0)
- {
- alpha = 0.0;
- ui->lineEdit_alpha->setText(qstr + QString::number( alpha, 'f', 2));
- }
- else if (T > 100.0)
- {
- T = 100.0;
- ui->lineEdit_alpha->setText(qstr + QString::number( alpha, 'f', 2));
- }
- }
- else
- {
- ui->lineEdit_alpha->setText(qstr + QString::number( alpha, 'f', 2));
- }
+ T = validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,1,4.0);
+
+ // > For alpha
+ alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.1,1.0,2,0.1);
+
+ // Unlock the mutex
+ m_controller_parameter_mutex.unlock();
// Call the function to publish the controller parameters
publishControllerParamters(k,T,alpha);
@@ -475,28 +514,9 @@ void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
void CsoneControllerTab::on_set_time_delay_button_clicked()
{
- // Initialise local variable for the time delay
- float time_delay_int = 0;
-
// Take the new value if available, otherwise use default
- if(!ui->lineEdit_time_delay->text().isEmpty())
- {
- float time_delay_float = (ui->lineEdit_time_delay->text()).toFloat();
- // Ensure that it is in the range [0,1000]
- if (time_delay_float < 0.0)
- time_delay_int = 0;
- else if (time_delay_float > 1000.0)
- time_delay_int = 1000;
- else
- time_delay_int = int(time_delay_float);
- }
- else
- {
- time_delay_int = 0;
- }
-
- // Put the value back into the line edit
- ui->lineEdit_time_delay->setText( QString::number( time_delay_int, 'd', 0) );
+ float time_delay_float = validate_and_get_value_from_lineEdit(ui->lineEdit_time_delay,0.0,1000.0,0,0.0);
+ float time_delay_int = int(time_delay_float);
// Call the function to publish the time delay
publishRequestForTimeDelayChange(time_delay_int);
@@ -563,6 +583,46 @@ 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);
+ 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,1,4.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);
+ m_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);
+ 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,20.0,0,10.0);
+ m_chart_mutex.unlock();
+}
+
+
+
+
// ----------------------------------------------------------------------------------
// PPPP OOO SSSS EEEEE DDDD A TTTTT A
// P P O O S E D D A A T A A
@@ -676,7 +736,7 @@ void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x, float pitc
m_shouldPerformStep = false;
// Extract the current step size
- float step_size = (ui->lineEdit_step_size->text()).toFloat();
+ float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
// Determine the new x setpoint
float new_x = 0.0;
@@ -1070,6 +1130,9 @@ void CsoneControllerTab::publishControllerParamters(float k, float T, float alph
// TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for: [" << k << ", "<< T << ", "<< alpha << "]";
#endif
+
+ // Call the function to convert this to a discrete-time state-space controller
+ convertIntoDiscreteTimeParameters(k, T, alpha);
}
@@ -1122,13 +1185,332 @@ void CsoneControllerTab::publishRequestForTimeDelayChange(int time_delay_to_publ
ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for time delay of " << time_delay_to_publish << " milliseconds");
#else
// TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
- QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for time delay of " << time_delay_to_publish << " milliseconds";
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for time delay of " << time_delay_to_publish << " milliseconds" << "\n";
+#endif
+}
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void CsoneControllerTab::convertIntoDiscreteTimeParameters(float k, float T, float alpha)
+{
+ float control_frequency = 200.0;
+
+ 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;}
+
+ // Lock the mutex
+ m_simulation_mutex.lock();
+
+ // Compute the A,B,C,D matrices
+ m_lead_compensator_A = pow(2.71828,(-1.0/(control_frequency*alpha*T)));
+ m_lead_compensator_B = -alpha*T*(m_lead_compensator_A-1.0);
+ m_lead_compensator_C = k/(alpha*T)*(1.0-1.0/alpha);
+ m_lead_compensator_D = k/alpha;
+
+ // Reset the state of the lead compensator to zero
+ m_lead_compensator_state = 0.0;
+
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] Parameters changed to k=" << k << ", T=" << T << ", alpha=" << alpha << "\n";
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] Matrices changed to A=" << m_lead_compensator_A << ", B=" << m_lead_compensator_B << ", C=" << m_lead_compensator_C << ", D=" << m_lead_compensator_D;
#endif
+
+ // Unlock the mutex
+ m_simulation_mutex.unlock();
+
}
+void CsoneControllerTab::simulate_step_response()
+{
+ // GET THE CONTROLLER PARAMETERS INTO LOCAL VARIABLES
+
+ // Lock the mutex
+ m_simulation_mutex.lock();
+
+ float A = m_lead_compensator_A;
+ float B = m_lead_compensator_B;
+ float C = m_lead_compensator_C;
+ float D = m_lead_compensator_D;
+
+ float controller_state = 0.0;
+
+ // Unlock the mutex
+ m_simulation_mutex.unlock();
+
+ // GET THE STEP SPECIFICATIONS INTO LOCAL VARIABLES
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ 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);
+
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+ float duration = m_step_response_data_recording_duration;
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
+
+ // 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_int = int(time_delay_float);
+
+ // Compute the number of milliseconds per time step
+ float delta_T_in_milliseconds = 1000.0 / 200.0;
+
+ // Convert the time delay to a number of time steps
+ int time_delay_in_steps = int( (float(time_delay_int) + 0.1) / delta_T_in_milliseconds );
+
+ // Wrap this value into the allowed limits
+ if (time_delay_in_steps<0)
+ time_delay_in_steps=0;
+ if(time_delay_in_steps>(300-1))
+ time_delay_in_steps=300-1;
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] time delay in steps = " << time_delay_in_steps << "\n";
+#endif
+
+
+ // PERFORM THE DURATION
+
+ // Initialise things
+ float control_frequency = 200.0;
+ float control_deltaT = 0.005;
+
+ float sim_time = 0.0;
+
+ int num_time_steps = int( duration / control_deltaT );
+
+ int num_point_per_append = 10;
+ int next_index = 0;
+
+ QVector<float> sim_traj_time (num_point_per_append,0);
+
+ //std::vector<float> sim_traj_x (num_time_steps,0);
+ QVector<float> sim_traj_x (num_point_per_append,0);
+ //QList<QPointF> sim_traj_x;
+
+ QVector<float> sim_traj_pitch (num_point_per_append,0);
+
+ // The inertial x measurement during a time window of 300 measurements
+ QVector<float> x_buffer (300,0);
+
+ float x_reference_start = current_setpoint_x;
+ // Determine the new x setpoint
+ float x_reference_end = 0.0;
+ if (x_reference_start < 0)
+ x_reference_end = 0.5 * step_size;
+ else
+ x_reference_end = -0.5 * step_size;
+
+ // Initialise the current state
+ float current_x = x_reference_start;
+ float current_xdot = 0.0;
+ float current_pitch = 0.0;
+
+ // Initialise variables for keeping track of the min and max
+ float min_x_value_sim = -0.01;
+ float max_x_value_sim = 0.01;
+ float min_pitch_value_sim = -0.01;
+ float max_pitch_value_sim = 0.01;
+
+ // Update the min and max
+ min_x_value_sim = std::min( min_x_value_sim , current_x );
+ max_x_value_sim = std::max( max_x_value_sim , current_x );
+ min_pitch_value_sim = std::min( min_pitch_value_sim , current_pitch*float(RAD2DEG) );
+ max_pitch_value_sim = std::max( max_pitch_value_sim , current_pitch*float(RAD2DEG) );
+
+
+ // Put in the initial conditions
+ sim_traj_time[next_index] = sim_time;
+
+ sim_traj_x[next_index] = current_x;
+ //sim_traj_x.append( QPointF(sim_time,current_x) );
+
+ sim_traj_pitch[next_index] = current_pitch;
+
+ next_index++;
+
+
+
+
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // For the x reference
+ m_lineSeries_for_sim_setpoint_x->append(0.0, x_reference_start );
+ m_lineSeries_for_sim_setpoint_x->append(0.0, x_reference_end );
+ m_lineSeries_for_sim_setpoint_x->append(duration, x_reference_end );
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
+ int write_index = 0;
+ int read_index = 0;
+
+
+ // Iterate over the duration
+ for ( int itime=1 ; itime<num_time_steps ; itime++ )
+ {
+ // Increment the write index
+ write_index += 1;
+ // And wrap it back into range if necessary
+ if (write_index>=x_buffer.size())
+ {
+ write_index = 0;
+ }
+
+ // Compute the next read index based on the delay
+ read_index = write_index - time_delay_in_steps;
+ // And wrap it back into range if necessary
+ if (read_index<0)
+ {
+ read_index += x_buffer.size();
+ }
+
+ // Write the new data to the buffer
+ x_buffer[write_index] = current_x;
+
+ // Read the data for this time step from the buffer
+ float x_for_this_time_step = x_buffer[read_index];
+
+ // Compute the error for this time step
+ float x_error = x_reference_end - x_for_this_time_step;
+
+ // Compute the input to apply
+ // > FIRST: compute the new pitch reference
+ float pitch_ref = C*controller_state + D*x_error;
+ // > SECOND: evaluate the state update equation
+ controller_state = A*controller_state + B*x_error;
+ // > THIRD: 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;
+
+ // Update the min and max
+ min_x_value_sim = std::min( min_x_value_sim , current_x );
+ max_x_value_sim = std::max( max_x_value_sim , current_x );
+ min_pitch_value_sim = std::min( min_pitch_value_sim , current_pitch*float(RAD2DEG) );
+ max_pitch_value_sim = std::max( max_pitch_value_sim , current_pitch*float(RAD2DEG) );
+
+ // Increment the simulation time
+ sim_time += control_deltaT;
+
+ // Store the state and time
+ sim_traj_time[next_index] = sim_time;
+
+ sim_traj_x[next_index] = current_x;
+ //sim_traj_x.append( QPointF(sim_time,current_x) );
+
+ sim_traj_pitch[next_index] = current_pitch;
+
+ // Increment the next index pointer
+ next_index++;
+
+ // Update the chart if required
+ if (next_index >= num_point_per_append)
+ {
+ // Lock the mutex
+ m_chart_mutex.lock();
+ // Iterate through the point
+ for (int ipoint=0 ; ipoint<num_point_per_append ; ipoint++)
+ {
+ m_lineSeries_for_sim_measured_x->append( sim_traj_time[ipoint] , sim_traj_x[ipoint] );
+ m_lineSeries_for_sim_measured_pitch->append( sim_traj_time[ipoint] , sim_traj_pitch[ipoint]*RAD2DEG );
+
+ //ui->chartView_for_x->repaint();
+ }
+ next_index = 0;
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+ }
+ }
+
+
+ // PUT THE SIMULATION RESULTS INTO THE PLOTTED LINE SERIES
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Update the chart with any remaining points
+ if (next_index > 0)
+ {
+ // Iterate through the point
+ for (int ipoint=0 ; ipoint<next_index ; ipoint++)
+ {
+ m_lineSeries_for_sim_measured_x->append( sim_traj_time[ipoint] , sim_traj_x[ipoint] );
+ m_lineSeries_for_sim_measured_pitch->append( sim_traj_time[ipoint] , sim_traj_pitch[ipoint]*RAD2DEG );
+ }
+ next_index = 0;
+ }
+
+ // Rezoom the x position chart
+ float diff_of_x_values_sim = max_x_value_sim - min_x_value_sim;
+ ui->chartView_for_x->chart()->axisY()->setMin( min_x_value_sim - 0.1*diff_of_x_values_sim );
+ ui->chartView_for_x->chart()->axisY()->setMax( max_x_value_sim + 0.1*diff_of_x_values_sim );
+ // Rezoom the pitch angles chart
+ float diff_of_pitch_values_sim = max_pitch_value_sim - min_pitch_value_sim;
+ ui->chartView_for_pitch->chart()->axisY()->setMin( min_pitch_value_sim - 0.1*diff_of_pitch_values_sim );
+ ui->chartView_for_pitch->chart()->axisY()->setMax( max_pitch_value_sim + 0.1*diff_of_pitch_values_sim );
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
+ // Set the flag that a simulation is completed
+ m_simulation_mutex.lock();
+ m_simulationIsInProgress = false;
+ // Set the button to not be avaialble
+ ui->simulate_step_response_button->setEnabled(true);
+ ui->clear_simulation_button->setEnabled(true);
+ m_simulation_mutex.unlock();
+
+
+}
+
+
+
// ----------------------------------------------------------------------------------
// A GGGG EEEEE N N TTTTT III DDDD SSSS
// A A G E NN N T I D D S
--
GitLab