File content as of revision 0:9ed7b8e278fa:

/*LAB_DCMotor*/
#include "mbed.h"

//The number will be compiled as type "double" in default
//Add a "f" after the number can make it compiled as type "float"
#define Ts 0.01f    //period of timer1 (s)
#define Kp 0.053f
#define Ki 0.013f

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 bt(D10,D2);

Ticker timer1;
void timer1_interrupt(void);
void CN_interrupt(void);

void init_TIMER(void);
void init_PWM(void);
void init_CN(void);

void set_rpm(void);//bluetooth in

int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
int8_t state_1_dec = 0,state_2_dec = 0;//decide foward or inverse

int v1Count = 0;
int v2Count = 0;

double v1 = 0.0, v1_ref = 0.0;
double v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
double v2 = 0.0, v2_ref = 0.0;
double v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;

int sec_1 = 0, sec_2 = 0;  
//////////command/////////////
char RL;
double char_1;//藍芽讀到的第一位數字元
double char_2;//藍芽讀到的第二位數字元

double a;//個位數
double b;//十位數
//////////////////////////////

int main() {
    
    init_TIMER();
    init_PWM();
    init_CN();
    bt.baud(115200);
    
    v1_ref = 0.0;
    v2_ref = 0.0;
    
    while(1) 
    {
        
        set_rpm();
 
    }
}

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;
    PIout_1 = Kp * v1_err + Ki * Ts * 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;
    PIout_2 = Kp * v2_err + Ki * Ts * 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();
    
    state_1 = stateA_1 * 2 + stateB_1;
    
    state_1_dec = state_1 - state_1_old; //判別正反
    
    
    ///code for state determination///
    if(state_1_dec == 1 || state_1_dec == -3 )      //Forward
        {     
            v1Count++;
        }
        
    else if(state_1_dec == -1 || state_1_dec == 3 ) //Inverse
        {     
            v1Count--;
        }
        
    state_1_old = state_1;
    
    
    //Motor 2
    stateA_2 = HallA_2.read();
    stateB_2 = HallB_2.read();
    
    state_2 = stateA_2 * 2 + stateB_2;
    
    state_2_dec = state_2 - state_2_old; //判別正反
    
    
    ///code for state determination///
    if(state_2_dec == 1 || state_2_dec == -3 )      //Forward
        {     
            v1Count++;
        }
        
    else if(state_2_dec == -1 || state_2_dec == 3 ) //Inverse
        {     
            v1Count--;
        }
        
    state_2_old = state_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;
}

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 set_rpm(void) 
{
    
    RL = bt.getc();
    
    switch( RL ) 
        {
            case 'r':
                bt.printf("%c",RL);
                
                    char_1 = bt.getc();
                    a = char_1 - 48;
                
                if(a > -1 && a < 10)
                {
                    char_2 = bt.getc();
                    b = char_2 - 48;
                    v1_ierr = 0;
                    v1_ref = 10*a+b;
                    
                    bt.printf("%f\n",v1_ref);
                }
                
                else
                {
                    bt.printf("please key in number");
                }
                
                
                while(v1 < v1_ref*0.95 || v1 > v1_ref*1.05 )
                {   
                    bt.printf("state %d ",state_1_dec);
                    bt.printf("%f ",v1);
                    sec_1++;
                    bt.printf("%d\n",sec_1);
                    wait(1);
                }
            sec_1 = 0;
            bt.printf("end %d\n",sec_1);
            
            break;
            /////////////////////////////////
        
            case 'l':
                bt.printf("%c",RL);
                
                    char_1 = bt.getc();
                    a = char_1 - 48;
                
                if(a > -1 && a < 10)
                {
                    char_2 = bt.getc();
                    b = char_2 - 48;
                    v2_ierr = 0;
                    v2_ref = 10*a+b;
                    
                    bt.printf("%f\n",v2_ref);
                }
                
                else
                {
                    bt.printf("please key in number");
                }
                
                 
                while(v2 < v2_ref*0.95 || v2 > v2_ref*1.05 )
                {
                    bt.printf("%f ",v2);
                    sec_2++;
                    bt.printf("%d\n",sec_2);
                    wait(1);
                }
            sec_2 = 0;
            bt.printf("end %d\n",sec_2);
            
            break;
        }
             
}