Myo Thant Sin Aung
/
myIRRobo
01
Fork of my1stOLPFilter by
main.cpp
- Committer:
- k_waihlyan
- Date:
- 2018-07-29
- Revision:
- 1:4088422c9f8a
- Parent:
- 0:5da774f9c59c
- Child:
- 2:8db9af8bd3fd
File content as of revision 1:4088422c9f8a:
#include "QEI.h" #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; QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING); float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); } int main() { 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; 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; } }