File content as of revision 15:e5e34512a00e:

 #include "mbed.h"

extern                      Serial bt;
extern                      Serial pc;

//---------------------- testing ------------- 190703 BH
//DigitalOut                  enable(PB_12);
//DigitalOut                  nreset(PC_6);   
//PwmOut                      p1(PB_13);               
//PwmOut                      p2(PB_14);               
// -> changed like below
extern DigitalOut                  enable;
extern DigitalOut                  nreset;   
extern PwmOut                      p1;               
extern PwmOut                      p2;               
//---------------------- end ------------- 190703 BH

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;


void MotorButton()
{
        mkOn=0;
        milLoop = 0;
//        targetDis = 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;
                
                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;
            }
            
            case 3 : // stop
            
                enable = 0;
                nreset = 0;
                p1 = 0;
                p2 = 0;
            
                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;
//        float kp = ref_d/(errorPrevious);
        
        if(ref_d > 0.3f)
        {
                enable = 1;
                nreset = 1;
                p1 = 0;
                p2 = 1;
//                p2 = (kp<0.4?0.4:kp);
//                p2 = (ref_d>1?1:ref_d);
                
        }
        
        else if(ref_d < -0.3f)
        {
                
                enable = 1;
                nreset = 1;
//                p1 = -1*(ref_d<-1?-1:ref_d);
                p1 = 1;
//                p1 = -1*(kp>0.4?0.4:kp);
                p2 = 0;
        }
        
        else
        {
                enable = 0;
                nreset = 0;
                timer1.detach();
                  
        }
}


void MkAction()
{
//    bt.putc('3');

    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);
    
    bt.printf("%dth: %1.4f\r\n",onewayNum,s);

    if(!nFaultFlag)
    {
        return;
//        enable = 0;
//        nreset = 0;
//        p1=0;
//        p2=0;
    }
    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; //Hbridge ouput reset! 190627 nreset problem
        
        if(onewayNum >=milLoop*2)
            {
                onewayNum = 0;
                milLoop = 0;
                enable = 0;
                nreset = 0;
                timer3.detach();

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

//                bt.putc('<');
//                bt.putc('M');
//                bt.putc('E');
//                bt.putc('>');
//                bt.putc('\r');          
//                bt.putc('\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);
        }


}