Myo Thant Sin Aung
/
myIRRobo
01
Fork of my1stOLPFilter by
main.cpp@2:8db9af8bd3fd, 2018-08-07 (annotated)
- Committer:
- MTSAung
- Date:
- Tue Aug 07 10:28:00 2018 +0000
- Revision:
- 2:8db9af8bd3fd
- Parent:
- 1:4088422c9f8a
myIRMobileRobo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MTSAung | 0:5da774f9c59c | 1 | #include "QEI.h" |
MTSAung | 0:5da774f9c59c | 2 | #include "mbed.h" |
MTSAung | 0:5da774f9c59c | 3 | Serial pc(USBTX, USBRX); |
MTSAung | 0:5da774f9c59c | 4 | |
MTSAung | 2:8db9af8bd3fd | 5 | PwmOut pwmR (p21); |
MTSAung | 2:8db9af8bd3fd | 6 | DigitalOut pin_R1 (p22); |
MTSAung | 2:8db9af8bd3fd | 7 | DigitalOut pin_R2 (p23); |
MTSAung | 2:8db9af8bd3fd | 8 | |
MTSAung | 2:8db9af8bd3fd | 9 | PwmOut pwmL (p24); |
MTSAung | 2:8db9af8bd3fd | 10 | DigitalOut pin_L1 (p25); |
MTSAung | 2:8db9af8bd3fd | 11 | DigitalOut pin_L2 (p26); |
MTSAung | 2:8db9af8bd3fd | 12 | |
MTSAung | 2:8db9af8bd3fd | 13 | DigitalIn IRpin1 (p16); |
MTSAung | 2:8db9af8bd3fd | 14 | DigitalIn IRpin2 (p17); |
MTSAung | 2:8db9af8bd3fd | 15 | DigitalIn IRpin3 (p18); |
MTSAung | 2:8db9af8bd3fd | 16 | DigitalIn IRpin4 (p19); |
MTSAung | 2:8db9af8bd3fd | 17 | DigitalIn IRpin5 (p20); |
MTSAung | 2:8db9af8bd3fd | 18 | |
MTSAung | 2:8db9af8bd3fd | 19 | double voltR, voltL; |
MTSAung | 2:8db9af8bd3fd | 20 | int IRread1, IRread2, IRread3, IRread4, IRread5; |
MTSAung | 2:8db9af8bd3fd | 21 | int OIRread1, OIRread2, OIRread3, OIRread4, OIRread5; |
MTSAung | 2:8db9af8bd3fd | 22 | int prv_err, act, err; |
MTSAung | 2:8db9af8bd3fd | 23 | |
MTSAung | 2:8db9af8bd3fd | 24 | int des = 0; |
MTSAung | 2:8db9af8bd3fd | 25 | double alpha = 0.4; |
MTSAung | 2:8db9af8bd3fd | 26 | double tau = 0.1; |
MTSAung | 2:8db9af8bd3fd | 27 | long counts_per_rev = 64; |
MTSAung | 2:8db9af8bd3fd | 28 | double prv_ang, prv_time; |
MTSAung | 2:8db9af8bd3fd | 29 | int prv_errF = 0; |
MTSAung | 2:8db9af8bd3fd | 30 | double errL, errF, now_ang, now_omg, now_time, samp_time, GoLog; |
MTSAung | 0:5da774f9c59c | 31 | |
MTSAung | 0:5da774f9c59c | 32 | QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING); |
MTSAung | 0:5da774f9c59c | 33 | |
MTSAung | 2:8db9af8bd3fd | 34 | float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); } |
MTSAung | 0:5da774f9c59c | 35 | |
MTSAung | 0:5da774f9c59c | 36 | int main() |
MTSAung | 0:5da774f9c59c | 37 | { |
MTSAung | 0:5da774f9c59c | 38 | Timer myTime; |
MTSAung | 0:5da774f9c59c | 39 | myTime.reset(); |
MTSAung | 0:5da774f9c59c | 40 | myTime.start(); |
MTSAung | 2:8db9af8bd3fd | 41 | pc.baud(9600); |
MTSAung | 0:5da774f9c59c | 42 | while(1) { |
MTSAung | 2:8db9af8bd3fd | 43 | IRread1 = IRpin1.read(); |
MTSAung | 2:8db9af8bd3fd | 44 | IRread2 = IRpin2.read(); |
MTSAung | 2:8db9af8bd3fd | 45 | IRread3 = IRpin3.read(); |
MTSAung | 2:8db9af8bd3fd | 46 | IRread4 = IRpin4.read(); |
MTSAung | 2:8db9af8bd3fd | 47 | IRread5 = IRpin5.read(); |
MTSAung | 2:8db9af8bd3fd | 48 | |
MTSAung | 2:8db9af8bd3fd | 49 | now_time = myTime.read_ms()/1000.0; |
MTSAung | 2:8db9af8bd3fd | 50 | samp_time = now_time - prv_time; |
MTSAung | 2:8db9af8bd3fd | 51 | now_ang = pulsesToDegrees(wheel.getPulses()); |
MTSAung | 2:8db9af8bd3fd | 52 | now_omg = (now_ang - prv_ang)/samp_time; |
MTSAung | 2:8db9af8bd3fd | 53 | |
MTSAung | 2:8db9af8bd3fd | 54 | if(IRread1 == 0) {OIRread1 = 1;} else if(IRread1 == 1) {OIRread1 = 0;} |
MTSAung | 2:8db9af8bd3fd | 55 | if(IRread2 == 0) {OIRread2 = 1;} else if(IRread2 == 1) {OIRread2 = 0;} |
MTSAung | 2:8db9af8bd3fd | 56 | if(IRread3 == 0) {OIRread3 = 1;} else if(IRread3 == 1) {OIRread3 = 0;} |
MTSAung | 2:8db9af8bd3fd | 57 | if(IRread4 == 0) {OIRread4 = 1;} else if(IRread4 == 1) {OIRread4 = 0;} |
MTSAung | 2:8db9af8bd3fd | 58 | if(IRread5 == 0) {OIRread5 = 1;} else if(IRread5 == 1) {OIRread5 = 0;} |
MTSAung | 2:8db9af8bd3fd | 59 | |
MTSAung | 2:8db9af8bd3fd | 60 | act = ((OIRread1*(-4))+(OIRread2*(-2))+(OIRread3*(0))+(OIRread4*(2))+(OIRread5*(4)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ; |
MTSAung | 2:8db9af8bd3fd | 61 | err = des - act ; |
MTSAung | 2:8db9af8bd3fd | 62 | errL = double ( err + alpha * ((err - prv_err)/samp_time )); |
MTSAung | 2:8db9af8bd3fd | 63 | errF = ((errL*samp_time) + (tau*prv_errF))/( tau + samp_time ); |
MTSAung | 2:8db9af8bd3fd | 64 | |
MTSAung | 2:8db9af8bd3fd | 65 | if (errF < 0) { voltR = 0.00; voltL = 12.00; pin_R1 = 0; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 1; } |
MTSAung | 2:8db9af8bd3fd | 66 | else if(errF > 0) { voltR = 12.00; voltL = 0.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 0; pin_L2 = 0; GoLog = -1; } |
MTSAung | 2:8db9af8bd3fd | 67 | else if(errF == 0) { voltR = 12.00; voltL = 12.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 0; } |
MTSAung | 2:8db9af8bd3fd | 68 | pwmR = (voltR/12.0); |
MTSAung | 2:8db9af8bd3fd | 69 | pwmL = (voltL/12.0); |
MTSAung | 2:8db9af8bd3fd | 70 | |
MTSAung | 2:8db9af8bd3fd | 71 | //pc.printf(" %d %f %d %d %d %d %d %f\r", err, errL, IRread1, IRread2, IRread3, IRread4, IRread5, GoLog); |
MTSAung | 2:8db9af8bd3fd | 72 | pc.printf(" %d %f %f\r", err, errL, errF); |
MTSAung | 0:5da774f9c59c | 73 | printf("\n\r"); |
MTSAung | 0:5da774f9c59c | 74 | prv_ang = now_ang; |
MTSAung | 0:5da774f9c59c | 75 | prv_time = now_time; |
MTSAung | 2:8db9af8bd3fd | 76 | prv_err = err; |
MTSAung | 2:8db9af8bd3fd | 77 | prv_errF = errF; |
MTSAung | 0:5da774f9c59c | 78 | } |
MTSAung | 2:8db9af8bd3fd | 79 | } |