File content as of revision 9:b073f4ecb5f9:

/*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.003f
#define Ki 0.01f

PwmOut pwm1(D7);
PwmOut pwm1n(D11);
PwmOut pwm2(D8);
PwmOut pwm2n(A3);

Serial bluetooth(D10,D2);
Serial pc(D1, D0);

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);
void CN_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 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 = 60.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 = -60.0;
float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;

int main() {
    
    init_TIMER();
    init_PWM();
    init_CN();
    
    bluetooth.baud(115200); //設定鮑率
    pc.baud(57600);
    
    
    while(1) 
    {
        if(pc.readable())
        {
            bluetooth.putc(pc.getc());
        }
        if(bluetooth.readable())
        {
            pc.putc(bluetooth.getc());
        }
    }
}

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 = 0.01f*v1_err + v1_ierr;
    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 = 0.01f*v2_err + v2_ierr;
    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;
    
    switch(bluetooth.getc())
    {
    case '1':
        v1_ref = 30;
        v2_ref = -30;
        break;
    case '2':
        v1_ref = 40;
        v2_ref = -40;
        break;
    case '3':
        v1_ref = 50;
        v2_ref = -50;
        break;
    case '4':
        v1_ref = 60;
        v2_ref = -60;
        break;
    case '5':
        v1_ref = -70;
        v2_ref = 70;
        break;
    case '6':
        v1_ref = -80;
        v2_ref = 80;
        break;
    case '7':
        v1_ref = -100;
        v2_ref = 100;
        break;
    case '8':
        v1_ref = 0;
        v2_ref = 0;
        break;            
    }
    
    
}

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

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