diff_controller.h
Go to the documentation of this file.
00001 /* 
00002   ArbotiX Firmware for ROS driver
00003   Copyright (c) 2009-2011 Vanadium Labs LLC.  All right reserved.
00004  
00005   Redistribution and use in source and binary forms, with or without
00006   modification, are permitted provided that the following conditions are met:
00007 
00008       * Redistributions of source code must retain the above copyright
00009         notice, this list of conditions and the following disclaimer.
00010       * Redistributions in binary form must reproduce the above copyright
00011         notice, this list of conditions and the following disclaimer in the
00012         documentation and/or other materials provided with the distribution.
00013       * Neither the name of Vanadium Labs LLC nor the names of its 
00014         contributors may be used to endorse or promote products derived 
00015         from this software without specific prior written permission.
00016   
00017   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018   ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019   WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020   DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00021   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023   OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025   OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026   ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 */ 
00028 
00029 #include <Arduino.h>
00030 #include <math.h>
00031 
00032 /* Register Storage */
00033 int left_pwm;
00034 int right_pwm;
00035 int left_speed;
00036 int right_speed;
00037 
00038 /* PID Parameters */
00039 int Kp;
00040 int Kd;
00041 int Ki;
00042 int Ko;                      // output scale
00043 int maxAccel;                // maximum acceleration per frame (ticks)
00044 
00045 /* PID modes */
00046 unsigned int PIDmode;
00047 #define PID_NONE        0
00048 #define PID_SPEED       1
00049 
00050 #define FRAME_RATE      33   // frame rate in millis (30Hz)
00051 #define FRAMES          30
00052 unsigned long f_time;        // last frame
00053 
00054 unsigned char moving = 0;    // base in motion
00055 unsigned char paused = 0;    // base was in motion, can resume
00056 #define MAXOUTPUT       255  // motor PWM
00057 
00058 /* Setpoint Info For a Motor */
00059 typedef struct{
00060   int Velocity;              // desired actual speed (count/frame)
00061   long Encoder;              // actual reading
00062   long PrevEnc;              // last reading
00063   int PrevErr;
00064   int Ierror;   
00065   int output;                // last motor setting
00066 } SetPointInfo;
00067 
00068 SetPointInfo left, right;
00069 
00070 /* Initialize PID parameters to something known */
00071 void setupPID(){
00072   // Default values for the PR-MINI
00073   Kp = 25;
00074   Kd = 30;
00075   Ki = 0;
00076   Ko = 100;
00077   maxAccel = 50;
00078   f_time = 0;
00079 }
00080 
00081 /* PID control of motor speed */
00082 void DoPid(SetPointInfo * p){
00083   long Perror;
00084   long output;
00085   
00086   Perror = p->Velocity - (p->Encoder-p->PrevEnc);
00087           
00088   // Derivative error is the delta Perror
00089   output = (Kp*Perror + Kd*(Perror - p->PrevErr) + Ki*p->Ierror)/Ko;
00090   p->PrevErr = Perror;
00091   p->PrevEnc = p->Encoder;
00092   
00093   output += p->output;   
00094   // Accumulate Integral error *or* Limit output.
00095   // Stop accumulating when output saturates
00096   if (output >= MAXOUTPUT)
00097     output = MAXOUTPUT;
00098   else if (output <= -MAXOUTPUT)
00099     output = -MAXOUTPUT;
00100   else
00101     p->Ierror += Perror;
00102   
00103   p->output = output;
00104 }
00105 
00106 /* Clear accumulators */
00107 void ClearPID(){
00108   PIDmode = 0; moving = 0;
00109   left.PrevErr = 0;
00110   left.Ierror = 0;
00111   left.output = 0;
00112   right.PrevErr = 0;
00113   right.Ierror = 0;
00114   right.output = 0;
00115 }
00116 
00117 /* This is called by the main loop, does a X HZ PID loop. */
00118 void updatePID(){
00119   if((moving > 0) && (PIDmode > 0)){  // otherwise, just return
00120     unsigned long j = millis();
00121     if(j > f_time){
00122       // update encoders
00123       left.Encoder = Encoders.left;
00124       right.Encoder = Encoders.right;
00125       // do PID update on PWM
00126       DoPid(&left);
00127       DoPid(&right);
00128       // set updated motor outputs      
00129       if(PIDmode > 0){
00130         drive.set(left.output, right.output);
00131       }
00132       // update timing
00133       f_time = j + FRAME_RATE;
00134     }
00135   }
00136 }
00137 
00138 void clearAll(){
00139   PIDmode = 0;
00140   left.Encoder = 0;
00141   right.Encoder = 0;
00142   left.PrevEnc = 0;
00143   right.PrevEnc = 0;
00144   left.output = 0;
00145   right.output = 0;
00146   Encoders.Reset();  
00147 }
00148 


arbotix_firmware
Author(s): Michael Ferguson
autogenerated on Sun Oct 5 2014 22:16:01