Yoonseok Pyo / Mbed 2 deprecated ORTP-L_V01

ORTP-L Version 0.1

Dependencies:   Motordriver RIT mbed

Fork of ORTP-L_V01 by Yoonseok Pyo

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?

UserRevisionLine numberNew 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 }