File content as of revision 11:5f05b14649ee:

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

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(PA_5);                   // Operating LED signal 

AnalogIn            disSensor(PA_7);             // Distance Sensor

Timeout             ResetTimer;                  // Reset angle 0 position

Ticker              timer0;
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
//float               errorPrevious;

int                 onewayNum=0;                 // Counting Turning Point
int                 sysStatus=0;   
bool                mkOn = 0;

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

char                rxData[7];
bool                flagRx = 0;



float parA =73.671f;
float parB =-1*206.25f;
float parC =106.43f;

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

void Test()
{
    float d1 = disSensor.read();
    float d2 = parA*d1*d1+parB*d1+parC;

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

void ReadData()
{
    char inChar;
    
    static char rxCount = 0;
    static char rxBuf[7];
    
    while(1 == bt.readable())
    {
        
        inChar = bt.getc();
        
        bt.putc(inChar);
        
        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);
        }
        else{
            rxCount = 0;
            flagRx = 0;
        }
            
    }  
}


void stop()
{
    MotorTest(0);               
}

void ReadAng()
{
    float d1 = disSensor.read();
    float d2 = parA*d1*d1+parB*d1+parC;

//    original
//    bt.printf("%1.2f",d2);

//    fixed
    bt.printf("<DIO%1.2f>",d2);
    
}

void ModeSelect(int 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;
            
            float d1 = disSensor.read();
            float d2 = parA*d1*d1+parB*d1+parC;
            
            MotorTest(2);
            ResetTimer.attach(&stop,0.5f+d2/3.0f);
            
            break;         
        }
            
        case 2 :                            //Read Force
        
        timer2.attach(&ReadForce,0.05);

        break;

        case 3 :                            //Read Angle
        
        timer2.detach();        
        ReadAng();                          // return only one message

        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");              //<STA>
    }
    else
    {
        sysStatus = 6;
        MkActionManual();
    }


}

void ButtonSys()
{
    if( 1 == sysStatus | 2 == sysStatus)
    {
        bt.puts("Please Wait Moving Anybaro");              //<STA> 또는 ifdef debug 처리?
        
    }
    else
    {
        MotorButton();
    }
}

int main()
{
    bt.baud(115200);
//    pc.baud(115200);
    bt.puts("START 190627 Ver.8 \n");
    int modeNum = 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,"MOD")) 
            {   
                modeNum = rxVal;                    // 1 ~ 6
                sysStatus = rxVal;                  // Why this value are assigned directly?
                                                    // assigning sys variable after checking condition is correct progress, i think.
                ModeSelect(modeNum);
            }
            
            else if (0 == strcmp(tmpCommand,"MOT"))
            {           
                modeMotor = rxVal;
                MotorTest(modeMotor);
            }
            
            else if (0 == strcmp(tmpCommand,"POS"))
            {
//                errorPrevious = 0;
                targetDis = rxVal;
//                float d1 = disSensor.read();
//                float d2 = parA*d1*d1+parB*d1+parC;
//                errorPrevious = targetDis - d2;
                
                timer1.attach(&ControlAng,0.01);
            }
            
            if (0 == strcmp(tmpCommand,"MIL"))
            {
                sysStatus = 5;
                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;
   
            }
            
     
       }
           
        
    }
    
}