From 1941f42fc7bdbf6641d4df36fa28799f6b519b5d Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Thu, 28 Nov 2019 17:28:26 +0100
Subject: [PATCH] Connected the CS1 integrator buttons to the GUI and tested to
be working in simulation. Needs testing on the real system
---
.../forms/csonecontrollertab.ui | 111 ++++++++-
.../include/Constants_for_Qt_compile.h | 19 ++
.../include/csonecontrollertab.h | 29 ++-
.../flyingAgentGUI/src/csonecontrollertab.cpp | 221 +++++++++++++++---
.../src/dfall_pkg/include/nodes/Constants.h | 18 ++
.../include/nodes/CsoneControllerService.h | 48 +++-
.../src/dfall_pkg/param/CsoneController.yaml | 7 +-
.../src/nodes/CsoneControllerService.cpp | 153 +++++++++++-
8 files changed, 549 insertions(+), 57 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 3815c3ca..a67692b9 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>2011</width>
+ <width>2417</width>
<height>1336</height>
</rect>
</property>
@@ -421,6 +421,80 @@
<property name="rightMargin">
<number>0</number>
</property>
+ <item row="1" column="7">
+ <layout class="QGridLayout" name="gridLayout_8">
+ <item row="2" column="0">
+ <widget class="QPushButton" name="reset_integrator_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>Reset</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_integrator_state">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>off</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="toggle_integrator_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>Toggle</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="6">
+ <spacer name="horizontalSpacer_7">
+ <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="0" column="4">
<spacer name="horizontalSpacer_3">
<property name="orientation">
@@ -453,7 +527,7 @@
</property>
</widget>
</item>
- <item row="0" column="6">
+ <item row="0" column="9">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -849,7 +923,7 @@
</size>
</property>
<property name="text">
- <string>20</string>
+ <string>10</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -913,7 +987,7 @@
</spacer>
</item>
<item row="0" column="2">
- <widget class="QLabel" name="label_lead_compenstor_2">
+ <widget class="QLabel" name="label_pid_controller">
<property name="font">
<font>
<weight>75</weight>
@@ -928,6 +1002,35 @@
</property>
</widget>
</item>
+ <item row="0" column="7">
+ <widget class="QLabel" name="label_integrator">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Integrator</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="8">
+ <spacer name="horizontalSpacer_8">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
</layout>
</item>
<item row="7" column="0">
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
index 3b5dab86..3a40cb92 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
@@ -205,6 +205,25 @@
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// These constants deine the current and interaction with an intergrator
+#define INTEGRATOR_FLAG_ON 1
+#define INTEGRATOR_FLAG_OFF 2
+#define INTEGRATOR_FLAG_UNKNOWN 3
+#define INTEGRATOR_FLAG_TOGGLE 4
+#define INTEGRATOR_FLAG_RESET 5
+
+
+
+
+
// ----------------------------------------------------------------------------------
//
//
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 f40fc702..a468c4da 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
@@ -72,11 +72,12 @@ using namespace QtCharts;
//#include <std_msgs/String.h>
// Include the DFALL message types
-//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/CustomButtonWithHeader.h"
// Include the DFALL service types
+#include "dfall_pkg/IntIntService.h"
#include "dfall_pkg/GetSetpointService.h"
// Include the shared definitions
@@ -121,6 +122,15 @@ private slots:
void analyseStepResponse();
void updateStepAnalysisLineEdit(float value, QLineEdit * lineEdit, int num_dec_places);
+
+ void on_perform_step_button_clicked();
+ void on_log_data_button_clicked();
+
+ void on_set_lead_compensator_parameters_button_clicked();
+
+ void on_toggle_integrator_button_clicked();
+ void on_reset_integrator_button_clicked();
+
void on_lineEdit_k_returnPressed();
void on_lineEdit_T_returnPressed();
void on_lineEdit_alpha_returnPressed();
@@ -129,10 +139,7 @@ private slots:
void on_lineEdit_step_size_returnPressed();
void on_lineEdit_step_duration_returnPressed();
- void on_set_lead_compensator_parameters_button_clicked();
- void on_perform_step_button_clicked();
- void on_log_data_button_clicked();
@@ -188,13 +195,19 @@ private:
// > For requesting the setpoint to be changed
ros::Publisher requestSetpointChangePublisher;
- // > For requestion the controller paramters to be changed
+ // > For requesting the controller paramters to be changed
ros::Publisher requestControllerParamterChangePublisher;
+ // > For requesting a change in the integrator state
+ ros::Publisher requestIntegratorStateChangePublisher;
+
// SUBSCRIBER
// > For being notified when the setpoint is changed
ros::Subscriber setpointChangedSubscriber;
+ // > For being notified when the integrator state
+ ros::Subscriber integratorStateChangedSubscriber;
+
// PUBLISHER
// > For notifying that a custom button is pressed
ros::Publisher customButtonPublisher;
@@ -209,7 +222,11 @@ private:
// For receiving message that the setpoint was changed
void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+ // For receiving message that the integrator state changes
+ void integratorStateChangedCallback(const dfall_pkg::IntWithHeader& newIntegratorState);
+
// Fill the header for a message
+ void fillIntMessageHeader( dfall_pkg::IntWithHeader & msg );
void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
void fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg );
@@ -221,6 +238,8 @@ private:
void publishControllerParamters(float k, float T, float alpha);
+ void publishRequestForIntegratorStateChange(int flag_to_publish);
+
};
#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 f868f8e2..3f1658e6 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
@@ -157,17 +157,21 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
// CREATE THE REQUEST CONTROLLER PARAMTER CHANGE PUBLISHER
requestControllerParamterChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("CsoneControllerService/RequestControllerParametersChange", 1);
- // SUBSCRIBE TO SETPOINT CHANGES
+ // CREATE THE REQUEST INTEGRATOR STATE CHANGE PUBLISHER
+ requestIntegratorStateChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("CsoneControllerService/RequestIntegratorStateChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES AND INTEGRATOR STATE CHANGES
// Only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
- setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("CsoneControllerService/SetpointChanged", 1, &CsoneControllerTab::setpointChangedCallback, this);
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("CsoneControllerService/SetpointChanged" , 1, &CsoneControllerTab::setpointChangedCallback , this);
+ integratorStateChangedSubscriber = nodeHandle_for_this_gui.subscribe("CsoneControllerService/IntegratorStateChanged", 1, &CsoneControllerTab::integratorStateChangedCallback, this);
}
// CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("CsoneControllerService/CustomButtonPressed", 1);
- // GET THE CURRENT SETPOINT
+ // GET THE CURRENT SETPOINT AND CURRENT INTEGRATOR STATE
// Only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
@@ -185,6 +189,23 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
// Inform the user
ROS_INFO("[CSONE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
}
+
+ // > Request the current integrator state
+ ros::ServiceClient getCurrentIntegatorStateServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::IntIntService>("CsoneControllerService/GetCurrentIntegratorState", false);
+ dfall_pkg::IntIntService getIntegrtorStateCall;
+ getIntegrtorStateCall.request.data = 0;
+ getCurrentIntegatorStateServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentIntegatorStateServiceClient.call(getIntegrtorStateCall))
+ {
+ dfall_pkg::IntWithHeader temp_msg;
+ temp_msg.data = getIntegrtorStateCall.response.data;
+ integratorStateChangedCallback(temp_msg);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[CSONE CONTROLLER GUI] Failed to get current integrator state from controller using the \"GetCurrentIntegratorState\" service");
+ }
}
#endif
@@ -266,6 +287,14 @@ void CsoneControllerTab::on_perform_step_button_clicked()
//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());
+ // Set all the fields to be blank
+ ui->lineEdit_step_start ->setText("---");
+ ui->lineEdit_step_end ->setText("---");
+ ui->lineEdit_overshoot_value ->setText("---");
+ ui->lineEdit_overshoot_percent ->setText("---");
+ ui->lineEdit_rise_time ->setText("---");
+ ui->lineEdit_settling_time ->setText("---");
+
// Set the flag that should store data
m_shouldStoreData_for_plotting = true;
@@ -333,6 +362,14 @@ void CsoneControllerTab::on_log_data_button_clicked()
//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());
+ // Set all the fields to be blank
+ ui->lineEdit_step_start ->setText("---");
+ ui->lineEdit_step_end ->setText("---");
+ ui->lineEdit_overshoot_value ->setText("---");
+ ui->lineEdit_overshoot_percent ->setText("---");
+ ui->lineEdit_rise_time ->setText("---");
+ ui->lineEdit_settling_time ->setText("---");
+
// Set the flag that should store data
m_shouldStoreData_for_plotting = true;
@@ -418,38 +455,30 @@ void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
ui->lineEdit_alpha->setText(qstr + QString::number( alpha, 'f', 2));
}
- // Call the function to publish the setpoint
+ // Call the function to publish the controller parameters
publishControllerParamters(k,T,alpha);
}
-void CsoneControllerTab::publishControllerParamters(float k, float T, float alpha)
-{
-#ifdef CATKIN_MAKE
- // Initialise the message as a local variable
- dfall_pkg::SetpointWithHeader msg;
- // Fill the header of the message
- fillSetpointMessageHeader( msg );
- // Fill in the controller values
- msg.x = k;
- msg.y = T;
- msg.z = alpha;
- // Publish the setpoint
- this->requestControllerParamterChangePublisher.publish(msg);
+void CsoneControllerTab::on_toggle_integrator_button_clicked()
+{
+ // Call the function to publish the request for a change in the integrator state
+ publishRequestForIntegratorStateChange(INTEGRATOR_FLAG_TOGGLE);
+}
- // Inform the user about the change
- ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for new controller parameters (k,T,alpha) = (" << k << ", "<< T << ", "<< alpha << ")");
-#else
- // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
- QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for: [" << k << ", "<< T << ", "<< alpha << "]";
-#endif
+void CsoneControllerTab::on_reset_integrator_button_clicked()
+{
+ // Call the function to publish the request for a change in the integrator state
+ publishRequestForIntegratorStateChange(INTEGRATOR_FLAG_RESET);
}
+
+
void CsoneControllerTab::on_lineEdit_k_returnPressed()
{
ui->set_lead_compensator_parameters_button->animateClick();
@@ -488,7 +517,6 @@ void CsoneControllerTab::on_lineEdit_step_duration_returnPressed()
-
// ----------------------------------------------------------------------------------
// PPPP OOO SSSS EEEEE DDDD A TTTTT A
// P P O O S E D D A A T A A
@@ -797,22 +825,25 @@ void CsoneControllerTab::analyseStepResponse()
// DISPLAY THE RESULTS IN THE FIELDS OF THE GUI
// Lock the mutex
- //m_chart_mutex.lock();
+ m_chart_mutex.lock();
// Call the "update" function for each
updateStepAnalysisLineEdit(setpoint_start , ui->lineEdit_step_start , 3);
updateStepAnalysisLineEdit(setpoint_end , ui->lineEdit_step_end , 3);
updateStepAnalysisLineEdit(overshoot_value , ui->lineEdit_overshoot_value , 3);
updateStepAnalysisLineEdit(overshoot_percent , ui->lineEdit_overshoot_percent , 1);
+ updateStepAnalysisLineEdit(rise_time , ui->lineEdit_rise_time , 2);
+ updateStepAnalysisLineEdit(settling_time , ui->lineEdit_settling_time , 2);
// Unlock the mutex
- //m_chart_mutex.unlock();
+ m_chart_mutex.unlock();
}
+
void CsoneControllerTab::updateStepAnalysisLineEdit(float value, QLineEdit * lineEdit, int num_dec_places)
{
// Initialise a string variable for adding the "+"
@@ -879,6 +910,46 @@ void CsoneControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHe
+#ifdef CATKIN_MAKE
+void CsoneControllerTab::integratorStateChangedCallback(const dfall_pkg::IntWithHeader& newIntegratorState)
+{
+ // EXTRACT THE NEW STATE
+ int new_state = newIntegratorState.data;
+
+ // Lock the mutex
+ //m_chart_mutex.lock();
+
+ switch (new_state)
+ {
+ case INTEGRATOR_FLAG_ON:
+ {
+ ui->label_integrator_state->setText("on");
+ break;
+ }
+ case INTEGRATOR_FLAG_OFF:
+ {
+ ui->label_integrator_state->setText("off");
+ break;
+ }
+ case INTEGRATOR_FLAG_UNKNOWN:
+ {
+ ui->label_integrator_state->setText("Unknown");
+ break;
+ }
+ default:
+ {
+ ui->label_integrator_state->setText("Not recognised");
+ break;
+ }
+ }
+
+ // Unlock the mutex
+ //m_chart_mutex.unlock();
+}
+#endif
+
+
+
@@ -930,6 +1001,61 @@ void CsoneControllerTab::publishSetpoint(float x, float y, float z, float yaw_de
+void CsoneControllerTab::publishControllerParamters(float k, float T, float alpha)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the controller values
+ msg.x = k;
+ msg.y = T;
+ msg.z = alpha;
+
+ // Publish the setpoint
+ this->requestControllerParamterChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for new controller parameters (k,T,alpha) = (" << k << ", "<< T << ", "<< alpha << ")");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for: [" << k << ", "<< T << ", "<< alpha << "]";
+#endif
+}
+
+
+
+
+
+void CsoneControllerTab::publishRequestForIntegratorStateChange(int flag_to_publish)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::IntWithHeader msg;
+
+ // Fill the header of the message
+ fillIntMessageHeader( msg );
+
+ // Fill in the controller values
+ msg.data = flag_to_publish;
+
+ // Publish the setpoint
+ this->requestIntegratorStateChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for integrator state to change with flag " << flag_to_publish << ")");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for integrator state to change with flag " << flag_to_publish << ")";
+#endif
+}
+
+
+
+
// ----------------------------------------------------------------------------------
// A GGGG EEEEE N N TTTTT III DDDD SSSS
// A A G E NN N T I D D S
@@ -1025,6 +1151,49 @@ void CsoneControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool sh
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void CsoneControllerTab::fillIntMessageHeader( dfall_pkg::IntWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
#ifdef CATKIN_MAKE
// Fill the header for a message
void CsoneControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
index 2525931a..c28c297d 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
@@ -221,6 +221,24 @@
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// These constants deine the current and interaction with an intergrator
+#define INTEGRATOR_FLAG_ON 1
+#define INTEGRATOR_FLAG_OFF 2
+#define INTEGRATOR_FLAG_UNKNOWN 3
+#define INTEGRATOR_FLAG_TOGGLE 4
+#define INTEGRATOR_FLAG_RESET 5
+
+
+
+
// ----------------------------------------------------------------------------------
//
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index 085cdc5a..1dee19c3 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -68,6 +68,7 @@
// Include the DFALL service types
#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/IntIntService.h"
#include "dfall_pkg/GetSetpointService.h"
// Include the shared definitions
@@ -107,7 +108,6 @@ using namespace dfall_pkg;
-
// ----------------------------------------------------------------------------------
// V V A RRRR III A BBBB L EEEEE SSSS
// V V A A R R I A A B B L E S
@@ -163,13 +163,30 @@ std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00,
std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
+// Integrator gains
+float yaml_integratorGain_forThrust = 0.0;
+float yaml_integratorGain_forTauXY = 0.0;
+float yaml_integratorGain_forTauYaw = 0.0;
+
+// The state of the integrator
+int m_integratorState = INTEGRATOR_FLAG_OFF;
+
+// The current value of the integrator for each axis
+float m_newtons_intergal = 0.0;
+float m_tau_x_integral = 0.0;
+float m_tau_y_integral = 0.0;
+float m_tau_z_integral = 0.0;
+
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
// The state space matrices of lead compensator controller
-float m_A=0.0,m_B=0.0,m_C=0.0,m_D=1.0;
+float m_A=0.967216;
+float m_B=0.00491758;
+float m_C=-12;
+float m_D=2;
// The state of lead compensator controller
float m_controller_state = 0.0;
@@ -190,10 +207,14 @@ std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;
-// ROS Publisher for inform the network about
-// changes to the setpoin
+// ROS Publisher for informing the network about
+// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
+// ROS Publisher for informing the network about
+// changes to the integrator state
+ros::Publisher m_integratorStateChangedPublisher;
+
@@ -232,18 +253,27 @@ float computeMotorPolyBackward(float thrust);
// REQUEST SETPOINT CHANGE CALLBACK
void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
// REQUEST CONTROLLER PARAMETERS CALLBACK
-void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint);
+void requestIntegratorStateChangeCallback(const IntWithHeader& msg);
// CHANGE SETPOINT FUNCTION
-void setNewSetpoint(float x, float y, float z, float yaw);
+void setNewIntegratorState(int newIntegratorState);
+
+// GET CURRENT INTEGRATOR STATE SERVICE CALLBACK
+bool getCurrentIntegratorStateCallback(IntIntService::Request &request, IntIntService::Response &response);
+
+// REQUEST CONTROLLER PARAMETERS CALLBACK
+void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint);
// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
-// GET CURRENT SETPOINT SERVICE CALLBACK
-bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
-
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
diff --git a/dfall_ws/src/dfall_pkg/param/CsoneController.yaml b/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
index 89076bca..fd02486a 100644
--- a/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
@@ -1,5 +1,5 @@
# Mass of the crazyflie
-mass : 28
+mass : 30
# Frequency of the controller, in hertz
control_frequency : 200
@@ -26,3 +26,8 @@ gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.0
gainMatrixRollRate : [ 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00]
gainMatrixPitchRate : [ 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+
+# Integrator gains
+integratorGain_forThrust : 0.10
+integratorGain_forTauXY : 0.06
+integratorGain_forTauYaw : 0.05
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 1f90286f..0e94e7c6 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -272,17 +272,28 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > THIRD: perform the inner controller
pitchRate_forResponse = 5.0*(pitch_ref-request.ownCrazyflie.pitch);
-
-
- // m_beta_ref = 0.99*m_beta_ref - 1.71/5*stateErrorBody[0] - 1.33/5*m_controller_state;
- // m_controller_state = -stateErrorBody[0];
-
-
// Put the computed body rate commands into the "response" variable
response.controlOutput.roll = rollRate_forResponse;
response.controlOutput.pitch = pitchRate_forResponse;
response.controlOutput.yaw = yawRate_forResponse;
+
+
+
+ // PERFORM THE INTEGRATOR COMPUTATIONS
+ if (m_integratorState == INTEGRATOR_FLAG_ON)
+ {
+ m_newtons_intergal -= yaml_integratorGain_forThrust * stateErrorBody[2] / yaml_control_frequency;
+ m_tau_x_integral += yaml_integratorGain_forTauXY * stateErrorBody[1] / yaml_control_frequency;
+ m_tau_y_integral -= yaml_integratorGain_forTauXY * stateErrorBody[0] / yaml_control_frequency;
+ m_tau_z_integral -= yaml_integratorGain_forTauYaw * stateErrorBody[8] / yaml_control_frequency;
+ }
+
+ float cmd1_integral_adjustments = m_newtons_intergal - m_tau_x_integral - m_tau_y_integral - m_tau_z_integral;
+ float cmd2_integral_adjustments = m_newtons_intergal - m_tau_x_integral + m_tau_y_integral + m_tau_z_integral;
+ float cmd3_integral_adjustments = m_newtons_intergal + m_tau_x_integral + m_tau_y_integral - m_tau_z_integral;
+ float cmd4_integral_adjustments = m_newtons_intergal + m_tau_x_integral - m_tau_y_integral + m_tau_z_integral;
+
// Put the thrust commands into the "response" variable.
// The thrust adjustment computed by the controller need to be added to the
// the feed-forward thrust that "counter-acts" gravity.
@@ -296,10 +307,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float thrust_request_per_motor = thrustAdjustment / 4.0 + feed_forward_thrust_per_motor;
// > NOTE: the function "computeMotorPolyBackward" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrust_request_per_motor);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_request_per_motor);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_request_per_motor);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_request_per_motor);
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrust_request_per_motor + cmd1_integral_adjustments);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_request_per_motor + cmd2_integral_adjustments);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_request_per_motor + cmd3_integral_adjustments);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_request_per_motor + cmd4_integral_adjustments);
// PREPARE AND RETURN THE VARIABLE "response"
@@ -549,7 +560,6 @@ void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
}
}
-
// CHANGE SETPOINT FUNCTION
void setNewSetpoint(float x, float y, float z, float yaw)
{
@@ -574,7 +584,6 @@ void setNewSetpoint(float x, float y, float z, float yaw)
m_setpointChangedPublisher.publish(msg);
}
-
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
{
@@ -587,6 +596,93 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
return true;
}
+
+
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestIntegratorStateChangeCallback(const IntWithHeader& msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the request change
+ int requested_change = msg.data;
+
+ // Switch between the possible cases
+ switch (requested_change)
+ {
+ case INTEGRATOR_FLAG_ON:
+ setNewIntegratorState(INTEGRATOR_FLAG_ON);
+ break;
+
+ case INTEGRATOR_FLAG_OFF:
+ setNewIntegratorState(INTEGRATOR_FLAG_OFF);
+ break;
+
+ case INTEGRATOR_FLAG_UNKNOWN:
+ ROS_INFO("[CSONE CONTROLLER] Does not respond for the integrator to change to state INTEGRATOR_FLAG_UNKNOWN");
+ break;
+
+ case INTEGRATOR_FLAG_TOGGLE:
+ {
+ if (m_integratorState==INTEGRATOR_FLAG_OFF)
+ setNewIntegratorState(INTEGRATOR_FLAG_ON);
+ else
+ setNewIntegratorState(INTEGRATOR_FLAG_OFF);
+ break;
+ }
+
+ case INTEGRATOR_FLAG_RESET:
+ // Reset the integrator value to zero
+ m_newtons_intergal = 0.0;
+ m_tau_x_integral = 0.0;
+ m_tau_y_integral = 0.0;
+ m_tau_z_integral = 0.0;
+ // Inform the user
+ ROS_INFO("[CSONE CONTROLLER] integrator values were reset to zero.");
+ break;
+
+ default:
+ ROS_INFO_STREAM("[CSONE CONTROLLER] The requested integrator state change of " << requested_change << " is not recognised.");
+ break;
+ }
+ }
+}
+
+// CHANGE SETPOINT FUNCTION
+void setNewIntegratorState(int newIntegratorState){
+ // Directly set this to the class variable
+ m_integratorState = newIntegratorState;
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "IntWithHeader"
+ IntWithHeader msg;
+ // Fill in the integrator state
+ msg.data = m_integratorState;
+ // Publish the message
+ m_integratorStateChangedPublisher.publish(msg);
+}
+
+
+
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentIntegratorStateCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+ // Directly put the current integrator state into the response
+ response.data = m_integratorState;
+ // Return
+ return true;
+}
+
+
// REQUEST CONTROLLER PARAMETERS CALLBACK
// Using x y z from SetpointWithHeader for k T alpha controller parameters
void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint)
@@ -1038,6 +1134,39 @@ int main(int argc, char* argv[]) {
ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
+ // Instantiate the local variable:
+ // variable name: "requestIntegatorStateChangeSubscriber"
+ // variable type: ros::Subscriber
+ // name of message: "RequestIntegratorStateChange"
+ // The message has the structure defined in the file:
+ // "IntWithHeader.msg" (located in the "msg" folder).
+ // The messages received by this subscriber typically come from
+ // the "flying agent GUI".
+ ros::Subscriber requestIntegatorStateChangeSubscriber = nodeHandle.subscribe("RequestIntegratorStateChange", 1, requestIntegratorStateChangeCallback);
+
+ // Instantiate the class variable:
+ // variable name: "m_integratorStateChangedPublisher"
+ // variable type: ros::Publisher
+ // name of message: "IntegratorStateChanged"
+ // The message has the structure defined in the file:
+ // "IntWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current state of the integator.
+ m_integratorStateChangedPublisher = nodeHandle.advertise<IntWithHeader>("IntegratorStateChanged", 1);
+
+ // Instantiate the local variable:
+ // variable name: "getCurrentIntegratorStateService"
+ // variable type: ros::ServiceServer
+ // name of service: "GetCurrentIntegratorState"
+ // callback funcion: "getCurrentIntegratorStateCallback"
+ // This service has the input-output behaviour defined in the file:
+ // "IntIntService.srv" (located in the "srv" folder)
+ // This service, when called, is provided with an integer (that is
+ // essentially ignored), and is expected to respond with the current
+ // state of the integrator.
+ ros::ServiceServer getCurrentIntegratorStateService = nodeHandle.advertiseService("GetCurrentIntegratorState", getCurrentIntegratorStateCallback);
+
+
// Instantiate the local variable "service" to be a "ros::ServiceServer" type
// variable that advertises the service called "CsoneController". This service has
--
GitLab