File content as of revision 8:2a4298cbae3f:

#include "mbed.h"
#include "PololuLedStrip.h"
#include "QEI.h"
#include "MotorControl.h"
#include "RgbControl.h"

//Serial              bt(USBTX,USBRX);

Serial          bt(D1,D0);

InterruptIn         LeftReset(PF_0);
InterruptIn         RightReset(PB_4);

QEI                 re(PA_5,PA_7,NC,220);

Ticker              timer1; //control Position
Timeout             ResetTimer;


int                 targetDis;
char                rxData[7];
bool                flagRx = 0;
int                 dayLed;

int R;
int G;
int B;

void ReadData()
{
    char inChar;
    static char rxCount = 0;
    static char rxBuf[7];
    
    while(1 == bt.readable())
    {
        
        inChar = bt.getc();
     
        if('<' == inChar)
        {
            rxCount = 1;
        }
        
        else if (rxCount > 0 && rxCount <8)
        {
            rxBuf[rxCount-1] = inChar;
            rxCount++;            
        }
        
        else if ( 8 == rxCount && '>' == inChar)
        {
            rxCount = 0;
            flagRx = 1;
            memcpy(rxData, rxBuf, 7);
            
        }
    }  
}


void Reset_L()
{   
    MotorTest(0);
    re.reset();
}

void Reset_R()
{   
    MotorTest(0);
}

void Stop()
{
    MotorTest(0);
}

int main()
{
    
        bt.baud(115200);
        bt.puts("START\n");
                MotorTest(2);
        ResetTimer.attach(&Stop,15);
        

        LeftReset.rise(&Reset_L);
        RightReset.rise(&Reset_R);
        
        char tmpCommand[4] = {0,};                     // [ADD] command
        int rxVal = 0;
        
        int modeMotor = 0;
        int modeNum = 0;
        bt.attach(&ReadData, Serial::RxIrq);

        while(1)
        {
            
            if (1 == flagRx)
            {
                flagRx = 0;
                tmpCommand[0] = rxData[0];
                tmpCommand[1] = rxData[1];
                tmpCommand[2] = rxData[2];
                rxVal = atoi(rxData+3);
                
                if (0 == strcmp(tmpCommand,"MOT"))
                {               
                    bt.putc(tmpCommand[0]);
                    modeMotor = rxVal;
                    MotorTest(modeMotor);              
                                    
                }
                
                else if (0 == strcmp(tmpCommand,"POS"))
                {
                    bt.putc(tmpCommand[0]);     
                    targetDis = rxVal;
                    timer1.attach(&ControlPos,0.1);
                }
                
                else if (0 == strcmp(tmpCommand,"MOD"))
                {  
                    bt.putc(tmpCommand[0]);     
                    modeNum = rxVal;
                    ModeSelect(modeNum);
                    
                }
                
                else if (0 == strcmp(tmpCommand,"DAY"))
                {   
                    ModeSelect(4);
                    bt.putc(tmpCommand[0]);     
                    targetDis = atoi(rxData+5);                                     // [ADD] return 2 characters in the back of 4 characters
                    dayLed = 0.01*(atoi(rxData+3)-atoi(rxData+5));                 // [ADD] return 2 cahracters in the front of 4 characters
                    timer1.attach(&ControlPos,0.1);            
                }                
                
           }
            
//            int pulse = re.getPulses();  
//            int d = pulse/220;
//            int e = LeftReset.read();
//            bt.printf("%d %d \n",d,e);

            wait(0.1);

                    
        }



}