Commit 1a23b7ff authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Small changes to controller tuning

parent 1441cc60
......@@ -128,8 +128,8 @@ using namespace d_fall_pps;
// Variables for controller
float cf_mass; // Crazyflie mass in grams
std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
float control_frequency; // Frequency at which the controller is running
float gravity_force; // The weight of the Crazyflie in Newtons, i.e., mg
float control_frequency = 200.0; // Frequency at which the controller is running
float gravity_force = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
......@@ -137,10 +137,10 @@ std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z)
// The LQR Controller parameters for "LQR_RATE_MODE"
const float gainMatrixRollRate[9] = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
const float gainMatrixPitchRate[9] = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
const float gainMatrixYawRate[9] = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
const float gainMatrixThrust[9] = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
std::vector<float> gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
std::vector<float> gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
std::vector<float> gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
// ROS Publisher for debugging variables
......
......@@ -960,17 +960,17 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Cont
// > body frame yaw angle,
// > total thrust adjustment.
// These will be requested from the "inner-loop" LQR controller below
angleResponseTest_prev_rollAngle = 0;
//angleResponseTest_prev_pitchAngle = 0;
//angleResponseTest_prev_rollAngle = 0;
angleResponseTest_prev_pitchAngle = 0;
angleResponseTest_prev_thrustAdjustment = 0;
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
angleResponseTest_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
//angleResponseTest_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
//angleResponseTest_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
angleResponseTest_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
// > ALITUDE CONTROLLER (i.e., z-controller):
angleResponseTest_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
}
......@@ -980,13 +980,23 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Cont
angleResponseTest_prev_yawAngle = stateErrorBody[8];
// COMPUTE THE DISTANCE FROM THE ORIGIN
if (stateErrorBody[0] > angleResponseTest_distance_meters)
// > for pitch response testing
// if (stateErrorBody[0] > angleResponseTest_distance_meters)
// {
// angleResponseTest_prev_pitchAngle = -angleResponseTest_pitchAngle_radians;
// }
// else if (stateErrorBody[0] < (-angleResponseTest_distance_meters) )
// {
// angleResponseTest_prev_pitchAngle = angleResponseTest_pitchAngle_radians;
// }
// > for roll response testing
if (stateErrorBody[1] > angleResponseTest_distance_meters)
{
angleResponseTest_prev_pitchAngle = -angleResponseTest_pitchAngle_radians;
angleResponseTest_prev_rollAngle = angleResponseTest_pitchAngle_radians;
}
else if (stateErrorBody[0] < (-angleResponseTest_distance_meters) )
else if (stateErrorBody[1] < (-angleResponseTest_distance_meters) )
{
angleResponseTest_prev_pitchAngle = angleResponseTest_pitchAngle_radians;
angleResponseTest_prev_rollAngle = -angleResponseTest_pitchAngle_radians;
}
......
......@@ -121,6 +121,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//INFORMATION: this ugly fix was needed for the older firmware
//outYaw *= 0.5;
//ROS_INFO_STREAM("y-error = " << state[1]);
//ROS_INFO_STREAM("y-velocity = " << state[4]);
//ROS_INFO_STREAM("roll = " << state[6]);
//ROS_INFO_STREAM("rollRate = " << outRoll );
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
......@@ -492,22 +497,24 @@ int main(int argc, char* argv[]) {
// Call the class function that loads the parameters for this class.
// Sleep for some time (in seconds)
ros::Duration(1.0).sleep();
// ros::Duration(1.0).sleep();
// // Ask the Paramter Service to load the respective YAML file
// std::string namespace_to_own_agent_loadYamlFiles_service = namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
// loadYamlFilesService_own_agent = ros::service::createClient<LoadYamlFiles>(namespace_to_own_agent_loadYamlFiles_service, true);
// ROS_INFO_STREAM("[SAFE CONTROLLER] Loaded service: " << loadYamlFilesService_own_agent.getService());
// Ask the Paramter Service to load the respective YAML file
std::string namespace_to_own_agent_loadYamlFiles_service = namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
loadYamlFilesService_own_agent = ros::service::createClient<LoadYamlFiles>(namespace_to_own_agent_loadYamlFiles_service, true);
ROS_INFO_STREAM("[SAFE CONTROLLER] Loaded service: " << loadYamlFilesService_own_agent.getService());
// LoadYamlFiles loadYamlFilesService;
// std::vector<std::string> yamlFileNames_to_load = {"SafeController"};
// loadYamlFilesService.request.yamlFileNames = yamlFileNames_to_load;
// bool success = loadYamlFilesService_own_agent.call(loadYamlFilesService);
LoadYamlFiles loadYamlFilesService;
std::vector<std::string> yamlFileNames_to_load = {"SafeController"};
loadYamlFilesService.request.yamlFileNames = yamlFileNames_to_load;
bool success = loadYamlFilesService_own_agent.call(loadYamlFilesService);
// ROS_INFO_STREAM("[SAFE CONTROLLER] called Laod Yaml File service with success = " << success);
ROS_INFO_STREAM("[SAFE CONTROLLER] called Laod Yaml File service with success = " << success);
// ros::Duration(2.0).sleep();
ros::Duration(2.0).sleep();
// Call the class function that loads the parameters for this class.
fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
......
......@@ -73,7 +73,7 @@
// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
// ----------------------------------------------------------------------------------
// This function is the callback that is linked to the "CustomController" service that
// This function is the callback that is linked to the "StudentController" service that
// is advertised in the main function. This must have arguments that match the
// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
// folder)
......@@ -276,8 +276,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
// **************************************
// BBBB OOO DDDD Y Y X X
......@@ -326,9 +325,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Put the computed roll rate into the "response" variable
response.controlOutput.roll = rollRate_forResponse;
// PREPARE AND RETURN THE VARIABLE "response"
/*choosing the Crazyflie onBoard controller type.
......@@ -579,7 +577,7 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
default:
{
// Let the user know that the command was not relevant
//ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
//ROS_INFO("The StudentControllerService received the message that YAML parameters were (re-)loaded");
//ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
break;
}
......@@ -592,28 +590,28 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
// your controller easier and quicker.
void fetchYamlParameters(ros::NodeHandle& nodeHandle)
{
// Here we load the parameters that are specified in the CustomController.yaml file
// Here we load the parameters that are specified in the StudentController.yaml file
// Add the "CustomController" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_customController(nodeHandle, "StudentController");
// Add the "StudentController" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_studentController(nodeHandle, "StudentController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
cf_mass = getParameterFloat(nodeHandle_for_studentController , "mass");
// Display one of the YAML parameters to debug if it is working correctly
//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
control_frequency = getParameterFloat(nodeHandle_for_customController, "control_frequency");
control_frequency = getParameterFloat(nodeHandle_for_studentController, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
getParameterFloatVector(nodeHandle_for_customController, "motorPoly", motorPoly, 3);
getParameterFloatVector(nodeHandle_for_studentController, "motorPoly", motorPoly, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass);
// Call the function that computes details an values that are needed from these
// parameters loaded above
......@@ -805,7 +803,7 @@ int main(int argc, char* argv[]) {
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
// Call the class function that loads the parameters for this class.
//fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
// *********************************************************************************
......@@ -823,7 +821,7 @@ int main(int argc, char* argv[]) {
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
// Instantiate the local variable "service" to be a "ros::ServiceServer" type
// variable that advertises the service called "CustomController". This service has
// variable that advertises the service called "StudentController". This service has
// the input-output behaviour defined in the "Controller.srv" file (located in the
// "srv" folder). This service, when called, is provided with the most recent
// measurement of the Crazyflie and is expected to respond with the control action
......
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