File content as of revision 8:2a4298cbae3f:

#include "mbed.h"
#include "QEI.h"
 
extern              Serial bt;
DigitalOut          Enable(A0);
DigitalOut          M1(PA_1);
DigitalOut          M2(PF_1);


extern              QEI re;
extern              Ticker      timer1;
extern int          targetDis;

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

            break;
    
        case 1 : // right
    
            Enable = 1;
            M1 = 1;
            M2 = 0;            
    
            break;
    
        case 2 : // left
        
            Enable = 1;
            M1 = 0;
            M2 = 1;
        
            break;       
            
    }
    
              
}

void ControlPos()
{
        
        int pulse = re.getPulses();
        int d = pulse/200;
       
        float ref_d = (targetDis-d);
        float abs_ref_d = abs(ref_d);
//        float s;
//        s = abs_ref_d; 
        
        if(-0.8f > ref_d)
        {
                Enable = 1;
                M1 = 0;
                M2 = 1; //(s<3)?s/5:1;
                
        }
        
        else if ( 0.8f <ref_d)
        {
                    
                Enable = 1;
                M1 = 1; //(s<3)?s/5:1;
                M2 = 0;
                        
        }
        
        if (abs_ref_d < 0.8f)
        {
            
                Enable = 0;
                timer1.detach();
                  
        }
}