Commit da24b1c1 authored by beuchatp's avatar beuchatp
Browse files

Adapted the Tuning Contorller Service ROS node to be integrated with the...

Adapted the Tuning Contorller Service ROS node to be integrated with the current standard and with its new GUI
parent a5eb01bc
......@@ -338,7 +338,8 @@ add_executable(StudentControllerService src/nodes/StudentControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
add_executable(TuningControllerService src/nodes/TuningControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
......
......@@ -39,9 +39,9 @@
#endif
#define P_GAIN_MIN_GUI 1
#define P_GAIN_MAX_GUI 10
#define P_TO_D_GAIN_RATIO_GUI 0.4
#define P_GAIN_MIN_GUI 0.10
#define P_GAIN_MAX_GUI 1.00
#define P_TO_D_GAIN_RATIO_GUI 0.6
#define DECIMAL_PLACES_POSITION_MEASURED 3
#define DECIMAL_PLACES_ANGLE_DEGREES 1
......
......@@ -55,19 +55,49 @@
#include "ros/ros.h"
#include <ros/package.h>
//the generated structs from the msg-files have to be included
// Include the standard message types
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
#include <std_msgs/String.h>
// Include the DFALL message types
#include "d_fall_pps/IntWithHeader.h"
#include "d_fall_pps/FloatWithHeader.h"
//#include "d_fall_pps/StringWithHeader.h"
#include "d_fall_pps/SetpointWithHeader.h"
//#include "d_fall_pps/CustomButtonWithHeader.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
//#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/ViconSubscribeObjectName.h"
// Include the Parameter Service shared definitions
// Include the DFALL service types
#include "d_fall_pps/LoadYamlFromFilename.h"
#include "d_fall_pps/GetSetpointService.h"
// Include the shared definitions
#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
// //the generated structs from the msg-files have to be included
// #include "d_fall_pps/ViconData.h"
// #include "d_fall_pps/Setpoint.h"
// #include "d_fall_pps/ControlCommand.h"
// #include "d_fall_pps/Controller.h"
// #include "d_fall_pps/DebugMsg.h"
// #include "d_fall_pps/CustomButton.h"
// #include "d_fall_pps/ViconSubscribeObjectName.h"
// // Include the Parameter Service shared definitions
// #include "nodes/Constants.h"
// #include <std_msgs/Int32.h>
......@@ -175,6 +205,16 @@ using namespace d_fall_pps;
// ----------------------------------------------------------------------------------
float m_gain_P = 0.31;
float m_gain_P_to_D_ratio = 0.6;
// ******************************************************************************* //
// VARIABLES SPECIFIC TO THE TUNING CONTROL FEATURE
......@@ -259,7 +299,7 @@ float setpoint[4] = {0.0,0.0,0.4,0.0};
// The controller type to run in the "calculateControlOutput" function
int controller_mode = CONTROLLER_MODE_LQR_RATE;
int controller_mode = CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED;
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
......@@ -461,24 +501,45 @@ void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float
// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);
// SETPOINT CHANGE CALLBACK
void setpointCallback(const Setpoint& newSetpoint);
//void setpointCallback(const Setpoint& newSetpoint);
// 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 SETPOINT CHANGE CALLBACK
void requestGainChangeCallback(const FloatWithHeader& newGain);
// LOAD PARAMETERS
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
// float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
// void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
// int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
// void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
// int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
// bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
// std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
//void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
//void fetchYamlParameters(ros::NodeHandle& nodeHandle);
//void processFetchedParameters();
void isReadyTuningControllerYamlCallback(const IntWithHeader & msg);
void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle);
// ******************************************************************************* //
......
......@@ -76,7 +76,7 @@ shouldDisplayDebugInfo : false
# - Swaps between pitch set-points to test angle set-point response time
# i.e., this controller test the assumption that "the inner loop is infinitely fast"
#
controller_mode : 3
controller_mode : 5
# A flag for which estimator to use, defined as:
......
......@@ -848,7 +848,7 @@ int main(int argc, char* argv[]) {
// <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
// This line of code adds a parameter named "agentID" to the
// "PPSClient" node.
// > Thus, to get access to this "studentID" paremeter, we first
// > Thus, to get access to this "agentID" paremeter, we first
// need to get a handle to the "PPSClient" node within which this
// controller service is nested.
......
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