Skip to content
Snippets Groups Projects
Commit f6c0ce23 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Added first draft of simulation to CS1 GUI

parent 005c044c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
+ 113
− 10
View file @ f6c0ce23
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>2417</width> <width>3197</width>
<height>1336</height> <height>1336</height>
</rect> </rect>
</property> </property>
...@@ -421,6 +421,22 @@ ...@@ -421,6 +421,22 @@
<property name="rightMargin"> <property name="rightMargin">
<number>0</number> <number>0</number>
</property> </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"> <item row="0" column="2">
<widget class="QLabel" name="label_time_delay"> <widget class="QLabel" name="label_time_delay">
<property name="font"> <property name="font">
...@@ -437,7 +453,7 @@ ...@@ -437,7 +453,7 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="8"> <item row="1" column="10">
<layout class="QGridLayout" name="gridLayout_8"> <layout class="QGridLayout" name="gridLayout_8">
<item row="2" column="0"> <item row="2" column="0">
<widget class="QPushButton" name="reset_integrator_button"> <widget class="QPushButton" name="reset_integrator_button">
...@@ -495,7 +511,7 @@ ...@@ -495,7 +511,7 @@
</item> </item>
</layout> </layout>
</item> </item>
<item row="0" column="7"> <item row="0" column="9">
<spacer name="horizontalSpacer_7"> <spacer name="horizontalSpacer_7">
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
...@@ -543,7 +559,7 @@ ...@@ -543,7 +559,7 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="0" column="10"> <item row="0" column="12">
<spacer name="horizontalSpacer_2"> <spacer name="horizontalSpacer_2">
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
...@@ -592,7 +608,7 @@ ...@@ -592,7 +608,7 @@
</font> </font>
</property> </property>
<property name="text"> <property name="text">
<string>0.1</string> <string>0.10</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
...@@ -641,7 +657,7 @@ ...@@ -641,7 +657,7 @@
</property> </property>
<property name="maximumSize"> <property name="maximumSize">
<size> <size>
<width>180</width> <width>240</width>
<height>60</height> <height>60</height>
</size> </size>
</property> </property>
...@@ -651,7 +667,7 @@ ...@@ -651,7 +667,7 @@
</font> </font>
</property> </property>
<property name="text"> <property name="text">
<string>0.016</string> <string>0.0160</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
...@@ -922,7 +938,7 @@ ...@@ -922,7 +938,7 @@
</font> </font>
</property> </property>
<property name="text"> <property name="text">
<string>0.2</string> <string>0.5</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
...@@ -1028,7 +1044,7 @@ ...@@ -1028,7 +1044,7 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="0" column="8"> <item row="0" column="10">
<widget class="QLabel" name="label_integrator"> <widget class="QLabel" name="label_integrator">
<property name="font"> <property name="font">
<font> <font>
...@@ -1044,7 +1060,7 @@ ...@@ -1044,7 +1060,7 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="0" column="9"> <item row="0" column="11">
<spacer name="horizontalSpacer_8"> <spacer name="horizontalSpacer_8">
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
...@@ -1123,6 +1139,80 @@ ...@@ -1123,6 +1139,80 @@
</item> </item>
</layout> </layout>
</item> </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> </layout>
</item> </item>
<item row="7" column="0"> <item row="7" column="0">
...@@ -1167,15 +1257,28 @@ ...@@ -1167,15 +1257,28 @@
<tabstop>lineEdit_k</tabstop> <tabstop>lineEdit_k</tabstop>
<tabstop>lineEdit_T</tabstop> <tabstop>lineEdit_T</tabstop>
<tabstop>lineEdit_alpha</tabstop> <tabstop>lineEdit_alpha</tabstop>
<tabstop>lineEdit_time_delay</tabstop>
<tabstop>lineEdit_kp</tabstop> <tabstop>lineEdit_kp</tabstop>
<tabstop>lineEdit_kd</tabstop> <tabstop>lineEdit_kd</tabstop>
<tabstop>lineEdit_step_size</tabstop> <tabstop>lineEdit_step_size</tabstop>
<tabstop>lineEdit_step_duration</tabstop> <tabstop>lineEdit_step_duration</tabstop>
<tabstop>set_lead_compensator_parameters_button</tabstop> <tabstop>set_lead_compensator_parameters_button</tabstop>
<tabstop>set_time_delay_button</tabstop>
<tabstop>set_pd_controller_parameters_button</tabstop> <tabstop>set_pd_controller_parameters_button</tabstop>
<tabstop>perform_step_button</tabstop> <tabstop>perform_step_button</tabstop>
<tabstop>log_data_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_x</tabstop>
<tabstop>chartView_for_pitch</tabstop>
<tabstop>lineEdit_overshoot_value</tabstop>
<tabstop>lineEdit_overshoot_percent</tabstop>
</tabstops> </tabstops>
<resources/> <resources/>
<connections/> <connections/>
......
dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
+ 43
− 1
View file @ f6c0ce23
...@@ -59,6 +59,8 @@ ...@@ -59,6 +59,8 @@
// For the chart: // For the chart:
#include <QChart> #include <QChart>
#include <QLineSeries> #include <QLineSeries>
#include <QList>
#include <QPointF>
using namespace QtCharts; using namespace QtCharts;
#ifdef CATKIN_MAKE #ifdef CATKIN_MAKE
...@@ -118,6 +120,9 @@ public slots: ...@@ -118,6 +120,9 @@ public slots:
private 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 newDataForPerformingStepAndPlotting(float x, float pitch);
void analyseStepResponse(); void analyseStepResponse();
void updateStepAnalysisLineEdit(float value, QLineEdit * lineEdit, int num_dec_places); void updateStepAnalysisLineEdit(float value, QLineEdit * lineEdit, int num_dec_places);
...@@ -126,6 +131,9 @@ private slots: ...@@ -126,6 +131,9 @@ private slots:
void on_perform_step_button_clicked(); void on_perform_step_button_clicked();
void on_log_data_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_lead_compensator_parameters_button_clicked();
void on_set_time_delay_button_clicked(); void on_set_time_delay_button_clicked();
...@@ -142,10 +150,17 @@ private slots: ...@@ -142,10 +150,17 @@ private slots:
void on_lineEdit_step_size_returnPressed(); void on_lineEdit_step_size_returnPressed();
void on_lineEdit_step_duration_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: private:
Ui::CsoneControllerTab *ui; Ui::CsoneControllerTab *ui;
...@@ -171,6 +186,9 @@ private: ...@@ -171,6 +186,9 @@ private:
float m_current_setpoint_z = 0.4; float m_current_setpoint_z = 0.4;
float m_current_setpoint_yaw = 0.0; 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 // FOR PLOTTING DATA ON THE CHART
// > Mutex for serialising acess to any charting variable // > Mutex for serialising acess to any charting variable
QMutex m_chart_mutex; QMutex m_chart_mutex;
...@@ -183,7 +201,7 @@ private: ...@@ -183,7 +201,7 @@ private:
// > Time (as a float) for the horizontal axis // > Time (as a float) for the horizontal axis
float m_time_for_step = 0.0f; float m_time_for_step = 0.0f;
// > The duration for which to record the step // > 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 // > Line Series for the x position data
QLineSeries *m_lineSeries_for_setpoint_x; QLineSeries *m_lineSeries_for_setpoint_x;
QLineSeries *m_lineSeries_for_measured_x; QLineSeries *m_lineSeries_for_measured_x;
...@@ -192,6 +210,28 @@ private: ...@@ -192,6 +210,28 @@ private:
QLineSeries *m_lineSeries_for_measured_pitch; 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 #ifdef CATKIN_MAKE
// PUBLISHER // PUBLISHER
...@@ -248,6 +288,8 @@ private: ...@@ -248,6 +288,8 @@ private:
void publishRequestForTimeDelayChange(int time_delay_to_publish); void publishRequestForTimeDelayChange(int time_delay_to_publish);
void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
}; };
#endif // CSONECONTROLLERTAB_H #endif // CSONECONTROLLERTAB_H
dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
+ 511
− 129
View file @ f6c0ce23
...@@ -56,6 +56,21 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) : ...@@ -56,6 +56,21 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
ui->lineEdit_kd->hide(); ui->lineEdit_kd->hide();
ui->set_pd_controller_parameters_button->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 // SETUP THE CHART VIEW FOR THE X POSITION
// //
...@@ -82,14 +97,26 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) : ...@@ -82,14 +97,26 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
m_lineSeries_for_setpoint_x = new QLineSeries(); m_lineSeries_for_setpoint_x = new QLineSeries();
m_lineSeries_for_measured_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 // 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_setpoint_x);
ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_measured_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 // Set the style of the series
m_lineSeries_for_setpoint_x->setPen(QPen( QBrush("blue") , 5.0 )); 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_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 // Set the initial axes limits
ui->chartView_for_x->chart()->createDefaultAxes(); ui->chartView_for_x->chart()->createDefaultAxes();
ui->chartView_for_x->chart()->axisX()->setMin(-1.0); ui->chartView_for_x->chart()->axisX()->setMin(-1.0);
...@@ -117,20 +144,23 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) : ...@@ -117,20 +144,23 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
// Initialise the line series to be used for plotting // Initialise the line series to be used for plotting
//m_lineSeries_for_setpoint_pitch = new QLineSeries(); //m_lineSeries_for_setpoint_pitch = new QLineSeries();
m_lineSeries_for_measured_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 // 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_setpoint_pitch);
ui->chartView_for_pitch->chart()->addSeries(m_lineSeries_for_measured_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 // Set the style of the series
//m_lineSeries_for_setpoint_pitch->setPen(QPen( QBrush("blue") , 5.0 )); //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_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 // Set the initial axes limits
ui->chartView_for_pitch->chart()->createDefaultAxes(); ui->chartView_for_pitch->chart()->createDefaultAxes();
ui->chartView_for_pitch->chart()->axisX()->setMin(-1.0); 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()->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); ui->chartView_for_pitch->chart()->axisY()->setMax(20.0);
...@@ -233,6 +263,37 @@ CsoneControllerTab::~CsoneControllerTab() ...@@ -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 // BBBB U U TTTTT TTTTT OOO N N SSSS
// B B U U T T O O NN N S // B B U U T T O O NN N S
...@@ -262,29 +323,7 @@ void CsoneControllerTab::on_perform_step_button_clicked() ...@@ -262,29 +323,7 @@ void CsoneControllerTab::on_perform_step_button_clicked()
ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step); ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
// Update the duration from the field // Update the duration from the field
if(!ui->lineEdit_step_duration->text().isEmpty()) m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
{
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));
}
// Set the minimum of the x-axis to agree with the duration // 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); ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
...@@ -338,29 +377,7 @@ void CsoneControllerTab::on_log_data_button_clicked() ...@@ -338,29 +377,7 @@ void CsoneControllerTab::on_log_data_button_clicked()
ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step); ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
// Update the duration from the field // Update the duration from the field
if(!ui->lineEdit_step_duration->text().isEmpty()) m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
{
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));
}
// Set the minimum of the x-axis to agree with the duration // 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); ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
...@@ -395,77 +412,99 @@ void CsoneControllerTab::on_log_data_button_clicked() ...@@ -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 void CsoneControllerTab::on_simulate_step_response_button_clicked()
// > For k {
if(!ui->lineEdit_k->text().isEmpty()) // 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(); shouldStartSimulation = false;
// 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));
}
} }
else 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 // Call the function that performs the simulation
if(!ui->lineEdit_T->text().isEmpty()) if (shouldStartSimulation)
{
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
{ {
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 // > For T
if(!ui->lineEdit_alpha->text().isEmpty()) T = validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,1,4.0);
{
alpha = (ui->lineEdit_alpha->text()).toFloat(); // > For alpha
// Ensure that it is in the range [0,100] alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.1,1.0,2,0.1);
if (alpha < 0.0)
{ // Unlock the mutex
alpha = 0.0; m_controller_parameter_mutex.unlock();
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));
}
// Call the function to publish the controller parameters // Call the function to publish the controller parameters
publishControllerParamters(k,T,alpha); publishControllerParamters(k,T,alpha);
...@@ -475,28 +514,9 @@ void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked() ...@@ -475,28 +514,9 @@ void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
void CsoneControllerTab::on_set_time_delay_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 // Take the new value if available, otherwise use default
if(!ui->lineEdit_time_delay->text().isEmpty()) 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);
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) );
// Call the function to publish the time delay // Call the function to publish the time delay
publishRequestForTimeDelayChange(time_delay_int); publishRequestForTimeDelayChange(time_delay_int);
...@@ -563,6 +583,46 @@ void CsoneControllerTab::on_lineEdit_step_duration_returnPressed() ...@@ -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 // PPPP OOO SSSS EEEEE DDDD A TTTTT A
// P P O O S E D D A A T A 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 ...@@ -676,7 +736,7 @@ void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x, float pitc
m_shouldPerformStep = false; m_shouldPerformStep = false;
// Extract the current step size // 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 // Determine the new x setpoint
float new_x = 0.0; float new_x = 0.0;
...@@ -1070,6 +1130,9 @@ void CsoneControllerTab::publishControllerParamters(float k, float T, float alph ...@@ -1070,6 +1130,9 @@ void CsoneControllerTab::publishControllerParamters(float k, float T, float alph
// TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator" // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for: [" << k << ", "<< T << ", "<< alpha << "]"; QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for: [" << k << ", "<< T << ", "<< alpha << "]";
#endif #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 ...@@ -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"); ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for time delay of " << time_delay_to_publish << " milliseconds");
#else #else
// TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator" // 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 #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 GGGG EEEEE N N TTTTT III DDDD SSSS
// A A G E NN N T I D D S // A A G E NN N T I D D S
......
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