diff --git a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
index 04cc2b2d890a58871fd500a0172774db4ff052b3..400aa725d77dcba6ccdde758d216659853a489a2 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
@@ -185,11 +185,21 @@ float m_picker_string_length;
// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.4,0.0};
+float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
float m_yAdjustment = 0.0f;
+// Boolean for whether to limit rate of change of the setpoint
+bool m_shouldSmoothSetpointChanges = true;
+
+// Max setpoint change per second
+float m_max_setpoint_change_per_second_horizontal;
+float m_max_setpoint_change_per_second_vertical;
+float m_max_setpoint_change_per_second_yaw_degrees;
+float m_max_setpoint_change_per_second_yaw_radians;
+
@@ -275,10 +285,6 @@ ros::Publisher debugPublisher;
// Boolean whether to execute the convert into body frame function
bool shouldPerformConvertIntoBodyFrame = false;
-// Boolean for whether to clip the current position to setpoint distance
-bool shouldSmoothVerticalSetpointChanges = false;
-bool shouldSmoothHorizonatalSetpointChanges = false;
-
// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
@@ -348,6 +354,9 @@ ros::Publisher pickerSetpointToGUIPublisher;
// ADDED FOR THE PICKER
+void perControlCycleOperations();
+
+// CALLBACK FROM ROS MESSAGES RECEIVED
void buttonPressedCallback(const std_msgs::Int32& msg);
void zSetpointCallback(const std_msgs::Float32& msg);
void yawSetpointCallback(const std_msgs::Float32& msg);
diff --git a/pps_ws/src/d_fall_pps/param/PickerController.yaml b/pps_ws/src/d_fall_pps/param/PickerController.yaml
index e20851cd0fe96b6acace8f0eb510f4909cf009e2..18b2ed57e6503a5bc0e965149ce28fde300be92c 100644
--- a/pps_ws/src/d_fall_pps/param/PickerController.yaml
+++ b/pps_ws/src/d_fall_pps/param/PickerController.yaml
@@ -17,6 +17,12 @@ dropoff_coordinates_xy_for_H : [0.28, 0.0]
# to the end of the Picker, in [meters]
picker_string_length : 0.38
+# Max setpoint change per second
+max_setpoint_change_per_second_horizontal : 0.20 # [meters]
+max_setpoint_change_per_second_vertical : 0.10 # [meters]
+max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
+
+
@@ -33,10 +39,6 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
# Boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame : true
-# Boolean for whether to clip the current position to setpoint distance
-shouldSmoothVerticalSetpointChanges : true
-shouldSmoothHorizontalSetpointChanges : true
-
# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
shouldPublishCurrent_xyz_yaw : true
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
index de0de83d528f2ab3583978bda4973e09679b2beb..4205f208c1bb6033203803a53ff93d0086d47fce 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
@@ -83,15 +83,75 @@
+void perControlCycleOperations()
+{
+ if (m_shouldSmoothSetpointChanges)
+ {
+ for(int i = 0; i < 4; ++i)
+ {
+ float max_for_this_coordinate;
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (i)
+ {
+ case 0:
+ max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / vicon_frequency;
+ break;
+ case 1:
+ max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / vicon_frequency;
+ break;
+ case 2:
+ max_for_this_coordinate = m_max_setpoint_change_per_second_vertical / vicon_frequency;
+ break;
+ case 3:
+ max_for_this_coordinate = m_max_setpoint_change_per_second_yaw_radians / vicon_frequency;
+ break;
+ // Handle the exception
+ default:
+ max_for_this_coordinate = 0.0f;
+ break;
+ }
+
+ // Compute the difference in setpoint
+ float setpoint_difference = m_setpoint[i] - m_setpoint_for_controller[i];
+
+ // Clip the difference to the maximum
+ if (setpoint_difference > max_for_this_coordinate)
+ {
+ setpoint_difference = max_for_this_coordinate;
+ }
+ else if (setpoint_difference < -max_for_this_coordinate)
+ {
+ setpoint_difference = -max_for_this_coordinate;
+ }
+
+ // Update the setpoint of the controller
+ m_setpoint_for_controller[i] += setpoint_difference;
+ }
+
+ }
+ else
+ {
+ m_setpoint_for_controller[0] = m_setpoint[0];
+ m_setpoint_for_controller[1] = m_setpoint[1];
+ m_setpoint_for_controller[2] = m_setpoint[2];
+ m_setpoint_for_controller[3] = m_setpoint[3];
+ }
+}
+
+
+
+
+
+
void buttonPressed_gotoStart()
{
ROS_INFO("[PICKER CONTROLLER] Goto Start button pressed");
- shouldSmoothVerticalSetpointChanges = true;
+ m_shouldSmoothSetpointChanges = true;
m_setpoint[0] = m_pickup_coordinates_xy[0];
m_setpoint[1] = m_pickup_coordinates_xy[1];
- m_setpoint[2] = m_picker_string_length + 0.10;
+ m_setpoint[2] = m_picker_string_length + 0.15;
publishCurrentSetpoint();
}
@@ -99,7 +159,7 @@ void buttonPressed_connect()
{
ROS_INFO("[PICKER CONTROLLER] Connect button pressed");
- m_setpoint[2] = m_picker_string_length + 0.01;
+ m_setpoint[2] = m_picker_string_length + 0.02;
publishCurrentSetpoint();
}
@@ -108,7 +168,7 @@ void buttonPressed_pickup()
ROS_INFO("[PICKER CONTROLLER] Pick up button pressed");
m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
- m_setpoint[2] = m_picker_string_length + 0.15;
+ m_setpoint[2] = m_picker_string_length + 0.25;
publishCurrentSetpoint();
}
@@ -125,7 +185,7 @@ void buttonPressed_putdown()
{
ROS_INFO("[PICKER CONTROLLER] Put down button pressed");
- m_setpoint[2] = m_picker_string_length - 0.10;
+ m_setpoint[2] = m_picker_string_length + 0.02;
m_mass_total_grams = m_mass_CF_grams;
publishCurrentSetpoint();
}
@@ -134,7 +194,7 @@ void buttonPressed_disconnect()
{
ROS_INFO("[PICKER CONTROLLER] Disconnect button pressed");
- shouldSmoothVerticalSetpointChanges = false;
+ m_shouldSmoothSetpointChanges = false;
m_setpoint[2] = m_picker_string_length + 0.15;
m_mass_total_grams = m_mass_CF_grams;
publishCurrentSetpoint();
@@ -344,6 +404,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_time_ticks++;
m_time_seconds = float(m_time_ticks) / vicon_frequency;
+
+ // CALL THE FUNCTION FOR PER CYLCE OPERATIONS
+ perControlCycleOperations();
+
// THIS IS THE START OF THE "OUTER" CONTROL LOOP
// > i.e., this is the control loop run on this laptop
// > this function is called at the frequency specified
@@ -362,7 +426,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > Define a local array to fill in with the body frame error
float current_bodyFrameError[12];
// > Call the function to perform the conversion
- convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,m_setpoint,current_bodyFrameError);
+ convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,m_setpoint_for_controller,current_bodyFrameError);
@@ -752,16 +816,13 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
stateInertial[1] = stateInertial[1] - setpoint[1] - m_yAdjustment;
stateInertial[2] = stateInertial[2] - setpoint[2];
- if (shouldSmoothVerticalSetpointChanges)
+ if (stateInertial[2] > 30.0f)
{
- if (stateInertial[2] > 10.0f)
- {
- stateInertial[2] = 10.0f;
- }
- else if (stateInertial[2] < -10.0f)
- {
- stateInertial[2] = 10.0f;
- }
+ stateInertial[2] = 30.0f;
+ }
+ else if (stateInertial[2] < -30.0f)
+ {
+ stateInertial[2] = 30.0f;
}
// Fill in the yaw angle error
@@ -1048,6 +1109,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Length of the string from the Crazyflie to the end of the Picker, in [meters]
m_picker_string_length = getParameterFloat(nodeHandle_for_pickerController , "picker_string_length");
+ // Max setpoint change per second
+ m_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_horizontal");
+ m_max_setpoint_change_per_second_vertical = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_vertical");
+ m_max_setpoint_change_per_second_yaw_degrees = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_yaw_degrees");
// THE FOLLOWING PARAMETERS ARE USED FOR THE LOW-LEVEL CONTROLLER
@@ -1066,10 +1131,6 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_pickerController, "shouldPerformConvertIntoBodyFrame");
- // Boolean for whether to clip the current position to setpoint distance
- shouldSmoothVerticalSetpointChanges = getParameterBool(nodeHandle_for_pickerController, "shouldSmoothVerticalSetpointChanges");
- shouldSmoothHorizonatalSetpointChanges = getParameterBool(nodeHandle_for_pickerController, "shouldSmoothHorizonatalSetpointChanges");
-
// > The boolean indicating whether the (x,y,z,yaw) of this agent should be published
// or not
shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_pickerController, "shouldPublishCurrent_xyz_yaw");
@@ -1123,6 +1184,9 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// your controller easier and quicker.
void processFetchedParameters()
{
+ // Convert from degrees to radians
+ m_max_setpoint_change_per_second_yaw_radians = DEG2RAD * m_max_setpoint_change_per_second_yaw_degrees;
+
// Set that the estimator frequency is the same as the control frequency
estimator_frequency = vicon_frequency;
}