main.cpp

00001 #include "mbed.h"
00002 #include "PololuLedStrip.h"
00003 #include "QEI.h"
00004 #include "MotorControl.h"
00005 #include "RgbControl.h"
00006 
00007 //Serial              bt(USBTX,USBRX);
00008 
00009 Serial          bt(D1,D0);
00010 
00011 InterruptIn         LeftReset(PF_0);
00012 InterruptIn         RightReset(PB_4);
00013 
00014 QEI                 re(PA_5,PA_7,NC,220);
00015 
00016 Ticker              timer1; //control Position
00017 Timeout             ResetTimer;
00018 
00019 
00020 int                 targetDis;
00021 char                rxData[7];
00022 bool                flagRx = 0;
00023 int                 dayLed;
00024 
00025 int R;
00026 int G;
00027 int B;
00028 
00029 void ReadData()
00030 {
00031     char inChar;
00032     static char rxCount = 0;
00033     static char rxBuf[7];
00034     
00035     while(1 == bt.readable())
00036     {
00037         
00038         inChar = bt.getc();
00039      
00040         if('<' == inChar)
00041         {
00042             rxCount = 1;
00043         }
00044         
00045         else if (rxCount > 0 && rxCount <8)
00046         {
00047             rxBuf[rxCount-1] = inChar;
00048             rxCount++;            
00049         }
00050         
00051         else if ( 8 == rxCount && '>' == inChar)
00052         {
00053             rxCount = 0;
00054             flagRx = 1;
00055             memcpy(rxData, rxBuf, 7);
00056             
00057         }
00058     }  
00059 }
00060 
00061 
00062 void Reset_L()
00063 {   
00064     MotorTest(0);
00065     re.reset();
00066 }
00067 
00068 void Reset_R()
00069 {   
00070     MotorTest(0);
00071 }
00072 
00073 void Stop()
00074 {
00075     MotorTest(0);
00076 }
00077 
00078 int main()
00079 {
00080     
00081         bt.baud(115200);
00082         bt.puts("START\n");
00083                 MotorTest(2);
00084         ResetTimer.attach(&Stop,15);
00085         
00086 
00087         LeftReset.rise(&Reset_L);
00088         RightReset.rise(&Reset_R);
00089         
00090         char tmpCommand[4] = {0,};                     // [ADD] command
00091         int rxVal = 0;
00092         
00093         int modeMotor = 0;
00094         int modeNum = 0;
00095         bt.attach(&ReadData, Serial::RxIrq);
00096 
00097         while(1)
00098         {
00099             
00100             if (1 == flagRx)
00101             {
00102                 flagRx = 0;
00103                 tmpCommand[0] = rxData[0];
00104                 tmpCommand[1] = rxData[1];
00105                 tmpCommand[2] = rxData[2];
00106                 rxVal = atoi(rxData+3);
00107                 
00108                 if (0 == strcmp(tmpCommand,"MOT"))
00109                 {               
00110                     bt.putc(tmpCommand[0]);
00111                     modeMotor = rxVal;
00112                     MotorTest(modeMotor);              
00113                                     
00114                 }
00115                 
00116                 else if (0 == strcmp(tmpCommand,"POS"))
00117                 {
00118                     bt.putc(tmpCommand[0]);     
00119                     targetDis = rxVal;
00120                     timer1.attach(&ControlPos,0.1);
00121                 }
00122                 
00123                 else if (0 == strcmp(tmpCommand,"MOD"))
00124                 {  
00125                     bt.putc(tmpCommand[0]);     
00126                     modeNum = rxVal;
00127                     ModeSelect(modeNum);
00128                     
00129                 }
00130                 
00131                 else if (0 == strcmp(tmpCommand,"DAY"))
00132                 {   
00133                     ModeSelect(4);
00134                     bt.putc(tmpCommand[0]);     
00135                     targetDis = atoi(rxData+5);                                     // [ADD] return 2 characters in the back of 4 characters
00136                     dayLed = 0.01*(atoi(rxData+3)-atoi(rxData+5));                 // [ADD] return 2 cahracters in the front of 4 characters
00137                     timer1.attach(&ControlPos,0.1);            
00138                 }                
00139                 
00140            }
00141             
00142 //            int pulse = re.getPulses();  
00143 //            int d = pulse/220;
00144 //            int e = LeftReset.read();
00145 //            bt.printf("%d %d \n",d,e);
00146 
00147             wait(0.1);
00148 
00149                     
00150         }
00151 
00152 
00153 
00154 }