Merge pull request #6 from KristofRobot/enhancedPID

Enhanced PID algorithm

ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino

@@ -197,6 +197,7 @@ int runCommand() {
     break;
    case RESET_ENCODERS:
     resetEncoders();
+    resetPID();
     Serial.println("OK");
     break;
   case MOTOR_SPEEDS:
@@ -236,6 +237,7 @@ void setup() {
 // Initialize the motor controller if used */
 #ifdef USE_BASE
   initMotorController();
+  resetPID();
 #endif
 
 /* Attach servos if used */

ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h

@@ -10,9 +10,23 @@ typedef struct {
   double TargetTicksPerFrame;    // target speed in ticks per frame
   long Encoder;                  // encoder count
   long PrevEnc;                  // last encoder count
-  int PrevErr;                   // last error
-  int Ierror;                    // integrated error
-  int output;                    // last motor setting
+
+  /*
+  * Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
+  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+  */
+  int PrevInput;                // last input
+  //int PrevErr;                   // last error
+
+  /*
+  * Using integrated term (ITerm) instead of integrated error (Ierror),
+  * to allow tuning changes,
+  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+  */
+  //int Ierror;
+  int ITerm;                    //integrated term
+
+  long output;                    // last motor setting
 }
 SetPointInfo;
 
@@ -26,16 +40,49 @@ int Ko = 50;
 
 unsigned char moving = 0; // is the base in motion?
 
+/*
+* Initialize PID variables to zero to prevent startup spikes
+* when turning PID on to start moving
+* In particular, assign both Encoder and PrevEnc the current encoder value
+* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
+* Note that the assumption here is that PID is only turned on
+* when going from stop to moving, that's why we can init everything on zero.
+*/
+void resetPID(){
+   leftPID.TargetTicksPerFrame = 0.0;
+   leftPID.Encoder = readEncoder(0);
+   leftPID.PrevEnc = leftPID.Encoder;
+   leftPID.output = 0;
+   leftPID.PrevInput = 0;
+   leftPID.ITerm = 0;
+
+   rightPID.TargetTicksPerFrame = 0.0;
+   rightPID.Encoder = readEncoder(1);
+   rightPID.PrevEnc = rightPID.Encoder;
+   rightPID.output = 0;
+   rightPID.PrevInput = 0;
+   rightPID.ITerm = 0;
+}
+
 /* PID routine to compute the next motor commands */
 void doPID(SetPointInfo * p) {
   long Perror;
   long output;
+  int input;
 
-  Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
+  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
+  input = p->Encoder - p->PrevEnc;
+  Perror = p->TargetTicksPerFrame - input;
 
-  // Derivative error is the delta Perror
-  output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
-  p->PrevErr = Perror;
+
+  /*
+  * Avoid derivative kick and allow tuning changes,
+  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+  */
+  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
+  // p->PrevErr = Perror;
+  output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;
   p->PrevEnc = p->Encoder;
 
   output += p->output;
@@ -46,9 +93,13 @@ void doPID(SetPointInfo * p) {
   else if (output <= -MAX_PWM)
     output = -MAX_PWM;
   else
-    p->Ierror += Perror;
+  /*
+  * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+  */
+    p->ITerm += Ki * Perror;
 
   p->output = output;
+  p->PrevInput = input;
 }
 
 /* Read the encoder values and call the PID routine */
@@ -58,8 +109,16 @@ void updatePID() {
   rightPID.Encoder = readEncoder(1);
   
   /* If we're not moving there is nothing more to do */
-  if (!moving)
+  if (!moving){
+    /*
+    * Reset PIDs once, to prevent startup spikes,
+    * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
+    * PrevInput is considered a good proxy to detect
+    * whether reset has already happened
+    */
+    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
     return;
+  }
 
   /* Compute PID update for each motor */
   doPID(&rightPID);