File content as of revision 9:85cbef9febe7:

/*DCMotor*/ // 10400/s+21.28
#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.02f    //period of timer1 (s)
#define Kp1 0.0048   //0.0085f
#define Kp2 0.0048   //0.0085f
#define Ki 0.1023//0.008f

Serial bluetooth(D10,D2); //宣告藍牙腳位

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);


Ticker timer1;
void timer1_interrupt(void);
int  timer1_counter;

void CN_interrupt(void);

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

char speedCmd;

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;

int v1Count = 0;
int v2Count = 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 v1_old[10] = {}, v1_avg = 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 v2_old[10] = {}, v2_avg = 0.0;
int main() {
    
    init_BLUETOOTH();
    init_TIMER();
    init_PWM();
    init_CN();
    
    v1_ref = 0.0;
    v2_ref = 0.0;
    
    while(1) 
    {
        if(bluetooth.readable())
        {
           speedCmd = bluetooth.getc();
            
            switch(speedCmd)
            {
                case '1':
                    v1_ref = 17.0f;
                    v2_ref = -17.0f;
                    break;
                case '2':
                    v1_ref = 50.0f;
                    v2_ref = -50.0f;
                    break;
                case '3':
                    v1_ref = 86.0f;
                    v2_ref = -86.0f;
                    break;
                case '4':
                    v1_ref = 120.0f;
                    v2_ref = -120.0f;
                    break;
                case '5':
                    v1_ref = 153.0f;
                    v2_ref = -153.0f;
                    break;
                case '6':
                    v1_ref = 187.0f;
                    v2_ref = -187.0f;
                    break;
                case '7':
                    v1_ref = -20.0f;
                    v2_ref = 20.0f;
                    break;
                case '8':
                    v1_ref = -80.0f;
                    v2_ref = 80.0f;
                    break;
                case '9':
                    v1_ref = -120.0f;
                    v2_ref = 120.0f;
                    break;
            }
        }
    }
}

void timer1_interrupt(void)
{
    //Motor 1
    v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f;
    for(int i = 9; i > 0 ; i--)
    {
        v1_old[i] = v1_old[i-1];
    }
    v1_old[0] = v1;
    v1Count = 0;
    
    ///code for PI control///
    v1_err = v1_ref - v1;
    v1_ierr += v1_err;
    PIout_1 = Kp1 * v1_err + Ki * Ts * (v1_ierr - v1_err);
    
    // saturation
    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;
    
    PIout_1_old = PIout_1;
    
    //Motor 2
    v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f;
    for(int i = 9; i > 0; i--)
    {
        v2_old[i] = v2_old[i-1];
    }
    v2_old[0] = v2;
    v2Count = 0;
    
    ///code for PI control///
    v2_err = v2_ref - v2;
    v2_ierr += v2_err;
    PIout_2 = Kp2 * v2_err + Ki * Ts * (v2_ierr - v2_err);
    
    //saturation
    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;
    
    PIout_2_old = PIout_2;
    
    timer1_counter ++;
    if (timer1_counter == 50)
    {
       timer1_counter = 0;
       if(bluetooth.writeable())
       {
           bluetooth.printf("V1: %4.2f  V2: %4.2f\n",v1_avg,v2_avg);
       }
    }
}

void CN_interrupt(void)
{
    //Motor 1
    stateA_1 = HallA_1.read();
    stateB_1 = HallB_1.read();
    
    ///code for state determination///
    if (stateA_1 == 0)
    {
        if (stateB_1 == 0)
            state_1 = 0;
        else
            state_1 = 1;
    }
    else
    {
        if (stateB_1 == 1)
            state_1 = 2;
        else
            state_1 = 3;
    }
    
    //Forward: v1Count +1
    //Inverse: v1Count -1
    if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) )
        v1Count++;
    else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0))
        v1Count--;
            
    state_1_old = state_1;
    
    //Motor 2
    stateA_2 = HallA_2.read();
    stateB_2 = HallB_2.read();
    
    ///code for state determination///
    if (stateA_2 == 0)
    {
        if (stateB_2 == 0)
            state_2 = 0;
        else
            state_2 = 1;
    }
    else
    {
        if (stateB_2 == 1)
            state_2 = 2;
        else
            state_2 = 3;
    }
    
    //Forward: v2Count +1
    //Inverse: v2Count -1
    if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) )
        v2Count++;
    else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) )
        v2Count--;
        
    state_2_old = state_2;
}

void init_TIMER(void)
{
    timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz)
    timer1_counter = 0;
}         
   
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 init_BLUETOOTH(void)
{
    bluetooth.baud(115200);
}