라인트레이서 동작 소스
Dependencies: Motordriver RIT mbed
Fork of ORTP-L_V01 by
main.cpp
- Committer:
- wjsgur12
- Date:
- 2014-07-11
- Revision:
- 2:7ec3f832f1ed
- Parent:
- 1:1fa7efbfc2d3
File content as of revision 2:7ec3f832f1ed:
#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); // 1ms 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); // 10us rit.append(RIT_IRQHandler); // Append function isr to global interrupt handler. 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 { // Forward 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(); }