Myo Thant Sin Aung
/
myIRRobo
01
Fork of my1stOLPFilter by
Diff: main.cpp
- Revision:
- 1:4088422c9f8a
- Parent:
- 0:5da774f9c59c
- Child:
- 2:8db9af8bd3fd
diff -r 5da774f9c59c -r 4088422c9f8a main.cpp --- a/main.cpp Fri Jul 20 11:16:46 2018 +0000 +++ b/main.cpp Sun Jul 29 08:54:36 2018 +0000 @@ -2,15 +2,21 @@ #include "mbed.h" Serial pc(USBTX, USBRX); -long counts_per_rev = (1440); +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 = 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; +//double now_x = 0.0; +//double now_y = 0.0; QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING); @@ -24,13 +30,33 @@ Timer myTime; myTime.reset(); myTime.start(); + pc.baud(57600); 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; - - pc.printf(" %F %F \r", now_time, now_ang); + 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); printf("\n\r"); prv_ang = now_ang; prv_time = now_time;