File content as of revision 3:e4bdbe286f7a:

#include "mbed.h"
 
#define Ts 0.01f    //period of timer1 (s)
#define Kp 0.006f
#define Ki 0.02f

PwmOut mypwm(A0);
PwmOut pwm1(D7);
PwmOut pwm1n(D11);
PwmOut pwm2(D8);
PwmOut pwm2n(A3);
 
DigitalOut led1(A4);
DigitalOut led2(A5);
 
//Motor1 sensor
InterruptIn HallA_1(A1);
InterruptIn HallB_1(A2);
//Motor2 sensor
InterruptIn HallA_2(D13);
InterruptIn HallB_2(D12);
 
Serial bluetooth (D10,D2);
Serial pc(D1,D0);
 
Ticker timer1;
void timer1_interrupt(void);
void CN_interrupt(void);
void BT_interrupt(void);
 
void init_TIMER(void);
void init_PWM(void);
void init_CN(void);
 
int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
int8_t stateA_1_old = 0, stateB_1_old = 0, stateA_2_old = 0, stateB_2_old = 0;
 
int c = 0; 
int d = 0; 
int v1Count = 0;
int v2Count = 0;

/*char Receive_Data;*/
char Receive_Data[10],Temp;
float Position_Error = 0.0,Degree_Error= 0.0;
int Command_Flag,Receive_Counter,Receive_Flag;
float sigma = 0.0,delta = 0.0,fSpeedRef_1 = 0.0,fSpeedRef_2 = 0.0;

 
float v1 = 0.0, v1_ref = 0.0;
float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
float v2 = 0.0, v2_ref = 0.0;
float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
 
float k1 = 0.0, k2 = 0.0, servo_duty = 0.121;

int main() 
{
    init_TIMER();
    init_PWM();
    init_CN();
    
    bluetooth.baud(115200);
    pc.baud(57600);
    bluetooth.attach(&BT_interrupt);
    
    while(1)    
    {
        if(Command_Flag==1)
        {
            Position_Error=(Receive_Data[2]-0x30)*100+(Receive_Data[3]-0x30)*10+(Receive_Data[4]-0x30); 
            
            if(Receive_Data[1]=='-')
                Position_Error=-1*Position_Error;
            else 
                Position_Error=Position_Error; 
                
            Degree_Error=(Receive_Data[6]-0x30)*100+(Receive_Data[7]-0x30)*10+(Receive_Data[8]-0x30);
            
            if(Receive_Data[5]=='-')
                Degree_Error=-1*Degree_Error;
            else 
                Degree_Error=Degree_Error;
                
            if(Receive_Data[9]=='j')
                mypwm.write(0.079);
            else 
                mypwm.write(0.1257);
                          
            
            if  (Position_Error >= 12 || Position_Error <= -12)
                k1 = 2;
            else 
                k1 = 1;
            
            if  (Degree_Error >= 15 || Degree_Error <= -15)
                k2 = 1;
            else
                k2 = 0.5;
            
            
            sigma = k1*Position_Error;
            delta = k2*Degree_Error;
            
            if (sigma > 500){sigma = 500;}
            if (sigma < -500){sigma = -500;}
            if (delta > 500){delta = 500;}
            if (delta < -500){delta = -500;}
            
            
            fSpeedRef_1 = sigma + delta;
            fSpeedRef_2 = sigma - delta;
            v1_ref = fSpeedRef_1;
            v2_ref = -1*fSpeedRef_2;
            
            Command_Flag = 0;
        }
    }
}
 
void timer1_interrupt(void)
{
    //Motor 1
    v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    v1Count = 0;
    
    ///code for PI control///
    
    v1_err = v1_ref - v1;
    v1_ierr = v1_ierr + v1_err*Ts;
    PIout_1 = Kp*v1_err + Ki*v1_ierr;
    
    /////////////////////////
    
    if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
    else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
    pwm1.write(PIout_1 + 0.5f);
    TIM1->CCER |= 0x4;
    
    
    //Motor 2
    v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    v2Count = 0;
    
    ///code for PI control///
    
    v2_err = v2_ref - v2;
    v2_ierr = v2_ierr + v2_err*Ts;
    PIout_2 = Kp*v2_err + Ki*v2_ierr;
    
    /////////////////////////
    
    if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
    else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
    pwm2.write(PIout_2 + 0.5f);
    TIM1->CCER |= 0x40;
    

}
 
void CN_interrupt(void)
{
    //Motor 1
    stateA_1 = HallA_1.read();
    stateB_1 = HallB_1.read();
    
    ///code for state determination///
    
    if(stateA_1 == stateB_1_old && c == 1)
    {
        v1Count = v1Count + 1;
    }
        
    if(stateB_1 == stateA_1_old && c == 1) 
    {
        v1Count = v1Count - 1;
    }
    
    c = 1;
    stateA_1_old = stateA_1;
    stateB_1_old = stateB_1;
    
    //////////////////////////////////
    
    
    //Motor 2
    stateA_2 = HallA_2.read();
    stateB_2 = HallB_2.read();
    
    ///code for state determination///
    
    if(stateA_2 == stateB_2_old && d == 1)
    {
        v2Count = v2Count + 1;
    }
        
    if(stateB_2 == stateA_2_old && d == 1) 
    {
        v2Count = v2Count - 1;
    }
    
    d = 1;
    stateA_2_old = stateA_2;
    stateB_2_old = stateB_2;
    
    //////////////////////////////////
    
}
 
void init_TIMER(void)
{
    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
}         
   
void init_PWM(void)
{
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
    
    pwm2.period_us(50);
    pwm2.write(0.5);
    TIM1->CCER |= 0x40;
    
    mypwm.period_ms(20);
    mypwm.write(servo_duty);
}
 
void init_CN(void)
{
    HallA_1.rise(&CN_interrupt);
    HallA_1.fall(&CN_interrupt);
    HallB_1.rise(&CN_interrupt);
    HallB_1.fall(&CN_interrupt);
    
    HallA_2.rise(&CN_interrupt);
    HallA_2.fall(&CN_interrupt);
    HallB_2.rise(&CN_interrupt);
    HallB_2.fall(&CN_interrupt);
    
    stateA_1 = HallA_1.read();
    stateB_1 = HallB_1.read();
    stateA_2 = HallA_2.read();
    stateB_2 = HallB_2.read();
}

void BT_interrupt(void)
{  
    if(bluetooth.readable())
    {
        Temp = bluetooth.getc(); 
        
        if (Receive_Flag==1)
        {
            Receive_Counter++;
            Receive_Data[Receive_Counter]=Temp;
         
            if(Receive_Counter==9)
            {
                Command_Flag=1;
                Receive_Flag=0;
                Receive_Counter=0;
            }
        }
        else
        {
            if(Temp == 36)
            {
                Receive_Flag = 1;
                Receive_Counter = 0;
                Receive_Data[0]= Temp;
            }
        }
    }
}