Commit 2fbe4f72 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Adjusted some variables names in the Student Controller. Removed includes of...

Adjusted some variables names in the Student Controller. Removed includes of the non-existent ...YAML.h message header. Database is the standard 4 agent grid
parent bd10c79c
......@@ -48,7 +48,6 @@
#include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h"
#include "d_fall_pps/CustomControllerYAML.h"
#include <std_msgs/Int32.h>
......
......@@ -63,7 +63,6 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/ViconSubscribeObjectName.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
......
......@@ -56,7 +56,6 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomControllerYAML.h"
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
......@@ -127,12 +126,12 @@ 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 = 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 cf_mass_in_grams = 25.0; // Crazyflie mass in grams
std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
float control_frequency = 200.0; // Frequency at which the controller is running
float cf_weight_in_newtons = 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
float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
......
......@@ -63,7 +63,6 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/ViconSubscribeObjectName.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
......
4,PPS_CF03,0/16/2M/E7E7E7E703,0,0.345924,-1.31629,-0.2,4.30616,1.15886,1.2
1,CF01,0/0/2M/E7E7E7E701,0,-1.95374,-0.0143279,-0.2,-1.01034,0.774692,1.2
2,CF02,0/8/2M/E7E7E7E702,1,-0.844536,-0.025763,-0.2,0.0759869,0.751821,1.2
3,CF03,0/16/2M/E7E7E7E703,2,0.213208,0.00282468,-0.2,1.18519,0.757539,1.2
4,CF04,0/24/2M/E7E7E7E704,3,1.34528,-0.0143279,-0.2,2.31154,0.723234,1.2
5,CF05,0/32/2M/E7E7E7E705,4,1.351,0.791844,-0.2,2.31154,1.54084,1.2
6,CF06,0/40/2M/E7E7E7E706,5,0.20749,0.843302,-0.2,1.18519,1.55799,1.2
7,CF07,0/48/2M/E7E7E7E707,6,-0.867406,0.843302,-0.2,0.0702693,1.54656,1.2
8,CF08,0/56/2M/E7E7E7E708,7,-1.95374,0.854737,-0.2,-0.987474,1.54656,1.2
......@@ -56,7 +56,6 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
......
......@@ -268,13 +268,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > 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.
// > NOTE: the "gravity_force" value was already divided by 4 when is was
// loaded from the .yaml file via the "fetchYamlParameters"
// class function in this file.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
// > NOTE: the "cf_weight_in_newtons" value is the total thrust required
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
float feed_forward_thrust_per_motor = cf_weight_in_newtons / 4.0;
// > NOTE: the function "computeMotorPolyBackward" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
......@@ -665,10 +668,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
ros::NodeHandle nodeHandle_for_studentController(nodeHandle, "StudentController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_studentController , "mass");
cf_mass_in_grams = 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 );
//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass_in_grams );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
......@@ -680,7 +683,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass);
ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass_in_grams);
// Call the function that computes details an values that are needed from these
// parameters loaded above
......@@ -696,10 +699,10 @@ void processFetchedParameters()
{
// Compute the feed-forward force that we need to counteract gravity (i.e., mg)
// > in units of [Newtons]
gravity_force = cf_mass * 9.81/(1000*4);
cf_weight_in_newtons = cf_mass_in_grams * 9.81/(1000*4);
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the graity force = " << gravity_force);
ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << cf_weight_in_newtons);
}
......
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