ORTP-L Version 0.1
Dependencies: Motordriver RIT mbed
Fork of ORTP-L_V01 by
Diff: main.cpp
- Revision:
- 0:efc7a2c2f18e
- Child:
- 1:1fa7efbfc2d3
diff -r 000000000000 -r efc7a2c2f18e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 29 09:19:21 2013 +0000 @@ -0,0 +1,109 @@ +#include "mbed.h" +#include "motordriver.h" +#include "RIT.h" + +AnalogIn L_IR1(p15); // Analog In Pin +AnalogIn L_IR2(p16); // Analog In Pin +AnalogIn L_IR3(p17); // Analog In Pin +AnalogIn R_IR3(p18); // Analog In Pin +AnalogIn R_IR2(p19); // Analog In Pin +AnalogIn R_IR1(p20); // Analog In Pin + +DigitalOut LLED(p7); // Digital Out Pin +DigitalOut RLED(p8); + +DigitalOut L_IRLED1(p9); // Digital Out Pin +DigitalOut L_IRLED2(p10); // Digital Out Pin +DigitalOut L_IRLED3(p11); // Digital Out Pin +DigitalOut R_IRLED3(p12); // Digital Out Pin +DigitalOut R_IRLED2(p13); // Digital Out Pin +DigitalOut R_IRLED1(p14); // Digital Out Pin + +Motor L_Motor(p25, p22, p21, 1); // pwm, fwd, rev, can break +Motor R_Motor(p26, p24, p23, 1); // pwm, fwd, rev, can break + + +Serial serial(USBTX,USBRX); // Tx, Rx Pin + + +volatile uint32_t rithits = 0; //timer1 stops when timer1hits==imer1loop + +Timer rit_timing; + +void RIT_IRQHandler(void) +{ + //Flash Led. + L_IRLED1=!L_IRLED1; + L_IRLED2=!L_IRLED2; + L_IRLED3=!L_IRLED3; + R_IRLED3=!R_IRLED3; + R_IRLED2=!R_IRLED2; + R_IRLED1=!R_IRLED1; + + //Count Hits. + rithits++; +} + +RIT rit(1); //10ms + +int main() +{ + LLED=1; + RLED=1; + int data[6]; + bool sensor[6]={0,0,0,0,0,0}; + + + serial.baud(115200); + rit.setup_us(10); + + rit.append(RIT_IRQHandler); + + rit_timing.start(); + rit.enable(); + + while(1) + { + data[0] = L_IR1 * 100; // ain (0.0 ~ 1.0) + data[1] = L_IR2 * 100; // ain (0.0 ~ 1.0) + data[2] = L_IR3 * 100; // ain (0.0 ~ 1.0) + data[3] = R_IR3 * 100; // ain (0.0 ~ 1.0) + data[4] = R_IR2 * 100; // ain (0.0 ~ 1.0) + data[5] = R_IR1 * 100; // ain (0.0 ~ 1.0) + + serial.printf("| %03d | %03d | %03d | %03d | %03d | %03d |\n", data[0], data[1], data[2], data[3], data[4], data[5]); // mbed -> PC, data(0~100) + + for(int i=0; i < 6; i++) + { + sensor[i] = data[i] > 90 ? 0 : 1; + } + + + if(sensor[0] == 1 || sensor[1] == 1 || sensor[2] == 1) + { //Left + L_Motor.speed(0.3); + R_Motor.speed(0.7); + LLED = 1; RLED = 0; + } + else if(sensor[4] == 1 || sensor[5] == 1 || sensor[6] == 1) + { + //Right + L_Motor.speed(0.7); + R_Motor.speed(0.3); + LLED = 0; RLED = 1; + } + else + { + L_Motor.speed(0.3); + R_Motor.speed(0.3); + LLED = 0; RLED = 0; + } + } + + rit.unappend();rit.append(RIT_IRQHandler); + rit.disable(); + rit_timing.stop(); + + rit_timing.start(); + rit.enable(); +}