Commit bbe5f841 authored by beuchatp's avatar beuchatp
Browse files

Tuning Controller Service now tested and working (although the z-controller is a bit sloppy)

parent da24b1c1
......@@ -136,6 +136,7 @@ set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/safecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/studentcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/topbanner.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/tuningcontrollertab.h
)
# Flying Agent GUI -- wrap UI file and QOBJECT files
qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
......@@ -150,6 +151,7 @@ qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/safecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/studentcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/topbanner.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/tuningcontrollertab.ui
)
qt5_wrap_cpp(SRC_MOC_HDRS_FLYING_AGENT_GUI ${SRC_HDRS_QOBJECT_FLYING_AGENT_GUI})
# Flying Agent GUI -- wrap resource file qrc->rcc
......@@ -392,6 +394,7 @@ set(MY_CPP_SOURCES_FLYING_AGENT_GUI # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/safecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/studentcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/topbanner.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/tuningcontrollertab.cpp
)
......
......@@ -39,8 +39,8 @@
#endif
#define P_GAIN_MIN_GUI 0.10
#define P_GAIN_MAX_GUI 1.00
#define P_GAIN_MIN_GUI 0.05
#define P_GAIN_MAX_GUI 1.60
#define P_TO_D_GAIN_RATIO_GUI 0.6
#define DECIMAL_PLACES_POSITION_MEASURED 3
......
......@@ -36,6 +36,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->picker_controller_tab_widget , &PickerControllerTab::setMeasuredPose
);
QObject::connect(
this , &ControllerTabs::measuredPoseValueChanged ,
ui->tuning_controller_tab_widget , &TuningControllerTab::setMeasuredPose
);
// CONNECT THE "MEASUREMENTS UNAVAILABLE" SIGNAL TO
......@@ -55,6 +60,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->picker_controller_tab_widget , &PickerControllerTab::poseDataUnavailableSlot
);
QObject::connect(
this , &ControllerTabs::poseDataUnavailableSignal ,
ui->tuning_controller_tab_widget , &TuningControllerTab::poseDataUnavailableSlot
);
// CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
// WITH THE LIST OF AGENT IDs TO COORDINATE
......@@ -76,6 +86,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->picker_controller_tab_widget , &PickerControllerTab::setAgentIDsToCoordinate
);
QObject::connect(
this , &ControllerTabs::agentIDsToCoordinateChanged ,
ui->tuning_controller_tab_widget , &TuningControllerTab::setAgentIDsToCoordinate
);
......
......@@ -350,7 +350,7 @@ void TuningControllerTab::publishGain(float new_gain)
d_fall_pps::FloatWithHeader msg;
// Fill the header of the message
fillSetpointMessageHeader( msg );
fillFloatMessageHeader( msg );
// Fill in the gain value
msg.data = new_gain;
......@@ -463,10 +463,10 @@ void TuningControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
setpointChangedSubscriber.shutdown();
// Set information back to the default
ui->lineEdit_setpoint_current_x->setText("xx.xx");
ui->lineEdit_setpoint_current_y->setText("xx.xx");
ui->lineEdit_setpoint_current_z->setText("xx.xx");
ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
ui->lineEdit_setpoint->setText("xx.xx");
//ui->lineEdit_setpoint_current_y->setText("xx.xx");
//ui->lineEdit_setpoint_current_z->setText("xx.xx");
//ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
}
#endif
......@@ -537,7 +537,7 @@ void TuningControllerTab::fillFloatMessageHeader( d_fall_pps::FloatWithHeader &
{
case TYPE_AGENT:
{
msg.shouldCheckForAgentID = false;
msg.shouldCheckForID = false;
break;
}
case TYPE_COORDINATOR:
......@@ -545,7 +545,7 @@ void TuningControllerTab::fillFloatMessageHeader( d_fall_pps::FloatWithHeader &
// Lock the mutex
m_agentIDs_toCoordinate_mutex.lock();
// Add the "coordinate all" flag
msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
msg.shouldCheckForID = !(m_shouldCoordinateAll);
// Add the agent IDs if necessary
if (!m_shouldCoordinateAll)
{
......@@ -561,7 +561,7 @@ void TuningControllerTab::fillFloatMessageHeader( d_fall_pps::FloatWithHeader &
default:
{
msg.shouldCheckForAgentID = true;
msg.shouldCheckForID = true;
ROS_ERROR("[TUNING CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
break;
}
......
......@@ -215,6 +215,8 @@ float m_gain_P_to_D_ratio = 0.6;
// ******************************************************************************* //
// VARIABLES SPECIFIC TO THE TUNING CONTROL FEATURE
......@@ -398,13 +400,13 @@ std::vector<float> PMKF_Kinf_for_angles (2,0.0);
// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service;
std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string namespace_to_coordinator_parameter_service;
std::string m_namespace_to_coordinator_parameter_service;
// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
ros::Publisher debugPublisher;
ros::Publisher m_debugPublisher;
// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
......@@ -425,8 +427,11 @@ bool shouldDisplayDebugInfo = false;
// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
// POSITION
// The ID of this agent, i.e., the ID of this compute
int my_agentID = 0;
// The ID of the agent that this node is monitoring
int m_agentID;
// The ID of the agent that can coordinate this node
int m_coordID;
......
......@@ -40,7 +40,7 @@ multiplier_heading_max : 3.0
# Mass of the crazyflie
mass : 29
mass : 30
# Frequency of the controller, in hertz
vicon_frequency : 200
......@@ -93,7 +93,7 @@ estimator_method : 1
# The LQR Controller parameters for "mode = 3"
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.76, 0.00, 0.00, 0.32, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
......
......@@ -739,6 +739,7 @@ void setInstantController(int controller) //for right now, temporal use
break;
case TUNING_CONTROLLER:
loadTuningController();
break;
case PICKER_CONTROLLER:
loadPickerController();
break;
......
......@@ -914,15 +914,15 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
//lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
//lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// > ALITUDE CONTROLLER (i.e., z-controller):
lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i] * multiplier_vertical;
}
lqr_angleRateNested_prev_rollAngle -= ( -1 * m_gain_P[i] * stateErrorBody[1] - m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[4] );
lqr_angleRateNested_prev_pitchAngle -= ( m_gain_P[i] * stateErrorBody[0] + m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[3] );
//lqr_angleRateNested_prev_rollAngle -= ( -1 * m_gain_P * stateErrorBody[1] - m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[4] );
lqr_angleRateNested_prev_pitchAngle -= ( m_gain_P * stateErrorBody[0] + m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[3] );
// BODY FRAME Z CONTROLLER
//lqr_angleRateNested_prev_yawAngle = setpoint[3];
......@@ -962,6 +962,17 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
}
float thrustAdjustment_200Hz = 0.0;
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 9; ++i)
{
// > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment_200Hz -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i] * multiplier_vertical;
}
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
......@@ -973,7 +984,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
float thrustAdjustment = thrustAdjustment_200Hz / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
......@@ -1049,7 +1060,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// debugMsg.value_10 = your_variable_name;
// Publish the "debugMsg"
debugPublisher.publish(debugMsg);
m_debugPublisher.publish(debugMsg);
}
......@@ -1305,12 +1316,14 @@ void setNewSetpoint(float x, float y, float z, float yaw)
void requestGainChangeCallback(const FloatWithHeader& newGain)
{
// Check whether the message is relevant
bool isRevelant = checkMessageHeader( m_agentID , newGain.shouldCheckForAgentID , newGain.agentIDs );
bool isRevelant = checkMessageHeader( m_agentID , newGain.shouldCheckForID , newGain.agentIDs );
// Continue if the message is relevant
if (isRevelant)
{
m_gain_P = newGain.data;
ROS_INFO_STREAM("[TUNING CONTROLLER] proportional gain changed to " << m_gain_P );
}
}
......@@ -1558,6 +1571,10 @@ int main(int argc, char* argv[]) {
// node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
// Get the namespace of this "TuningControllerService" node
std::string m_namespace = ros::this_node::getNamespace();
ROS_INFO_STREAM("[TUNING CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
// AGENT ID AND COORDINATOR ID
// NOTES:
......@@ -1729,7 +1746,15 @@ int main(int argc, char* argv[]) {
// And now we can instantiate the subscriber:
ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TuningControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
// Instantiate the local variable "requestGainChangeSubscriber"
// to be a "ros::Subscriber" type variable that subscribes to the
// "RequestGainChange" topic and calls the class function
// "requestGainChangeCallback" each time a messaged is received
// on this topic and the message is passed as an input argument to
// the callback function. This subscriber will mainly receive
// messages from the "flying agent GUI" when the gain is changed
// by the user.
ros::Subscriber requestGainChangeSubscriber = nodeHandle.subscribe("RequestGainChange", 1, requestGainChangeCallback);
......
Supports Markdown
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