main.cpp

00001 #include "mbed.h"
00002 #include "omuni.h"
00003 #include "ds3_si.h"
00004 #define DEBUG 1
00005 
00006 #define CON_OFFSET 15
00007 
00008 I2C i2c(p28, p27);
00009 omuni omu(&i2c, 0x10, 0x14, 0x16);
00010 
00011 Serial pc(USBTX, USBRX);
00012 ds3_si con(p9,p10,2400);
00013 
00014 BusOut serialsel(LED1,LED2,LED3,LED4);
00015 Timer serialtimer;
00016 //Ticker readtimer;
00017 
00018 char ConData[5];
00019 
00020 /*
00021 void mbedreset()
00022 {
00023     NVIC_SystemReset();
00024 }
00025 */
00026 
00027 void GetData()
00028 {
00029     __disable_irq();
00030     NVIC_ClearPendingIRQ(UART0_IRQn);
00031     NVIC_ClearPendingIRQ(UART1_IRQn);
00032     NVIC_ClearPendingIRQ(UART2_IRQn);
00033     NVIC_ClearPendingIRQ(UART3_IRQn);
00034     static bool main_flag = 1 ;
00035     if ( pc.getc() == 'H' ) {
00036         ConData[0] = 'H';
00037         for (int i = 1; i < 5; i++)
00038         {
00039          char t = (char)pc.getc();
00040          ConData[i] = t;
00041         }
00042         if(main_flag)
00043         {
00044             serialtimer.start();
00045             main_flag = 0;
00046         }
00047             
00048         serialtimer.reset();
00049     }
00050     __enable_irq();
00051 }
00052 #define deadZone 2
00053 bool triggerAnalog(){
00054     return abs(con.analogstate(L3x)) > deadZone || abs(con.analogstate(L3y))  > deadZone 
00055     || abs(con.analogstate(R3y))  > deadZone || abs(con.analogstate(R3y)) > deadZone;
00056 }
00057 
00058 char MotorReset[] = {'H', 0, 0, 0, 0};
00059 int main() {
00060         serialsel = 0x05;
00061         pc.baud(115200);
00062         pc.attach(&GetData,Serial::RxIrq);
00063         
00064         serialtimer.stop();
00065         serialtimer.reset();
00066         
00067         while(1){
00068             
00069             wait(0.01);
00070             int8_t MotorData[] = {'H', ConData[1], ConData[2], ConData[3], ConData[4]};
00071             MotorData[3] = con.analogstate(L3x);
00072             MotorData[4] = con.analogstate(L3y);
00073             MotorData[1] = con.analogstate(R3x);
00074             MotorData[2] = con.analogstate(R3y);
00075             if(serialtimer.read_ms() > 500)
00076             {
00077                 serialsel = 1;
00078                 omu.out(MotorReset);
00079             }else{
00080                 if(triggerAnalog() == true){
00081                     if(ConData[1] == 'S' && ConData[2] == 'T'&& ConData[3] == 'O' && ConData[4] == 'P'){
00082                         MotorData[4] = MotorData[4] > 0 ? 0 : MotorData[4];  
00083                         serialsel = 0xF;  
00084                     }else if(ConData[1] == 'O' && ConData[2] == 'N'&& ConData[3] == 'E'){
00085                         MotorData[4] = MotorData[4] > 60 ? 60 : MotorData[4]; 
00086                         if(MotorData[4] > 0) 
00087                             if(ConData[4] == 'R'){
00088                                 MotorData[1] = 30;
00089                                 MotorData[2] = 0;
00090                                 serialsel = 0x2;
00091                             }
00092                             else  if(ConData[4] == 'L')  {
00093                                 MotorData[1] = -30;
00094                                 MotorData[2] = 0;
00095                                 serialsel = 0x4;
00096                             }
00097                     }else if(ConData[1] == 'E' && ConData[2] == 'S'&& ConData[3] == 'T'){
00098                         MotorData[4] = MotorData[4] > 40 ? 40 : MotorData[4]; 
00099                         if(MotorData[4] > 0) 
00100                         if(ConData[4] == 'R'){
00101                             MotorData[1] = 30;
00102                             MotorData[2] = 0;
00103                             serialsel = 0x2;
00104                         }else  if(ConData[4] == 'L')  {
00105                             MotorData[1] = -30;
00106                             MotorData[2] = 0;
00107                             serialsel = 0x4;
00108                         }else if(ConData[4] == 'G'){
00109                             MotorData[3] = 0;
00110                             MotorData[2] = 0;
00111                         }
00112                     }else if(ConData[1] == 'F' && ConData[2] == 'R'&& ConData[3] == 'E' && ConData[4] == 'E'){
00113                             serialsel = 0x0;
00114                     }
00115                     omu.out((char*)MotorData);
00116                 }else{
00117                     serialsel = 1;
00118                     omu.out(MotorReset);
00119                 }
00120                 /*
00121                 #if DEBUG 
00122                 for(int i = 0; i < 5; i++)
00123                 {
00124                         pc.printf("%d",MotorData[i]);
00125                 }
00126                         pc.printf("\n");
00127                 #endif*/
00128                 
00129             }
00130 
00131 
00132         }  
00133 }