diff_controller.h
Go to the documentation of this file.
00001 /* Functions and type-defs for PID control.
00002 
00003    Taken mostly from Mike Ferguson's ArbotiX code which lives at:
00004    
00005    http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
00006 */
00007 
00008 /* PID setpoint info For a Motor */
00009 typedef struct {
00010   double TargetTicksPerFrame;    // target speed in ticks per frame
00011   long Encoder;                  // encoder count
00012   long PrevEnc;                  // last encoder count
00013 
00014   /*
00015   * Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
00016   * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
00017   */
00018   int PrevInput;                // last input
00019   //int PrevErr;                   // last error
00020 
00021   /*
00022   * Using integrated term (ITerm) instead of integrated error (Ierror),
00023   * to allow tuning changes,
00024   * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
00025   */
00026   //int Ierror;
00027   int ITerm;                    //integrated term
00028 
00029   long output;                    // last motor setting
00030 }
00031 SetPointInfo;
00032 
00033 SetPointInfo leftPID, rightPID;
00034 
00035 /* PID Parameters */
00036 int Kp = 20;
00037 int Kd = 12;
00038 int Ki = 0;
00039 int Ko = 50;
00040 
00041 unsigned char moving = 0; // is the base in motion?
00042 
00043 /*
00044 * Initialize PID variables to zero to prevent startup spikes
00045 * when turning PID on to start moving
00046 * In particular, assign both Encoder and PrevEnc the current encoder value
00047 * See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
00048 * Note that the assumption here is that PID is only turned on
00049 * when going from stop to moving, that's why we can init everything on zero.
00050 */
00051 void resetPID(){
00052    leftPID.TargetTicksPerFrame = 0.0;
00053    leftPID.Encoder = readEncoder(LEFT);
00054    leftPID.PrevEnc = leftPID.Encoder;
00055    leftPID.output = 0;
00056    leftPID.PrevInput = 0;
00057    leftPID.ITerm = 0;
00058 
00059    rightPID.TargetTicksPerFrame = 0.0;
00060    rightPID.Encoder = readEncoder(RIGHT);
00061    rightPID.PrevEnc = rightPID.Encoder;
00062    rightPID.output = 0;
00063    rightPID.PrevInput = 0;
00064    rightPID.ITerm = 0;
00065 }
00066 
00067 /* PID routine to compute the next motor commands */
00068 void doPID(SetPointInfo * p) {
00069   long Perror;
00070   long output;
00071   int input;
00072 
00073   //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
00074   input = p->Encoder - p->PrevEnc;
00075   Perror = p->TargetTicksPerFrame - input;
00076 
00077 
00078   /*
00079   * Avoid derivative kick and allow tuning changes,
00080   * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
00081   * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
00082   */
00083   //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
00084   // p->PrevErr = Perror;
00085   output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;
00086   p->PrevEnc = p->Encoder;
00087 
00088   output += p->output;
00089   // Accumulate Integral error *or* Limit output.
00090   // Stop accumulating when output saturates
00091   if (output >= MAX_PWM)
00092     output = MAX_PWM;
00093   else if (output <= -MAX_PWM)
00094     output = -MAX_PWM;
00095   else
00096   /*
00097   * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
00098   */
00099     p->ITerm += Ki * Perror;
00100 
00101   p->output = output;
00102   p->PrevInput = input;
00103 }
00104 
00105 /* Read the encoder values and call the PID routine */
00106 void updatePID() {
00107   /* Read the encoders */
00108   leftPID.Encoder = readEncoder(LEFT);
00109   rightPID.Encoder = readEncoder(RIGHT);
00110   
00111   /* If we're not moving there is nothing more to do */
00112   if (!moving){
00113     /*
00114     * Reset PIDs once, to prevent startup spikes,
00115     * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
00116     * PrevInput is considered a good proxy to detect
00117     * whether reset has already happened
00118     */
00119     if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
00120     return;
00121   }
00122 
00123   /* Compute PID update for each motor */
00124   doPID(&rightPID);
00125   doPID(&leftPID);
00126 
00127   /* Set the motor speeds accordingly */
00128   setMotorSpeeds(leftPID.output, rightPID.output);
00129 }
00130 


ros_arduino_firmware
Author(s): Patrick Goebel
autogenerated on Wed Aug 26 2015 15:57:06