Merge pull request #38 from BCLab-UNM/PID_tuning

Pid tuned and tested by @jschlind411 and @JCHJones.

Author: gmfricke

src/abridge/src/abridge.cpp

@@ -170,7 +170,7 @@ void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message) {
     // Assumes left and right are always between -1 and 1
     float linear = left * max_motor_cmd; 
     float angular = right * max_motor_cmd; 
-	  
+
     left = linear - angular;
     right = linear + angular;
   }

src/behaviours/src/DriveController.cpp

@@ -306,9 +306,9 @@ PIDConfig DriveController::fastVelConfig() {
 PIDConfig DriveController::fastYawConfig() {
   PIDConfig config;
 
-  config.Kp = 240;
-  config.Ki = 25;
-  config.Kd = 0.5;
+  config.Kp = 100;
+  config.Ki = 10;
+  config.Kd = 14;
   config.satUpper = 255;
   config.satLower = -255;
   config.antiWindup = config.satUpper/2;
@@ -350,9 +350,9 @@ PIDConfig DriveController::slowVelConfig() {
 PIDConfig DriveController::slowYawConfig() {
   PIDConfig config;
 
-  config.Kp = 80;
-  config.Ki = 15;
-  config.Kd = 1.7;
+  config.Kp = 70;
+  config.Ki = 16;
+  config.Kd = 10;
   config.satUpper = 255;
   config.satLower = -255;
   config.antiWindup = config.satUpper/4;

src/behaviours/src/PID.cpp

@@ -105,20 +105,24 @@ float PID::PIDOut(float calculatedError, float setPoint) {
   if (fabs(P) < config.antiWindup)
   {
     float avgPrevError = 0;
-    for (int i = 1; i < Error.size(); i++) {
+    for (int i = 1; i < Error.size(); i++)
+    {
       avgPrevError += Error[i];
     }
-    if (Error.size() > 1) {
+    if (Error.size() > 1)
+    {
       avgPrevError /= Error.size()-1;
     }
-    else {
+    else
+    {
       avgPrevError = Error[0];
     }
 
-    D = config.Kd * (((1-config.derivativeAlpha)*Error[0]) - (config.derivativeAlpha * avgPrevError));
+    D = config.Kd * (Error[0] - Error[1]) * hz;
   }
 
   float PIDOut = P + I + D + FF;
+
   if (PIDOut > config.satUpper) //cap vel command
   {
     PIDOut = config.satUpper;

src/behaviours/src/PID.h

@@ -44,7 +44,7 @@ class PID
   float prevSetPoint = std::numeric_limits<float>::min();
   vector<float> integralErrorHistArray;
   int step = 0;
-  float hz = 10;
+  float hz = 10;	//Rate that PID is running at
 
 };
 

src/behaviours/src/SearchController.cpp

@@ -1,4 +1,5 @@
 #include "SearchController.h"
+#include <angles/angles.h>
 
 SearchController::SearchController() {
   rng = new random_numbers::RandomNumberGenerator();
@@ -38,7 +39,8 @@ Result SearchController::DoWork() {
     }
     return result;
   }
-  else if (attemptCount >= 5 || attemptCount == 0) {
+  else if (attemptCount >= 5 || attemptCount == 0) 
+  {
     attemptCount = 1;
 
 
@@ -63,7 +65,7 @@ Result SearchController::DoWork() {
 
     result.wpts.waypoints.clear();
     result.wpts.waypoints.insert(result.wpts.waypoints.begin(), searchLocation);
-
+    
     return result;
   }