Evan Brown / Mbed 2 deprecated Rat_motor_control

pid for rat no turns

Dependencies:   QEI mbed

main.cpp@0:e997997dca8e, 2017-11-09 (annotated)

Committer:
evenbrownie
Date:
Thu Nov 09 01:08:18 2017 +0000
Revision:
0:e997997dca8e
blah blah pid just about done. no turns or anything

Who changed what in which revision?

UserRevisionLine numberNew contents of line
evenbrownie 0:e997997dca8e 1 #include "mbed.h"
evenbrownie 0:e997997dca8e 2 #include "QEI.h"
evenbrownie 0:e997997dca8e 3
evenbrownie 0:e997997dca8e 4 Serial pc(SERIAL_TX, SERIAL_RX);
evenbrownie 0:e997997dca8e 5
evenbrownie 0:e997997dca8e 6 PwmOut lpwmf(PA_7);
evenbrownie 0:e997997dca8e 7 PwmOut lpwmb(PB_6);
evenbrownie 0:e997997dca8e 8
evenbrownie 0:e997997dca8e 9 PwmOut rpwmf(PB_10);
evenbrownie 0:e997997dca8e 10 PwmOut rpwmb(PC_7);
evenbrownie 0:e997997dca8e 11
evenbrownie 0:e997997dca8e 12
evenbrownie 0:e997997dca8e 13 PinName rfront(PB_3);
evenbrownie 0:e997997dca8e 14 PinName rback(PA_15);
evenbrownie 0:e997997dca8e 15 //DigitalIn lfront(PB_3);
evenbrownie 0:e997997dca8e 16 //DigitalIn lback(PA_15);
evenbrownie 0:e997997dca8e 17 //Right Encoder
evenbrownie 0:e997997dca8e 18 PinName lfront(PA_1);
evenbrownie 0:e997997dca8e 19 PinName lback(PC_4);
evenbrownie 0:e997997dca8e 20
evenbrownie 0:e997997dca8e 21 //QEI::Encoding encoding = QEI::Encoding[1];
evenbrownie 0:e997997dca8e 22
evenbrownie 0:e997997dca8e 23 QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
evenbrownie 0:e997997dca8e 24 QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);
evenbrownie 0:e997997dca8e 25
evenbrownie 0:e997997dca8e 26 const float rbase = 0.135f;
evenbrownie 0:e997997dca8e 27 const float lbase = 0.12f;
evenbrownie 0:e997997dca8e 28
evenbrownie 0:e997997dca8e 29 Timer t_time;
evenbrownie 0:e997997dca8e 30
evenbrownie 0:e997997dca8e 31 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?)
evenbrownie 0:e997997dca8e 32 pid(){
evenbrownie 0:e997997dca8e 33 integral = 0.0f;
evenbrownie 0:e997997dca8e 34 prev = 0.0f;
evenbrownie 0:e997997dca8e 35 kp = .01f; //the ks should be negative to counteract error
evenbrownie 0:e997997dca8e 36 ki = 0.0f;
evenbrownie 0:e997997dca8e 37 kd = 0.8f;
evenbrownie 0:e997997dca8e 38 }
evenbrownie 0:e997997dca8e 39 float integral;
evenbrownie 0:e997997dca8e 40 int prev;
evenbrownie 0:e997997dca8e 41 float kp; //the ks should be negative to counteract error
evenbrownie 0:e997997dca8e 42 float ki;
evenbrownie 0:e997997dca8e 43 float kd;
evenbrownie 0:e997997dca8e 44 };
evenbrownie 0:e997997dca8e 45
evenbrownie 0:e997997dca8e 46 void resetpid(struct pid *e) {
evenbrownie 0:e997997dca8e 47 e->integral = 0.0f;
evenbrownie 0:e997997dca8e 48 e->prev = 0.0f;
evenbrownie 0:e997997dca8e 49 }
evenbrownie 0:e997997dca8e 50
evenbrownie 0:e997997dca8e 51 float getFix(struct pid *e, float error, float time) {
evenbrownie 0:e997997dca8e 52 float d = (error - e->prev)/time;
evenbrownie 0:e997997dca8e 53 e->integral += error * time;
evenbrownie 0:e997997dca8e 54 e->prev = error;
evenbrownie 0:e997997dca8e 55 float temp = (e->kp * error + e->ki * e->integral + e->kd * d);
evenbrownie 0:e997997dca8e 56 return temp;
evenbrownie 0:e997997dca8e 57
evenbrownie 0:e997997dca8e 58 }
evenbrownie 0:e997997dca8e 59
evenbrownie 0:e997997dca8e 60 float constrain(float l_limit, float u_limit, float val){
evenbrownie 0:e997997dca8e 61 if (val < l_limit){
evenbrownie 0:e997997dca8e 62 return l_limit;
evenbrownie 0:e997997dca8e 63 }
evenbrownie 0:e997997dca8e 64 else if(val > u_limit){
evenbrownie 0:e997997dca8e 65 return u_limit;
evenbrownie 0:e997997dca8e 66 }
evenbrownie 0:e997997dca8e 67 return val;
evenbrownie 0:e997997dca8e 68 }
evenbrownie 0:e997997dca8e 69
evenbrownie 0:e997997dca8e 70 int main() {
evenbrownie 0:e997997dca8e 71 pid lman, rman;
evenbrownie 0:e997997dca8e 72 lman.kp = .0040f;
evenbrownie 0:e997997dca8e 73 lman.ki = .0002f;
evenbrownie 0:e997997dca8e 74 lman.kd = .0015f;
evenbrownie 0:e997997dca8e 75 rman.kp = .5f;
evenbrownie 0:e997997dca8e 76 rman.ki = .1f;
evenbrownie 0:e997997dca8e 77 rman.kd = .4f;
evenbrownie 0:e997997dca8e 78
evenbrownie 0:e997997dca8e 79
evenbrownie 0:e997997dca8e 80 lpwmf.period(0.01f);
evenbrownie 0:e997997dca8e 81 lpwmf = lbase;
evenbrownie 0:e997997dca8e 82 rpwmf.period(0.01f);
evenbrownie 0:e997997dca8e 83 rpwmf = rbase;
evenbrownie 0:e997997dca8e 84 t_time.start();
evenbrownie 0:e997997dca8e 85
evenbrownie 0:e997997dca8e 86 while(1){
evenbrownie 0:e997997dca8e 87 float dt = t_time.read();
evenbrownie 0:e997997dca8e 88 t_time.reset();
evenbrownie 0:e997997dca8e 89 float error = (LeftEncoder.getPulses() - RightEncoder.getPulses())/16.0f; //Can be pos or neg, Range from 0 - 255 for one revolution
evenbrownie 0:e997997dca8e 90
evenbrownie 0:e997997dca8e 91 // pc.printf("no %d \n", RightEncoder.getPulses());
evenbrownie 0:e997997dca8e 92 // pc.printf("yas %d \n", LeftEncoder.getPulses());
evenbrownie 0:e997997dca8e 93
evenbrownie 0:e997997dca8e 94
evenbrownie 0:e997997dca8e 95 if(error == 0.0f){
evenbrownie 0:e997997dca8e 96 resetpid(&lman);
evenbrownie 0:e997997dca8e 97 resetpid(&rman);
evenbrownie 0:e997997dca8e 98 }
evenbrownie 0:e997997dca8e 99 float adjust_l = getFix(&lman, error, dt);
evenbrownie 0:e997997dca8e 100 pc.printf("%f \n", adjust_l);
evenbrownie 0:e997997dca8e 101
evenbrownie 0:e997997dca8e 102 float lspeed = constrain(0.0f, 0.4f, lbase - adjust_l);
evenbrownie 0:e997997dca8e 103 float rspeed = rbase;
evenbrownie 0:e997997dca8e 104
evenbrownie 0:e997997dca8e 105 pc.printf("no %f \n", rspeed);
evenbrownie 0:e997997dca8e 106 pc.printf("yas %f \n", lspeed);
evenbrownie 0:e997997dca8e 107
evenbrownie 0:e997997dca8e 108 lpwmf = lspeed;
evenbrownie 0:e997997dca8e 109 rpwmf = rspeed;
evenbrownie 0:e997997dca8e 110
evenbrownie 0:e997997dca8e 111 wait(0.2f);
evenbrownie 0:e997997dca8e 112
evenbrownie 0:e997997dca8e 113 }
evenbrownie 0:e997997dca8e 114 t_time.stop();
evenbrownie 0:e997997dca8e 115 }