File content as of revision 19:67584cb64b9c:

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

#define QUALIFYING_MODE

Serial              bt(PA_2,PA_3);              // USART1 Bluetooth (Tx, Rx)
// Serial              pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)
//Serial              pc(USBTX,USBRX);               // USART2 pc (Tx, Rx)

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

DigitalOut          led(PA_5);                   // Operating LED signal 

AnalogIn            disSensor(PA_7);             // Distance Sensor

Timeout             ResetTimer;                  // Reset angle 0 position

#ifdef QUALIFYING_MODE
Timer               Qtimer;                       // timer for qualifying test
int                 Qtime;                        // Qtime stores result from Qtimer
bool                Qflag;                        // Qflag rises when result is stored
#endif
DigitalOut          enable(PB_12);
DigitalOut          nreset(PC_6);   
PwmOut              p1(PB_13);               
PwmOut              p2(PB_14);               


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


/*----------------------- Global Variables ------------------------*/

char                rxData[7];
bool                flagRx = 0;

int                 targetDis;                   // Target Of Distance
int                 milLoop;                     // Number of Millking Action

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

float parA = 73.671f;
float parB = -1*206.25f;
float parC = 106.43f;
/*----------------------- Callback Function Prototypes ------------------------*/
void nFaultLow();
void nFaultHigh();
void ButtonSys();
void ManualMk();
void OperateLED();
void ModeSelect(int mode);
void ReadAng();
void stop();
void ReadData();
void Test();
/*----------------------- Main function ------------------------*/
int main()
{
    bt.baud(115200);
//    pc.baud(115200);


    #ifdef QUALIFYING_MODE
    bt.puts("START 190822 Q_MODE\r\n");
    #else
    bt.puts("START 190822 Ver.9\r\n");
    #endif
    
//    pc.puts("START 190703 Ver.9\r\n");
    int modeNum = 0;   
    int modeMotor = 0;

    char tmpCommand[4]={0,};                                // [ADD] command
    int rxVal;
    
    nFault.mode(PullUp);
    nFault.fall(&nFaultLow);
    nFault.rise(&nFaultHigh);
    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)
    {
        #ifdef QUALIFYING_MODE
        if(Qflag){
            Qflag = false;
            bt.printf("%dus from btn to motor output\r\n",Qtime);
        }
        #endif
     
        if(!nFaultFlag)
        {
            enable = 0;
            nreset = 0;
            p1=0;
            p2=0;
            wait(0.01f);
        }
     
        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")) 
            {   
            bt.puts("<MOD>\r\n");
                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"))
            {           
            bt.puts("<MOT>\r\n");
                modeMotor = rxVal;
                MotorTest(modeMotor);
            }
            
            else if (0 == strcmp(tmpCommand,"POS"))
            {
            bt.puts("<POS>\r\n");
                targetDis = rxVal;               
                timer1.attach(&ControlAng,0.01);
            }
            
            if (0 == strcmp(tmpCommand,"MIL"))
            {
            bt.puts("<MIL>\r\n");
                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;
   
            }
       }
           
        
    }
    
}




/*----------------------- 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;

    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()
{
    #ifdef QUALIFYING_MODE
    Qtimer.reset();
    Qtimer.start();
    Qflag = false;
    #endif
    if( 1 == sysStatus | 2 == sysStatus)
    {
        bt.puts("Please Wait Moving Anybaro");              //<STA> 또는 ifdef debug 처리?
        
    }
    else
    {
        MotorButton();
    }
}

void nFaultHigh(){
    nFaultFlag = true;
}

void nFaultLow(){
    nFaultFlag = false;
}


//---------------------- end -------------