Prepared custom controller for class, needs testing

pps_ws/src/d_fall_pps/param/CustomController.yaml

@@ -5,42 +5,4 @@ mass : 31
 control_frequency : 200
 
 # Quadratic motor regression equation (a0, a1, a2)
-motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
-
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
\ No newline at end of file
-# Boolean for whether to execute the convert into body frame function
-shouldPerformConvertIntoBodyFrame : true
-
-# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
-shouldPublishCurrent_xyz_yaw : false
-
-# Boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
-# an attempt to follow the "my_current_xyz_yaw_topic" from another agent
-shouldFollowAnotherAgent : false
-
-# The order in which agents should follow eachother
-# > This parameter is a vector of integers that specifies  agent ID's
-# > The order of the agent ID's is the ordering of the line formation
-# > i.e., the first ID is the leader, the second ID should follow the first ID, and so on
-follow_in_a_line_agentIDs : [1, 2, 3]
-
-# Boolean indiciating whether the "Debug Message" of this agent should be published or not
-shouldPublishDebugMessage : false
-
-# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
-shouldDisplayDebugInfo : false
-
-# A flag for which controller to use, defined as:
-# 1  -  LQR controller based on the state vector: [position,velocity,angles]
-# 2  -  LQR controller based on the state vector: [position,velocity]
-controller_type : 1
-
-# The LQR Controller parameters for "mode = 1"
-gainMatrixRollRate                :  [ 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00]
-gainMatrixPitchRate               :  [ 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00]
-gainMatrixYawRate                 :  [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84]
-gainMatrixThrust_NineStateVector  :  [ 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00]
-
-# The LQR Controller parameters for "mode = 2"
-gainMatrixRollAngle               :  [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
-gainMatrixPitchAngle              :  [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
-gainMatrixThrust_SixStateVector   :  [ 0.00, 0.00, 0.31, 0.00, 0.00, 0.14]
\ No newline at end of file