File content as of revision 5:6bb52b2a79bf:

#include "mbed.h"
#include "MotorControl.h"
#include "ForceRead.h"

#define DEBUG

Serial              bt(PA_9,PA_10);              // USART1 Bluetooth (Tx, Rx)
Serial              pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)

InterruptIn         up(PC_8);                    // Button Interrupt Motor control
InterruptIn         down(PC_9);
InterruptIn         bt_ml(PC_10);                 // Millking action button       

DigitalOut          led(LED2);                   // Operating LED signal 

AnalogIn            disSensor(PA_7);             // Distance Sensor

Timeout             ResetTimer;                  // Reset angle 0 position

Ticker              timer0;                      // Operating MCU LED on 
Ticker              timer1;                      // Control Angle By Using Distance Sensor
Ticker              timer2;                      // Force Sensor
Ticker              timer3;                      // Millking Action Mode
Ticker              timer4;

int                 targetDis;                   // Target Of Distance
int                 milLoop;                     // Number of Millking Action
int                 onewayNum=0;                 // Counting Turning Point
int                 sysStatus=0;   
bool                mkOn = 0;

/*----------------------- ReadData Variables ------------------------*/

char                rxData[7];
bool                flagRx = 0;

#define parA 343.75f        // parmeter of equation (values of distance sensor)
#define parB -1*508.08f
#define parC 166.67f

/*----------------------- Callback Functions ------------------------*/

void Test()
{
    float d1 = disSensor.read();

    bt.printf("%1.3f\n",d1);
}

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 ControlLED(int mode)
{

    
    switch (mode)
    {
        case 0 :
        timer0.detach();
        led = 0;
        
        break;
        
        case 1 :
        timer0.detach();
        led = 1;
        
        break;
        
        case 2 :
        bt.puts("ANYBARO");
        
        break;      
    }
    
}

void stop()
{
    MotorTest(0);               // [ADD] Stop
}

void ReadAng()
{
    float d1 = disSensor.read();
    float d2 = parA*d1*d1+parB*d1+parC;
    bt.printf("%1.2f",d2);
}

void ModeSelect(int mode)       // [ADD] MODE
{
    
    switch(mode) 
    {
        
        case 0 :                            //Standard Mode
         
        timer1.detach();
        timer2.detach();
        timer3.detach();

        break;
            
        case 1 :                            //Reset Mode
        
        timer3.detach();
        timer2.detach();
        timer1.detach();
        onewayNum = 0;
        milLoop = 0;

        MotorTest(2);
        ResetTimer.attach(&stop,10);
        
        break;         
            
        case 2 :                            //Read Force
        
        timer2.attach(&CalForce,0.05);

        break;

        case 3 :                            //Read Angle
        
        timer2.detach();        
        ReadAng();

        break;
     
        case 4 :                            //Pause Temporarily
        
        MotorTest(0);
        
        break;
        

    }
}


void OperateLED()
{
    led =!led;
}

void ManualMk()
{
    
    if( 1 == sysStatus | 2 == sysStatus)
    {
        bt.puts("Please Wait Moving Anybaro");
    }
    else
    {
        sysStatus = 6;
        MkActionManual();
    }


}

void ButtonSys()
{
    if( 1 == sysStatus | 2 == sysStatus)
    {
        bt.puts("Please Wait Moving Anybaro");
    }
    else
    {
        MotorButton();
    }
}

int main()
{
    bt.baud(115200);
    pc.baud(115200);
    bt.puts("START 181203 Ver.6 \n");

    int modeNum = 0;    
    int modeLED = 0;
    int modeMotor =0;

    char tmpCommand[4]={0,};                     // [ADD] command
    int rxVal;
    
    up.mode(PullNone);
    down.mode(PullNone);
    bt_ml.mode(PullNone);                   // [ADD] Setup Millking Action Manual Button
    
    up.fall(&ButtonSys);                  
    up.rise(&ButtonSys);                 
    down.fall(&ButtonSys);
    down.rise(&ButtonSys);                
    
    bt_ml.fall(&ManualMk);            // [ADD] Interrupt Millking Action Manual Button    
    
    timer0.attach(&OperateLED,0.2);
//    timer4.attach(&Test,0.1);
    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,"LED"))
            {
                #ifdef DEBUG
                bt.puts("\nLED CONTROL MODE!!\n");
                #endif
                modeLED = rxVal;
                ControlLED(modeLED);
            }

            else if (0 == strcmp(tmpCommand,"MOD")) 
            {   
                #ifdef DEBUG
                bt.puts("\nMODE SELECT!!\n");
                #endif
                modeNum = rxVal;
                sysStatus = rxVal;
                ModeSelect(modeNum);
            }
            
            else if (0 == strcmp(tmpCommand,"MOT"))
            {   
                #ifdef DEBUG            
                bt.puts("\nMOTOR TEST CONTROL MODE!!\n");
                #endif
                modeMotor = rxVal;
                MotorTest(modeMotor);                      
            }
            
            else if (0 == strcmp(tmpCommand,"POS"))
            {
                #ifdef DEBUG
                bt.puts("\nMOTOR DISTANCE CONTROL MODE!!\n");
                #endif        
                targetDis = rxVal;
                timer1.attach(&ControlAng,0.8);
            }
            
            if (0 == strcmp(tmpCommand,"MIL"))
            {
                sysStatus = 5;
                #ifdef DEBUG
                bt.puts("\nMILLKING ACTION MODE!!\n");
                #endif
                targetDis = atoi(rxData+5);                                     // [ADD] return 2 characters in the back of 4 characters
                milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5));                 // [ADD] return 2 cahracters in the front of 4 characters
                timer3.attach(&MkAction,0.1);
                
                if ( 0 == mkOn) mkOn = 1;
   
            }
            
     
       }
           
        
    }
    
}