ORTP-L Version 0.1
Dependencies: Motordriver RIT mbed
Fork of ORTP-L_V01 by
main.cpp@1:1fa7efbfc2d3, 2013-03-30 (annotated)
- Committer:
- passionvirus
- Date:
- Sat Mar 30 01:16:25 2013 +0000
- Revision:
- 1:1fa7efbfc2d3
- Parent:
- 0:efc7a2c2f18e
Initial Release
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
passionvirus | 0:efc7a2c2f18e | 1 | #include "mbed.h" |
passionvirus | 0:efc7a2c2f18e | 2 | #include "motordriver.h" |
passionvirus | 0:efc7a2c2f18e | 3 | #include "RIT.h" |
passionvirus | 0:efc7a2c2f18e | 4 | |
passionvirus | 0:efc7a2c2f18e | 5 | AnalogIn L_IR1(p15); // Analog In Pin |
passionvirus | 0:efc7a2c2f18e | 6 | AnalogIn L_IR2(p16); // Analog In Pin |
passionvirus | 0:efc7a2c2f18e | 7 | AnalogIn L_IR3(p17); // Analog In Pin |
passionvirus | 0:efc7a2c2f18e | 8 | AnalogIn R_IR3(p18); // Analog In Pin |
passionvirus | 0:efc7a2c2f18e | 9 | AnalogIn R_IR2(p19); // Analog In Pin |
passionvirus | 0:efc7a2c2f18e | 10 | AnalogIn R_IR1(p20); // Analog In Pin |
passionvirus | 0:efc7a2c2f18e | 11 | |
passionvirus | 0:efc7a2c2f18e | 12 | DigitalOut LLED(p7); // Digital Out Pin |
passionvirus | 0:efc7a2c2f18e | 13 | DigitalOut RLED(p8); |
passionvirus | 0:efc7a2c2f18e | 14 | |
passionvirus | 0:efc7a2c2f18e | 15 | DigitalOut L_IRLED1(p9); // Digital Out Pin |
passionvirus | 0:efc7a2c2f18e | 16 | DigitalOut L_IRLED2(p10); // Digital Out Pin |
passionvirus | 0:efc7a2c2f18e | 17 | DigitalOut L_IRLED3(p11); // Digital Out Pin |
passionvirus | 0:efc7a2c2f18e | 18 | DigitalOut R_IRLED3(p12); // Digital Out Pin |
passionvirus | 0:efc7a2c2f18e | 19 | DigitalOut R_IRLED2(p13); // Digital Out Pin |
passionvirus | 0:efc7a2c2f18e | 20 | DigitalOut R_IRLED1(p14); // Digital Out Pin |
passionvirus | 0:efc7a2c2f18e | 21 | |
passionvirus | 0:efc7a2c2f18e | 22 | Motor L_Motor(p25, p22, p21, 1); // pwm, fwd, rev, can break |
passionvirus | 0:efc7a2c2f18e | 23 | Motor R_Motor(p26, p24, p23, 1); // pwm, fwd, rev, can break |
passionvirus | 0:efc7a2c2f18e | 24 | |
passionvirus | 0:efc7a2c2f18e | 25 | |
passionvirus | 0:efc7a2c2f18e | 26 | Serial serial(USBTX,USBRX); // Tx, Rx Pin |
passionvirus | 0:efc7a2c2f18e | 27 | |
passionvirus | 0:efc7a2c2f18e | 28 | |
passionvirus | 0:efc7a2c2f18e | 29 | volatile uint32_t rithits = 0; //timer1 stops when timer1hits==imer1loop |
passionvirus | 0:efc7a2c2f18e | 30 | |
passionvirus | 0:efc7a2c2f18e | 31 | Timer rit_timing; |
passionvirus | 0:efc7a2c2f18e | 32 | |
passionvirus | 0:efc7a2c2f18e | 33 | void RIT_IRQHandler(void) |
passionvirus | 0:efc7a2c2f18e | 34 | { |
passionvirus | 0:efc7a2c2f18e | 35 | //Flash Led. |
passionvirus | 0:efc7a2c2f18e | 36 | L_IRLED1=!L_IRLED1; |
passionvirus | 0:efc7a2c2f18e | 37 | L_IRLED2=!L_IRLED2; |
passionvirus | 0:efc7a2c2f18e | 38 | L_IRLED3=!L_IRLED3; |
passionvirus | 0:efc7a2c2f18e | 39 | R_IRLED3=!R_IRLED3; |
passionvirus | 0:efc7a2c2f18e | 40 | R_IRLED2=!R_IRLED2; |
passionvirus | 0:efc7a2c2f18e | 41 | R_IRLED1=!R_IRLED1; |
passionvirus | 0:efc7a2c2f18e | 42 | |
passionvirus | 0:efc7a2c2f18e | 43 | //Count Hits. |
passionvirus | 0:efc7a2c2f18e | 44 | rithits++; |
passionvirus | 0:efc7a2c2f18e | 45 | } |
passionvirus | 0:efc7a2c2f18e | 46 | |
passionvirus | 1:1fa7efbfc2d3 | 47 | RIT rit(1); // 1ms |
passionvirus | 0:efc7a2c2f18e | 48 | |
passionvirus | 0:efc7a2c2f18e | 49 | int main() |
passionvirus | 0:efc7a2c2f18e | 50 | { |
passionvirus | 0:efc7a2c2f18e | 51 | LLED=1; |
passionvirus | 0:efc7a2c2f18e | 52 | RLED=1; |
passionvirus | 0:efc7a2c2f18e | 53 | int data[6]; |
passionvirus | 0:efc7a2c2f18e | 54 | bool sensor[6]={0,0,0,0,0,0}; |
passionvirus | 0:efc7a2c2f18e | 55 | |
passionvirus | 0:efc7a2c2f18e | 56 | |
passionvirus | 0:efc7a2c2f18e | 57 | serial.baud(115200); |
passionvirus | 1:1fa7efbfc2d3 | 58 | rit.setup_us(10); // 10us |
passionvirus | 0:efc7a2c2f18e | 59 | |
passionvirus | 1:1fa7efbfc2d3 | 60 | rit.append(RIT_IRQHandler); // Append function isr to global interrupt handler. |
passionvirus | 0:efc7a2c2f18e | 61 | |
passionvirus | 0:efc7a2c2f18e | 62 | rit_timing.start(); |
passionvirus | 0:efc7a2c2f18e | 63 | rit.enable(); |
passionvirus | 0:efc7a2c2f18e | 64 | |
passionvirus | 0:efc7a2c2f18e | 65 | while(1) |
passionvirus | 0:efc7a2c2f18e | 66 | { |
passionvirus | 0:efc7a2c2f18e | 67 | data[0] = L_IR1 * 100; // ain (0.0 ~ 1.0) |
passionvirus | 0:efc7a2c2f18e | 68 | data[1] = L_IR2 * 100; // ain (0.0 ~ 1.0) |
passionvirus | 0:efc7a2c2f18e | 69 | data[2] = L_IR3 * 100; // ain (0.0 ~ 1.0) |
passionvirus | 0:efc7a2c2f18e | 70 | data[3] = R_IR3 * 100; // ain (0.0 ~ 1.0) |
passionvirus | 0:efc7a2c2f18e | 71 | data[4] = R_IR2 * 100; // ain (0.0 ~ 1.0) |
passionvirus | 0:efc7a2c2f18e | 72 | data[5] = R_IR1 * 100; // ain (0.0 ~ 1.0) |
passionvirus | 0:efc7a2c2f18e | 73 | |
passionvirus | 0:efc7a2c2f18e | 74 | 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) |
passionvirus | 0:efc7a2c2f18e | 75 | |
passionvirus | 0:efc7a2c2f18e | 76 | for(int i=0; i < 6; i++) |
passionvirus | 0:efc7a2c2f18e | 77 | { |
passionvirus | 0:efc7a2c2f18e | 78 | sensor[i] = data[i] > 90 ? 0 : 1; |
passionvirus | 0:efc7a2c2f18e | 79 | } |
passionvirus | 0:efc7a2c2f18e | 80 | |
passionvirus | 0:efc7a2c2f18e | 81 | |
passionvirus | 0:efc7a2c2f18e | 82 | if(sensor[0] == 1 || sensor[1] == 1 || sensor[2] == 1) |
passionvirus | 1:1fa7efbfc2d3 | 83 | { // Left |
passionvirus | 0:efc7a2c2f18e | 84 | L_Motor.speed(0.3); |
passionvirus | 0:efc7a2c2f18e | 85 | R_Motor.speed(0.7); |
passionvirus | 0:efc7a2c2f18e | 86 | LLED = 1; RLED = 0; |
passionvirus | 0:efc7a2c2f18e | 87 | } |
passionvirus | 0:efc7a2c2f18e | 88 | else if(sensor[4] == 1 || sensor[5] == 1 || sensor[6] == 1) |
passionvirus | 0:efc7a2c2f18e | 89 | { |
passionvirus | 1:1fa7efbfc2d3 | 90 | // Right |
passionvirus | 0:efc7a2c2f18e | 91 | L_Motor.speed(0.7); |
passionvirus | 0:efc7a2c2f18e | 92 | R_Motor.speed(0.3); |
passionvirus | 0:efc7a2c2f18e | 93 | LLED = 0; RLED = 1; |
passionvirus | 0:efc7a2c2f18e | 94 | } |
passionvirus | 0:efc7a2c2f18e | 95 | else |
passionvirus | 0:efc7a2c2f18e | 96 | { |
passionvirus | 1:1fa7efbfc2d3 | 97 | // Forward |
passionvirus | 0:efc7a2c2f18e | 98 | L_Motor.speed(0.3); |
passionvirus | 0:efc7a2c2f18e | 99 | R_Motor.speed(0.3); |
passionvirus | 0:efc7a2c2f18e | 100 | LLED = 0; RLED = 0; |
passionvirus | 0:efc7a2c2f18e | 101 | } |
passionvirus | 0:efc7a2c2f18e | 102 | } |
passionvirus | 0:efc7a2c2f18e | 103 | |
passionvirus | 0:efc7a2c2f18e | 104 | rit.unappend();rit.append(RIT_IRQHandler); |
passionvirus | 0:efc7a2c2f18e | 105 | rit.disable(); |
passionvirus | 0:efc7a2c2f18e | 106 | rit_timing.stop(); |
passionvirus | 0:efc7a2c2f18e | 107 | |
passionvirus | 0:efc7a2c2f18e | 108 | rit_timing.start(); |
passionvirus | 0:efc7a2c2f18e | 109 | rit.enable(); |
passionvirus | 0:efc7a2c2f18e | 110 | } |