diff --git a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
index 4174f8f453d5d4c22f40c0916735982e6b407f06..2dabd94ab846a7f7221bba50e6ec059e3dff576e 100644
--- a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
+++ b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
@@ -131,4 +131,7 @@ void constructNamespaceForCoordinator( int coordID, std::string & coord_namespac
void constructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace );
// Check the header of a message for whether it is relevant
-bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::vector<uint> & agentIDs );
\ No newline at end of file
+bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::vector<uint> & agentIDs );
+
+// FUNCTION FOR CONVERTING NEWTON TO MOTOR COMMAND
+float newtons2cmd_for_crazyflie( float thrust)
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/StudentController.yaml b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
index 737fea73b48980c4701f4c8f15ec4a9f74d3e076..9328045bc657c15ec4e183e46a5d00398da7c507 100644
--- a/dfall_ws/src/dfall_pkg/param/StudentController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
@@ -9,4 +9,9 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
# The default setpoint, the ordering is (x,y,z,yaw),
# with unit [meters,meters,meters,radians]
-default_setpoint : [0.0, 0.0, 0.4, 0.0]
\ No newline at end of file
+default_setpoint : [0.0, 0.0, 0.4, 0.0]
+
+# PID controller gain matrices
+proportionalgain_for_z : 4
+derivativegain_for_z : 1
+integralgain_for_z : 0.5
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
index c0865950e3f37775a4998a69faf505e1a7942634..762adfad75d38b1f76c36c412eaab4a6704e2884 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
@@ -242,4 +242,40 @@ bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::v
// If the function made it to here, then the message is
// NOT relevant, hence return false
return false;
+}
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+// FUNCTION FOR CONVERTING NEWTON TO MOTOR COMMAND
+
+float newtons2cmd_for_crazyflie( float thrust)
+{
+ // Compute the 16-bit command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float motoPoly = [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+ float cmd_16bit = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+
+ // Clip the cmd_16bit to avoid wrapping
+ if cmd_16bit > 60000
+ cmd_16bit = 60000
+ else if cmd_16bit < 2000
+ cmd_16bit = 0
+
+ // Return the result
+ return cmd_16bit;
}
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index 4266a77b9b69f81534e15062e2555bf9d534f4b2..742c5484551acb2366ea2664829293af1d454826 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -37,7 +37,27 @@
#include "nodes/StudentControllerService.h"
+// ---------------------------------------------------------------------------------------------------
+// CCCC L A SSSS SSSS V V A RRRR III A BBBB L EEEEE SSSS
+// C L A A S S V V A A R R I A A B B L E S
+// C L A A SSS SSS V V A A RRRR I A A BBBB L EEE SSS
+// C L AAAAA S S V V AAAAA R R I AAAAA B B L E S
+// CCCC LLLLL A A SSSS SSSS V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ---------------------------------------------------------------------------------------------------
+//DECLARE YOUR CLASS VARIABLES BELOW HERE
+
+
+
+// VARIABLES FOR VALUES LOADED FROM THE YAML FILE
+// > the proportional gain for z control
+float yaml_kp_z = 1.0
+
+// > the derivative gain for z control
+float yaml_kd_z = 1.0
+
+// > the integral gain for z control
+float yaml_ki_z = 1.0
@@ -293,12 +313,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
- // > NOTE: the function "computeMotorPolyBackward" converts the input argument
+ // > NOTE: the function "newtons2cmd_for_crazyflie" 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);
+ response.controlOutput.motorCmd1 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd2 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd3 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd4 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
@@ -498,46 +518,6 @@ void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float y
-// ------------------------------------------------------------------------------
-// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
-// NN N E W W T O O NN N C MM MM D D
-// N N N EEE W W T O O N N N --> C M M M D D
-// N NN E W W W T O O N NN C M M D D
-// N N EEEEE W W T OOO N N CCCC M M DDDD
-//
-// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
-// C O O NN N V V E R R S I O O NN N
-// C O O N N N V V EEE RRRR SSS I O O N N N
-// C O O N NN V V E R R S I O O N NN
-// CCCC OOO N N V EEEEE R R SSSS III OOO N N
-// ----------------------------------------------------------------------------------
-
-// This function DOES NEED TO BE edited for successful completion of
-// the exercise
-float computeMotorPolyBackward(float thrust)
-{
- // Compute the 16-bit command that would produce the requested
- // "thrust" based on the quadratic mapping that is described
- // by the coefficients in the "yaml_motorPoly" variable.
- float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
-
-
-
- // > Could this formula compute a result "cmd_16bit" that is
- // greater than ((2^16)-1)?
- // > What might happen when such a number is case as a 16-bit
- // integer? In other words, what is uint16(cmd_sixteen_bit)?
-
-
-
- // Return the result
- return cmd_16bit;
-}
-
-
-
-
-
// ----------------------------------------------------------------------------------
// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
// NN N E W W S E T P P O O I NN N T
@@ -626,6 +606,25 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
return true;
}
+// --------------------------------------------------------------------------------
+// DDDD EEEEE BBBB U U GGGG V V A L U U EEEEE SSSS
+// D D E B B U U G V V A A L U U E S
+// D D EEE BBBB U U G GG V V A A L U U EEE SSS
+// D D E B B U U G G V V AAAAA L U U E S
+// DDDD EEEEE BBBB UUU GGG V A A LLLLL UUU EEEEE SSSS
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// --------------------------------------------------------------------------------
+
+bool getDebugValuesCallback(GetDebugValuesService::Request &request, GetDebugValuesService::Response &response)
+{
+ // What should I write in this function? Or do the students fill it in depending on what they want to debug?
+}
+
@@ -816,6 +815,15 @@ void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle)
// GET THE PARAMETERS:
+ // > the proportional gain for z control
+ yaml_kp_z = getParameterFloat(nodeHandle_for_paramaters , "proportionalgain_for_z");
+
+ // > the derivative gain for z control
+ yaml_kd_z = getParameterFloat(nodeHandle_for_paramaters , "derivativegain_for_z");
+
+ // > the proportional gain for z control
+ yaml_ki_z = getParameterFloat(nodeHandle_for_paramaters , "integralgain_for_z");
+
// > The mass of the crazyflie
yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
@@ -1066,6 +1074,16 @@ int main(int argc, char* argv[]) {
// And now we can instantiate the subscriber:
ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
+ // Instantiate the local variable "getDebugValuesService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetDebugValues". This service has the input-output
+ // behaviour defined in the "GetDebugValuesService.srv" file (located
+ // in the "srv" folder). When a request is made
+ // of this service the "getDebugValuesCallback" function is called.
+ ros::ServiceServer getDebugValuesService = nodeHandle.advertiseService("GetDebugValues", getDebugValuesCallback);
+
// Print out some information to the user.