main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 #include "RIT.h"
00004 
00005 AnalogIn L_IR1(p15);      // Analog In Pin
00006 AnalogIn L_IR2(p16);      // Analog In Pin
00007 AnalogIn L_IR3(p17);      // Analog In Pin
00008 AnalogIn R_IR3(p18);      // Analog In Pin
00009 AnalogIn R_IR2(p19);      // Analog In Pin
00010 AnalogIn R_IR1(p20);      // Analog In Pin
00011 
00012 DigitalOut LLED(p7);   // Digital Out Pin
00013 DigitalOut RLED(p8);
00014 
00015 DigitalOut L_IRLED1(p9);    // Digital Out Pin
00016 DigitalOut L_IRLED2(p10);   // Digital Out Pin
00017 DigitalOut L_IRLED3(p11);   // Digital Out Pin
00018 DigitalOut R_IRLED3(p12);   // Digital Out Pin
00019 DigitalOut R_IRLED2(p13);   // Digital Out Pin
00020 DigitalOut R_IRLED1(p14);   // Digital Out Pin
00021 
00022 Motor L_Motor(p25, p22, p21, 1); // pwm, fwd, rev, can break
00023 Motor R_Motor(p26, p24, p23, 1); // pwm, fwd, rev, can break
00024 
00025 
00026 Serial serial(USBTX,USBRX); // Tx, Rx Pin
00027 
00028 
00029 volatile uint32_t rithits = 0;       //timer1 stops when timer1hits==imer1loop
00030 
00031 Timer rit_timing;
00032 
00033 void RIT_IRQHandler(void) 
00034 {
00035     //Flash Led.
00036     L_IRLED1=!L_IRLED1;
00037     L_IRLED2=!L_IRLED2;
00038     L_IRLED3=!L_IRLED3;
00039     R_IRLED3=!R_IRLED3;
00040     R_IRLED2=!R_IRLED2;
00041     R_IRLED1=!R_IRLED1;
00042             
00043     //Count Hits.
00044     rithits++;
00045 }
00046 
00047 RIT rit(1); // 1ms
00048 
00049 int main() 
00050 {
00051     LLED=1; 
00052     RLED=1;
00053     int data[6];
00054     bool sensor[6]={0,0,0,0,0,0};
00055 
00056     
00057     serial.baud(115200);
00058     rit.setup_us(10); // 10us
00059 
00060     rit.append(RIT_IRQHandler); // Append function isr to global interrupt handler. 
00061 
00062     rit_timing.start();
00063     rit.enable();
00064                
00065     while(1) 
00066     {
00067         data[0] = L_IR1 * 100;                   // ain (0.0 ~ 1.0)
00068         data[1] = L_IR2 * 100;                   // ain (0.0 ~ 1.0)
00069         data[2] = L_IR3 * 100;                   // ain (0.0 ~ 1.0)
00070         data[3] = R_IR3 * 100;                   // ain (0.0 ~ 1.0)
00071         data[4] = R_IR2 * 100;                   // ain (0.0 ~ 1.0)
00072         data[5] = R_IR1 * 100;                   // ain (0.0 ~ 1.0)
00073          
00074         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)
00075 
00076         for(int i=0; i < 6; i++)
00077         {
00078             sensor[i] = data[i] > 90 ? 0 : 1;
00079         }
00080    
00081 
00082         if(sensor[0] == 1 || sensor[1] == 1 || sensor[2] == 1)
00083         {   // Left
00084             L_Motor.speed(0.3);
00085             R_Motor.speed(0.7);
00086             LLED = 1; RLED = 0;   
00087         }
00088         else if(sensor[4] == 1 || sensor[5] == 1 || sensor[6] == 1)
00089         {
00090             // Right
00091             L_Motor.speed(0.7);
00092             R_Motor.speed(0.3);
00093             LLED = 0; RLED = 1;
00094         }
00095         else
00096         {
00097             // Forward
00098             L_Motor.speed(0.3);
00099             R_Motor.speed(0.3);
00100             LLED = 0; RLED = 0;        
00101         }
00102     }
00103     
00104     rit.unappend();rit.append(RIT_IRQHandler);
00105     rit.disable();
00106     rit_timing.stop();
00107 
00108     rit_timing.start();
00109     rit.enable();
00110 }