Pidcontroller wheelbot (#1022)

* Implementing PIDController to Wheelbot

* Implement PIDController to Wheelbot

trick_sims/SIM_wheelbot/S_define

@@ -2,7 +2,8 @@
 PURPOSE:
     ( Simulate a skydiver jump from very high altitude. )
 LIBRARY DEPENDENCIES:
-    ((Vehicle/src/vehicleOne.cpp))
+    ((Vehicle/src/vehicleOne.cpp)
+    (Control/src/PIDController.cpp))
 *************************************************************/
 
 #include "sim_objects/default_trick_sys.sm"

trick_sims/SIM_wheelbot/models/Control/include/PIDController.hh

@@ -0,0 +1,27 @@
+/*****************************************************************************
+PURPOSE:    ( PID Class H File )
+*****************************************************************************/
+
+#ifndef PIDController_HH
+#define PIDController_HH
+
+class PIDController {
+public:
+  double Kprop;
+  double Kderv;
+  double Kinteg;
+  double Dt;
+  double k;
+  double error;
+  double integral;
+  double out_max;
+  double out_min;
+  double previous_error;
+  double prev_setpoint_value;
+  bool integration_enabled;
+
+  PIDController(double kp, double ki, double kd, double omax, double omin, double dt, double tc);
+  double getOutput(double setpoint_value, double measured_value);
+};
+
+#endif

trick_sims/SIM_wheelbot/models/Control/include/differentialDriveController.hh

@@ -5,6 +5,7 @@
 #define DIFFERENTIAL_DRIVE_CONTROLER_HH
 
 #include "Motor/include/motorSpeedController.hh"
+#include "Control/include/PIDController.hh"
 
 class DifferentialDriveController {
 
@@ -40,5 +41,9 @@ class DifferentialDriveController {
         double leftMotorSpeedCommand;
         double desiredHeadingRate;
         double desiredRangeRate;
+
+        // PID Controller
+        PIDController headingctrl;
+        PIDController wheelspeedctrl;
 };
 #endif

trick_sims/SIM_wheelbot/models/Control/src/PIDController.cpp

@@ -0,0 +1,77 @@
+#include "Control/include/PIDController.hh"
+
+PIDController::PIDController(double kp, double ki, double kd, double omax, double omin, double dt, double tc) {
+  Kprop = kp;
+  Kinteg = ki;
+  Kderv = kd;
+  Dt = dt;
+  k = dt/tc;
+  error = 0.0;
+  integral = 0;
+  out_max = omax;
+  out_min = omin;
+  previous_error = 0.0;
+  prev_setpoint_value = 0.0;
+  integration_enabled = true;
+}
+
+double PIDController::getOutput(double setpoint_value, double measured_value) {
+
+  double error_unfiltered = setpoint_value - measured_value;
+
+  // Low Pass Filter
+  error = error + (k * (error_unfiltered - error));
+
+  // Check for drastic changes
+  if (prev_setpoint_value != setpoint_value) {
+    previous_error = error;
+  }
+
+  // integration
+  if (integration_enabled == true) {
+    integral = integral + (error * Dt);
+  }
+
+  // Update derivative (rate of change of error)
+  double derivative = ((error - previous_error) / Dt);
+
+  double proportionalterm = Kprop * error;
+  double integralterm = Kinteg * integral;
+  double derivateterm = Kderv * derivative;
+  double output = proportionalterm + integralterm + derivateterm;
+
+  // Clamping Actuator
+
+  // Sign Check
+  bool same_sign;
+  if ((error * output) > 0.0) {
+    same_sign = 1;
+  } else {
+    same_sign = 0;
+  }
+
+  //Saturation Check
+  bool output_limited;
+  if (output > out_max) {
+    output = out_max;
+    output_limited = 1;
+  }
+  if (output < out_min) {
+    output = out_min;
+    output_limited = 1;
+  }
+
+  //AND Gate Check
+  if (output_limited && same_sign) {
+    integration_enabled = false;
+  } else {
+    integration_enabled = true;
+  }
+
+  // Prepare for next calcuation
+  previous_error = error;
+  prev_setpoint_value = setpoint_value;
+
+  return (output);
+
+}

trick_sims/SIM_wheelbot/models/Control/src/differentialDriveController.cpp

@@ -23,7 +23,14 @@ DifferentialDriveController::
     leftMotorController(LeftMotorController),
 
     rightMotorSpeedCommand(0.0),
-    leftMotorSpeedCommand(0.0)
+    leftMotorSpeedCommand(0.0),
+    desiredHeadingRate(0.0),
+
+    // PID Controller Initialization
+    headingctrl(PIDController(1.0, 0.08, 0.5, headingRateLimit,
+                                  -headingRateLimit, 0.1, 0.1)),
+    wheelspeedctrl(PIDController(1.0, 0.082, 0.5, wheelSpeedLimit,
+                                      -wheelSpeedLimit, 0.1, 0.1))
 { }
 
 void DifferentialDriveController::stop() {
@@ -40,7 +47,8 @@ int DifferentialDriveController::update( double distance_err,  // m
     // If the vehicle heading is within 2 degrees of the target, then the
     // heading desired heading rate is proportional to the heading error.
     if ( cos(heading_err) > cos(2.0 * (PI/180.0))) {
-        desiredHeadingRate =  heading_err/(2.0*(PI/180.0)) * headingRateLimit;
+        // PID Controller output for heading
+        desiredHeadingRate = headingctrl.getOutput(headingRateLimit, heading_err);
     } else {
         if (heading_err > 0.0) {
             desiredHeadingRate =  headingRateLimit;
@@ -58,7 +66,8 @@ int DifferentialDriveController::update( double distance_err,  // m
     if (distance_err > slowDownDistance ) {
         wheelSpeedForRangeRate = availableWheelSpeedForRangeRate;
     } else {
-        wheelSpeedForRangeRate = (distance_err/slowDownDistance) * availableWheelSpeedForRangeRate;
+        // PID Controller output for wheelspeed
+        wheelSpeedForRangeRate = wheelspeedctrl.getOutput(availableWheelSpeedForRangeRate, distance_err);
     }
 
     desiredRangeRate = wheelSpeedForRangeRate * wheelRadius;