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   int PrevErr;                   // last error
00014   int Ierror;                    // integrated error
00015   int output;                    // last motor setting
00016 }
00017 SetPointInfo;
00018 
00019 SetPointInfo leftPID, rightPID;
00020 
00021 /* PID Parameters */
00022 int Kp = 20;
00023 int Kd = 12;
00024 int Ki = 0;
00025 int Ko = 50;
00026 
00027 unsigned char moving = 0; // is the base in motion?
00028 
00029 /* PID routine to compute the next motor commands */
00030 void doPID(SetPointInfo * p) {
00031   long Perror;
00032   long output;
00033 
00034   Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
00035 
00036   // Derivative error is the delta Perror
00037   output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
00038   p->PrevErr = Perror;
00039   p->PrevEnc = p->Encoder;
00040 
00041   output += p->output;
00042   // Accumulate Integral error *or* Limit output.
00043   // Stop accumulating when output saturates
00044   if (output >= MAX_PWM)
00045     output = MAX_PWM;
00046   else if (output <= -MAX_PWM)
00047     output = -MAX_PWM;
00048   else
00049     p->Ierror += Perror;
00050 
00051   p->output = output;
00052 }
00053 
00054 /* Read the encoder values and call the PID routine */
00055 void updatePID() {
00056   /* Read the encoders */
00057   leftPID.Encoder = readEncoder(0);
00058   rightPID.Encoder = readEncoder(1);
00059   
00060   /* If we're not moving there is nothing more to do */
00061   if (!moving)
00062     return;
00063 
00064   /* Compute PID update for each motor */
00065   doPID(&rightPID);
00066   doPID(&leftPID);
00067 
00068   /* Set the motor speeds accordingly */
00069   setMotorSpeeds(leftPID.output, rightPID.output);
00070 }
00071 


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