Myo Thant Sin Aung
/
myIRRobo
01
Fork of my1stOLPFilter by
Diff: main.cpp
- Revision:
- 2:8db9af8bd3fd
- Parent:
- 1:4088422c9f8a
--- a/main.cpp Sun Jul 29 08:54:36 2018 +0000 +++ b/main.cpp Tue Aug 07 10:28:00 2018 +0000 @@ -2,63 +2,78 @@ #include "mbed.h" Serial pc(USBTX, USBRX); -AnalogOut pwm (p18); -DigitalOut pin_a (p19); -DigitalOut pin_b (p20); -DigitalIn IRpin(p16); -DigitalIn IRpin2(p17); -int ir,ir2; -long counts_per_rev = (64); -double prv_ang = 0.0; -double now_ang; -double now_omg = 0.0; -double prv_time = 0.0; -double now_time = 0.0; -double samp_time = 0.0; -//double now_x = 0.0; -//double now_y = 0.0; +PwmOut pwmR (p21); +DigitalOut pin_R1 (p22); +DigitalOut pin_R2 (p23); + +PwmOut pwmL (p24); +DigitalOut pin_L1 (p25); +DigitalOut pin_L2 (p26); + +DigitalIn IRpin1 (p16); +DigitalIn IRpin2 (p17); +DigitalIn IRpin3 (p18); +DigitalIn IRpin4 (p19); +DigitalIn IRpin5 (p20); + +double voltR, voltL; +int IRread1, IRread2, IRread3, IRread4, IRread5; +int OIRread1, OIRread2, OIRread3, OIRread4, OIRread5; +int prv_err, act, err; + +int des = 0; +double alpha = 0.4; +double tau = 0.1; +long counts_per_rev = 64; +double prv_ang, prv_time; +int prv_errF = 0; +double errL, errF, now_ang, now_omg, now_time, samp_time, GoLog; QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING); -float pulsesToDegrees(float pulses) -{ - return ((pulses/counts_per_rev)*360); -} +float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); } int main() { Timer myTime; myTime.reset(); myTime.start(); - pc.baud(57600); + pc.baud(9600); while(1) { - ir = IRpin.read(); - ir2 = IRpin2.read(); - now_time = myTime.read_ms()/1000.0; - samp_time = now_time - prv_time; - now_ang = pulsesToDegrees(wheel.getPulses()); - now_omg = (now_ang - prv_ang)/samp_time; - if(ir == 0 && ir2 == 1) - { - pwm = 0.8 ; - pin_a = 1; - pin_b = 0; - } - else if(ir == 1 && ir2 == 0) - { - pwm = 0.8 ; - pin_a = 0; - pin_b = 1; - } - else - { - pwm = 0.8 ; - pin_a = 0; - pin_b = 0; - } - pc.printf("%d %d %f\r",ir,ir2,now_omg); + IRread1 = IRpin1.read(); + IRread2 = IRpin2.read(); + IRread3 = IRpin3.read(); + IRread4 = IRpin4.read(); + IRread5 = IRpin5.read(); + + now_time = myTime.read_ms()/1000.0; + samp_time = now_time - prv_time; + now_ang = pulsesToDegrees(wheel.getPulses()); + now_omg = (now_ang - prv_ang)/samp_time; + + if(IRread1 == 0) {OIRread1 = 1;} else if(IRread1 == 1) {OIRread1 = 0;} + if(IRread2 == 0) {OIRread2 = 1;} else if(IRread2 == 1) {OIRread2 = 0;} + if(IRread3 == 0) {OIRread3 = 1;} else if(IRread3 == 1) {OIRread3 = 0;} + if(IRread4 == 0) {OIRread4 = 1;} else if(IRread4 == 1) {OIRread4 = 0;} + if(IRread5 == 0) {OIRread5 = 1;} else if(IRread5 == 1) {OIRread5 = 0;} + + act = ((OIRread1*(-4))+(OIRread2*(-2))+(OIRread3*(0))+(OIRread4*(2))+(OIRread5*(4)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ; + err = des - act ; + errL = double ( err + alpha * ((err - prv_err)/samp_time )); + errF = ((errL*samp_time) + (tau*prv_errF))/( tau + samp_time ); + + if (errF < 0) { voltR = 0.00; voltL = 12.00; pin_R1 = 0; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 1; } + else if(errF > 0) { voltR = 12.00; voltL = 0.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 0; pin_L2 = 0; GoLog = -1; } + else if(errF == 0) { voltR = 12.00; voltL = 12.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 0; } + pwmR = (voltR/12.0); + pwmL = (voltL/12.0); + + //pc.printf(" %d %f %d %d %d %d %d %f\r", err, errL, IRread1, IRread2, IRread3, IRread4, IRread5, GoLog); + pc.printf(" %d %f %f\r", err, errL, errF); printf("\n\r"); prv_ang = now_ang; prv_time = now_time; + prv_err = err; + prv_errF = errF; } -} +} \ No newline at end of file