File content as of revision 19:67584cb64b9c:

#include "mbed.h"

extern Serial               bt;
//extern Serial               pc;


extern DigitalOut           enable;
extern DigitalOut           nreset;   
extern PwmOut               p1;               
extern PwmOut               p2;               

extern InterruptIn          up;
extern InterruptIn          down;

extern AnalogIn             disSensor;

extern Serial               bt;
extern Ticker               timer1;               
extern Ticker               timer3;

extern int                  targetDis;
extern int                  milLoop;
extern int                  onewayNum;
extern bool                 mkOn;
extern float                errorPrevious;

extern float                parA;
extern float                parB;
extern float                parC;

extern bool                 nFaultFlag;

#ifdef QUALIFYING_MODE

extern Timer               Qtimer;                       // timer for qualifying test
extern int                 Qtime;                        // Qtime stores result from Qtimer
extern bool                Qflag;                        // Qflag rises when result is stored

void Qfunc(){
    Qtimer.stop();
    Qtime = Qtimer.read_us();
    Qflag = true;
}
#endif

void MotorButton()
{
        mkOn=0;
        milLoop = 0;
        onewayNum = 0; 
        timer3.detach();
    
        float d1 = disSensor.read();
        float d2 = parA*d1*d1+parB*d1+parC;
        targetDis = d2;                         // setup to target distance in MkActionManual
        
        int a = up.read();
        int b = down.read();
        int c = a*2 + b;
        
        switch(c)
        {
            case 0 : //stop
        
                enable = 0;
                nreset = 0;
                p1 = 0;     
                p2 = 0;
                #ifdef QUALIFYING_MODE
                Qfunc();
                #endif
                break;
        
            case 1 : // up

                enable = 1;
                nreset = 1;
                p1 = 0;
                p2 = 1;
                #ifdef QUALIFYING_MODE
                Qfunc();
                #endif
                break;
        
            case 2 : // down

                enable = 1;
                nreset = 1;
                p1 = 1;
                p2 = 0;
                #ifdef QUALIFYING_MODE
                Qfunc();
                #endif
                break;

            
            case 3 : // stop
            
                enable = 0;
                nreset = 0;
                p1 = 0;
                p2 = 0;
                #ifdef QUALIFYING_MODE
                Qfunc();
                #endif
                break;
                
        }
       
            
    
}

void MotorTest(int mode)
{
    
    timer3.detach();
    
    switch (mode)
    
    {
        case 0 : //stop
            
            enable = 0;
            nreset = 0;
            p1 = 0;     
            p2 = 0;

            break;
    
        case 1 : // up
    
            enable = 1;
            nreset = 1;
            p1 = 0;
            p2 = 1;
    
            break;
    
        case 2 : // down
        
            enable = 1;
            nreset = 1;
            p1 = 1;
            p2 = 0;
        
            break;       
            
    }
    
              
}

void ControlAng()
{
        bt.putc('1');
        float d1 = disSensor.read();
        float d2 = parA*d1*d1+parB*d1+parC;
        float ref_d = targetDis-d2;
        
        if(ref_d > 0.3f)
        {
                enable = 1;
                nreset = 1;
                p1 = 0;
                p2 = 1;
        }
        
        else if(ref_d < -0.3f)
        {
                enable = 1;
                nreset = 1;
                p1 = 1;
                p2 = 0;
        }
        
        else
        {
                enable = 0;
                nreset = 0;
                timer1.detach();
        }
}


void MkAction()
{
    float d1 = disSensor.read();
    float d2 = parA*d1*d1+parB*d1+parC;
    
    static float curTarget;
    float s;
    float abs_error;
    
    curTarget = (onewayNum%2==0)? targetDis: 0.2f ; // initial angle
    
    s = (curTarget-d2)/targetDis;
    abs_error = abs(curTarget-d2);
    
     if(!nFaultFlag)
    {
        p1 = 0;
        p2 = 0;
        return;
    }
    if(s>=0)
    {
        enable = 1;
        nreset = 1;
            p1 = 0;
            p2 = 1;
    }
    
    else
    {
        enable = 1;
        nreset = 1;
            p2 = 0;
            p1 = 1;
    }

    if (abs_error <1.5f)
    {
        onewayNum = onewayNum +1;
        enable = 0; 
        nreset = 0;
        
        if(onewayNum >=milLoop*2)
            {
                onewayNum = 0;
                milLoop = 0;
                enable = 0;
                nreset = 0;
                timer3.detach();

                bt.puts("<ME>\r\n");
            }
    }
}

void MkActionManual()
{
       if( 1 == mkOn)
        {
            mkOn = 0;
            enable = 0;
            nreset = 0;
            timer3.detach();
        }
        
        else
        {
            mkOn = 1;
            onewayNum=0;
            milLoop = 99;
            timer3.attach(&MkAction,0.1);
        }
}