Enhanced PID algorithm

Enhanced PID algorithm based on the series of blog posts at http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
Specifically:
    1. Avoid derivative kickback - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
    2. Allow smooth on-the-fly tuning changes - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
    3. Reset windup mitigation - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-reset-windup/
    4. Bumpless initialization - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/

ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino

@@ -236,6 +236,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,8 +10,22 @@ 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
+  /*
+  * 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/
+  */
+  long 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;
+  long ITerm;                    //integrated term
+
   int output;                    // last motor setting
 }
 SetPointInfo;
@@ -26,16 +40,56 @@ 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;
+  long 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;
+  * Avoid reset windup,
-  p->PrevErr = Perror;
+  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-reset-windup/
+  */
+  p->ITerm += (Ki * Perror);
+  if (p->ITerm > MAX_PWM) p->ITerm = MAX_PWM;
+  else if (p->ITerm < -MAX_PWM) p->ITerm = MAX_PWM;
+
+  /*
+  * 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;
@@ -45,10 +99,12 @@ void doPID(SetPointInfo * p) {
     output = MAX_PWM;
   else if (output <= -MAX_PWM)
     output = -MAX_PWM;
-  else
+  //else
-    p->Ierror += Perror;
+  //  p->Ierror += Perror;
+
 
   p->output = output;
+  p->PrevInput = input;
 }
 
 /* Read the encoder values and call the PID routine */
@@ -58,8 +114,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);