pid for rat no turns
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 20:37:57 by
1.7.2