From dad031ee2c0c70ec6634d899587cac83c00cc6f6 Mon Sep 17 00:00:00 2001 From: Kristof Date: Fri, 16 Aug 2013 10:52:57 +0200 Subject: [PATCH 1/3] 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/ --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 1 + .../ROSArduinoBridge/diff_controller.h | 82 +++++++++++++++++-- 2 files changed, 74 insertions(+), 9 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 14ffbe7..dd9a8f8 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/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 */ diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h index 0284681..bbb6e58 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h +++ b/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; - p->PrevErr = Perror; + /* + * Avoid reset windup, + * 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 - p->Ierror += Perror; + //else + // 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); From 2c9881552bc939fc4231328ebc148ebbc2530ba1 Mon Sep 17 00:00:00 2001 From: Kristof Robot Date: Fri, 16 Aug 2013 15:04:33 +0200 Subject: [PATCH 2/3] Bugfix: added resetPID() call in RESET_ENCODERS statement, to ensure that PID gets reset when encoders get reset. --- .../src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index dd9a8f8..77322d2 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/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: From 7887d639f3d65f59f13de942b710db70aad19092 Mon Sep 17 00:00:00 2001 From: Kristof Robot Date: Fri, 16 Aug 2013 16:16:48 +0200 Subject: [PATCH 3/3] Bugfix - removed reset wind-up fix Removed the reset windup prevention code, as the old code already took care of that by only incrementing Ierror when output was not being clamped. --- .../ROSArduinoBridge/diff_controller.h | 27 ++++++++----------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h index bbb6e58..6bb1002 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h @@ -15,7 +15,7 @@ typedef struct { * 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 PrevInput; // last input //int PrevErr; // last error /* @@ -24,9 +24,9 @@ typedef struct { * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ //int Ierror; - long ITerm; //integrated term + int ITerm; //integrated term - int output; // last motor setting + long output; // last motor setting } SetPointInfo; @@ -45,8 +45,8 @@ unsigned char moving = 0; // is the base in motion? * 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. +* 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; @@ -68,19 +68,12 @@ void resetPID(){ void doPID(SetPointInfo * p) { long Perror; long output; - long input; + int input; //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); input = p->Encoder - p->PrevEnc; Perror = p->TargetTicksPerFrame - input; - /* - * Avoid reset windup, - * 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, @@ -99,9 +92,11 @@ void doPID(SetPointInfo * p) { output = MAX_PWM; else if (output <= -MAX_PWM) output = -MAX_PWM; - //else - // p->Ierror += Perror; - + else + /* + * 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;