File content as of revision 4:bf4ad2079096:

#include "mbed.h"

extern                      Serial bt;
extern                      Serial pc;

DigitalOut                  enable(PB_12);           
PwmOut                      p1(PB_13);               
PwmOut                      p2(PB_14);               

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;


#define parA 347.44f        // parmeter of equation (values of distance sensor)
#define parB -1*481.16f
#define parC 155.42f


void MotorButton()
{
        
        milLoop = 0;
        targetDis = 0;
        onewayNum = 0; 
        timer3.detach(); 
        
        int a = up.read();
        int b = down.read();
        int c = a*2 + b;
        
        switch(c)
        {
            case 0 : //stop
        
                enable = 0;
                p1 = 0;     
                p2 = 0;
                break;
        
            case 1 : // up
        
                enable = 1;
                p1 = 0;
                p2 = 1;
        
                break;
        
            case 2 : // down
            
                enable = 1;
                p1 = 1;
                p2 = 0;
            
                break;
            
            case 3 : // stop
            
                enable = 0;
                p1 = 0;
                p2 = 0;
            
                break;
                
        }
       
            
    
}

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

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

void ControlAng()
{
        
        float d1 = disSensor.read();
        float d2 = parA*d1*d1+parB*d1+parC;
        float ref_d = targetDis-d2;
        
        if(ref_d > 0.9f)
        {
                enable = 1;
                p1 = 0;
                p2 = 1;
                
        }
        
        else if(ref_d < -0.9f)
        {
                    
                enable = 1;
                p1 = 1;
                p2 = 0;
                     
        }
        
        else
        {
                enable = 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: 1.2f ; // initial angle
    
    s = (curTarget-d2)/targetDis;
    abs_error = abs(curTarget-d2);
    
      
    if(s>=0)
    {
        enable = 1;
            p1 = 0;
            p2 = 1; //(s>0.7f)?s:0.7f;
    }
    
    else
    {
        enable = 1;
            p1 = 1; //(s<-0.7f)?(-1*s):0.7f;
            p2 = 0;
    }
    
    if (abs_error <0.5f)
    {
          
        onewayNum = onewayNum +1;
        
        if(onewayNum >=milLoop*2)
            {
                onewayNum = 0;
                milLoop = 0;
                enable = 0;
                timer3.detach();             
            }
    }
    
    
}

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

        enable = 0;
        timer3.detach();
        onewayNum=0; 
        targetDis = d2;
        milLoop = 99;
        timer3.attach(&MkAction,0.1);

}