File content as of revision 1:6228de50cbf4:

#include "mbed.h"

Ticker timer1;
Serial bt(D10, D2);  // TXpin, RXpin

//RX
int readcount = 0;
int RX_flag1 = 0;
int RX_flag2 = 0;
char getData[6] = {0,0,0,0,0,0};
short data_received[3] = {0,0,0};
short data_received_old[3] = {0,0,0};

//函式宣告
void init_TIMER();
void timer1_ITR();
void init_UART();
void RX_ITR();

/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
// servo motor
PwmOut servo_cmd(A0);
// DC motor
PwmOut pwm1(D7);
PwmOut pwm1n(D11);
PwmOut pwm2(D8);
PwmOut pwm2n(A3);

// Motor1 sensor
InterruptIn HallA(A1);
InterruptIn HallB(A2);
// Motor2 sensor
InterruptIn HallA_2(D13);
InterruptIn HallB_2(D12);

void init_CN();
void CN_ITR();
void init_PWM();

// servo motor
float servo_duty = 0.025;  // 0.069 +(0.088/180)*angle, -90<angle<90
// 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
int angle = 0;

// Hall sensor
int HallA_1_state = 0;
int HallB_1_state = 0;
int state_1 = 0;
int state_1_old = 0;
int HallA_2_state = 0;
int HallB_2_state = 0;
int state_2 = 0;
int state_2_old = 0;
int i=0;

// DC motor rotation speed control
int speed_count_1 = 0;
float rotation_speed_1 = 0.0;
float rotation_speed_ref_1 = 0;
float pwm1_duty = 0.5;
float PI_out_1 = 0.0;
float err_1 = 0.0;
float ierr_1 = 0.0;
int speed_count_2 = 0;
float rotation_speed_2 = 0.0;
float rotation_speed_ref_2 = 0;
float pwm2_duty = 0.5;
float PI_out_2 = 0.0;
float err_2 = 0.0;
float ierr_2 = 0.0;
float p_gain=0.015;
float i_gain=0.003;
float duty;

float err_1_old=0;
float err_2_old=0;
bool servo=1;


Serial pc(USBTX,USBRX);
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
int main()
{
     pc.baud(9600);
    init_TIMER();
    init_UART();
    init_PWM();
    init_CN();
    while(1) {
    }
}

void init_TIMER()
{
    timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds)
}

void init_UART()
{
    bt.baud(115200);  // baud rate設為115200
    bt.attach(&RX_ITR, Serial::RxIrq);  // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
}

void timer1_ITR()
{
    // 避免收到錯誤資料，若超出設定範圍則用上次的資料
    if(data_received[0]>300 || data_received[0]<-300 || data_received[1]>300 || data_received[1]<-300 || data_received[2]>90 || data_received[0]<-90) {
        data_received[0] = data_received_old[0];
        data_received[1] = data_received_old[1];
        data_received[2] = data_received_old[2];
    } else {
        data_received_old[0] = data_received[0];
        data_received_old[1] = data_received[1];
        data_received_old[2] = data_received[2];
    }

    /*
    servo=1;
    while(servo==1){
       if(i==0 || i==100 || i==200 || i==300 || i==400 || i==500 || i==600){
      
        duty=0.113f-0.0000733f*(float)i;
  
        }
        //pc.printf("duty= %f ,\n",duty);
        //servo_cmd.period_ms(20);
        servo_cmd.write( data_received_old[2]);
        servo=0;
        i=i+1;
     }
     if(i==601){
        i=0; 
    }
     
     
    */
    /////////////////////////
    
    if(servo_duty >= 0.113f)servo_duty = 0.113;
    else if(servo_duty <= 0.025f)servo_duty = 0.025;
    servo_cmd.write(servo_duty);

    // motor1
    rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    speed_count_1 = 0;

    ///PI controller for motor1///
    
    err_1=(data_received_old[0]-rotation_speed_1)*p_gain;
    ierr_1=(err_1_old+err_1)*i_gain;
    PI_out_1=err_1+ierr_1;
    err_1_old=err_1;
    //////////////////////////////
    
    if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
    else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
    pwm1_duty = PI_out_1 + 0.5f;
    pwm1.write(pwm1_duty);
    TIM1->CCER |= 0x4;

    //motor2
    rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    speed_count_2 = 0;

    ///PI controller for motor2///
    
     err_2=(data_received_old[1]-rotation_speed_2)*p_gain;
     ierr_2=(err_2_old+err_2)*i_gain;
     PI_out_2=err_2+ierr_2;
     err_2_old=err_2;
    //////////////////////////////
    
    if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
    else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
    pwm2_duty = PI_out_2 + 0.5f;
    pwm2.write(pwm2_duty);
    TIM1->CCER |= 0x40;
    
    //pc.printf("SPEED= %f,/n",data_received_old[1]);
    
}

void RX_ITR()
{
    while(bt.readable()) {
        static char uart_read;
        uart_read = bt.getc();
        if(uart_read == 127 && RX_flag1 == 1) {
            RX_flag2 = 1;
        } else {
            RX_flag1 = 0;
        }

        if(RX_flag2 == 1) {
            getData[readcount] = uart_read;
            readcount++;
            if(readcount >= 5) {
                readcount = 0;
                RX_flag2 = 0;
                ///code for decoding///
                data_received[0] = (getData[2] << 8) | getData[1]; 
                data_received[1] = (getData[4] << 8) | getData[3];
                data_received[2] = getData[5];
                ///////////////////////
            }
        } 
        else if(uart_read == 254 && RX_flag1 == 0) {
            RX_flag1 = 1;
        }
    }
}
    
///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////
void init_PWM()
{
    servo_cmd.period_ms(20);
    servo_cmd.write(servo_duty);

    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()
{
    HallA.rise(&CN_ITR);
    HallA.fall(&CN_ITR);
    HallB.rise(&CN_ITR);
    HallB.fall(&CN_ITR);

    HallA_2.rise(&CN_ITR);
    HallA_2.fall(&CN_ITR);
    HallB_2.rise(&CN_ITR);
    HallB_2.fall(&CN_ITR);
}

void CN_ITR()
{
    // motor1
    HallA_1_state = HallA.read();
    HallB_1_state = HallB.read();

    ///code for state determination///
    if(state_1==1 && state_1_old==1){
        if(HallA_1_state==1 && HallB_1_state==0){
                speed_count_1=speed_count_1 + 1;
            }
        else if(HallA_1_state==0 && HallB_1_state==1){
                speed_count_1=speed_count_1 - 1;
            }
        }
    if(state_1==1 && state_1_old==0){
        if(HallA_1_state==0 && HallB_1_state==0){
                speed_count_1=speed_count_1 + 1;
            }
        else if(HallA_1_state==1 && HallB_1_state==1){
                speed_count_1=speed_count_1 - 1;
            }
        }
    if(state_1==0 && state_1_old==0){
        if(HallA_1_state==0 && HallB_1_state==1){
                speed_count_1=speed_count_1 + 1;
            }
        else if(HallA_1_state==1 && HallB_1_state==0){
                speed_count_1=speed_count_1 - 1;
            }
        }
    if(state_1==0 && state_1_old==1){
        if(HallA_1_state==1 && HallB_1_state==1){
                speed_count_1=speed_count_1 + 1;
            }
        else if(HallA_1_state==0 && HallB_1_state==0){
                speed_count_1=speed_count_1 - 1;
            }
        }
    state_1=HallA_1_state;
    state_1_old=HallB_1_state;
    //////////////////////////////////

    //forward : speed_count_1 + 1
    //backward : speed_count_1 - 1


    // motor2
    HallA_2_state = HallA_2.read();
    HallB_2_state = HallB_2.read();

    ///code for state determination///
    if(state_2==1 && state_2_old==1){
        if(HallA_2_state==1 && HallB_2_state==0){
                speed_count_2=speed_count_2 + 1;
            }
        else if(HallA_2_state==0 && HallB_2_state==1){
                speed_count_2=speed_count_2 - 1;
            }
        }
    if(state_2==1 && state_2_old==0){
        if(HallA_2_state==0 && HallB_2_state==0){
                speed_count_2=speed_count_2 + 1;
            }
        else if(HallA_2_state==1 && HallB_2_state==1){
                speed_count_2=speed_count_2 - 1;
            }
        }
    if(state_2==0 && state_2_old==0){
        if(HallA_2_state==0 && HallB_2_state==1){
                speed_count_2=speed_count_2 + 1;
            }
        else if(HallA_2_state==1 && HallB_2_state==0){
                speed_count_2=speed_count_2 - 1;
            }
        }
    if(state_2==0 && state_2_old==1){
        if(HallA_2_state==1 && HallB_2_state==1){
                speed_count_2=speed_count_2 + 1;
            }
        else if(HallA_2_state==0 && HallB_2_state==0){
                speed_count_2=speed_count_2 - 1;
            }
        }
    state_2=HallA_2_state;
    state_2_old=HallB_2_state;


    //////////////////////////////////

    //forward : speed_count_2 + 1
    //backward : speed_count_2 - 1
}