pid for rat no turns

Dependencies:   QEI mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003  
00004 Serial pc(SERIAL_TX, SERIAL_RX); 
00005  
00006 PwmOut lpwmf(PA_7);
00007 PwmOut lpwmb(PB_6);
00008  
00009 PwmOut rpwmf(PB_10);
00010 PwmOut rpwmb(PC_7);
00011  
00012  
00013 PinName rfront(PB_3);
00014 PinName rback(PA_15);
00015 //DigitalIn lfront(PB_3);
00016 //DigitalIn lback(PA_15);
00017 //Right Encoder
00018 PinName lfront(PA_1);
00019 PinName lback(PC_4);
00020  
00021 //QEI::Encoding encoding = QEI::Encoding[1];
00022  
00023 QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
00024 QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
00025  
00026 const float rbase = 0.135f;
00027 const float lbase = 0.12f;
00028  
00029 Timer t_time;
00030  
00031 struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
00032   pid(){
00033         integral = 0.0f;
00034         prev = 0.0f;
00035         kp = .01f; //the ks should be negative to counteract error
00036         ki = 0.0f;
00037         kd = 0.8f;
00038       }
00039   float integral;
00040   int prev;
00041   float kp; //the ks should be negative to counteract error
00042   float ki;
00043   float kd;
00044 };
00045  
00046 void resetpid(struct pid *e) {
00047   e->integral = 0.0f;
00048   e->prev = 0.0f;
00049 }
00050  
00051 float getFix(struct pid *e, float error, float time) {
00052   float d = (error - e->prev)/time;
00053   e->integral += error * time;
00054   e->prev = error;
00055   float temp = (e->kp * error + e->ki * e->integral + e->kd * d);
00056   return temp;
00057 
00058 }
00059  
00060 float constrain(float l_limit, float u_limit, float val){
00061     if (val < l_limit){
00062         return l_limit;
00063         }
00064     else if(val > u_limit){
00065         return u_limit;
00066         }    
00067     return val;    
00068     }
00069  
00070 int main() {
00071     pid lman, rman;
00072     lman.kp = .0040f;
00073     lman.ki = .0002f;
00074     lman.kd = .0015f;
00075     rman.kp = .5f;
00076     rman.ki = .1f;
00077     rman.kd = .4f;
00078     
00079     
00080     lpwmf.period(0.01f);
00081     lpwmf = lbase;
00082     rpwmf.period(0.01f);
00083     rpwmf = rbase;
00084     t_time.start();
00085     
00086     while(1){
00087         float dt = t_time.read();
00088         t_time.reset();
00089         float error = (LeftEncoder.getPulses() - RightEncoder.getPulses())/16.0f; //Can be pos or neg, Range from 0 - 255 for one revolution
00090 
00091 //        pc.printf("no %d \n", RightEncoder.getPulses());
00092 //        pc.printf("yas %d \n", LeftEncoder.getPulses());
00093 
00094 
00095         if(error == 0.0f){
00096             resetpid(&lman);
00097             resetpid(&rman);
00098         }
00099         float adjust_l = getFix(&lman, error, dt); 
00100         pc.printf("%f \n", adjust_l);
00101         
00102         float lspeed = constrain(0.0f, 0.4f, lbase - adjust_l);
00103         float rspeed = rbase;
00104         
00105         pc.printf("no %f \n", rspeed);
00106         pc.printf("yas %f \n", lspeed);
00107         
00108         lpwmf = lspeed;
00109         rpwmf = rspeed;
00110         
00111         wait(0.2f);
00112         
00113         }
00114         t_time.stop();
00115     }