File content as of revision 1:b222972c5930:

#include "mbed.h"


Serial pc(USBTX,USBRX);

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

Ticker timer1;
// 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_IO();
void init_TIMER();
void timer1_ITR();
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;

// DC motor rotation speed control
int speed_count_1 = 0;
float rotation_speed_1 = 0.0;
float rotation_speed_ref_1 = 150;
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 = 150;
float pwm2_duty = 0.5;
float PI_out_2 = 0.0;
float err_2 = 0.0;
float ierr_2 = 0.0;

float Ki = 0.02;
float Kp = 0.7;

int main()
{
 //    FILE *fp = fopen("C:/Users/user/Desktop/out.txt", "w");  // Open "out.txt" on the local file system for writing
    
    
    init_TIMER();
    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 (10000 micro-seconds)
}

void init_PWM()
{
    servo_cmd.period_ms(20);
    servo_cmd.write(servo_duty);

    pwm1.period_us(50);
    pwm1.write(0.5f);
    TIM1->CCER |= 0x4;

    pwm2.period_us(50);
    pwm2.write(0.5f);
    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();
    
    // motor2
    HallA_2_state = HallA_2.read();
    HallB_2_state = HallB_2.read();

    ///code for state determination///
    
    // state determine (hall1) //
    
    if(HallA_1_state==0 && HallB_1_state ==0)
    {
      state_1 = 0;  
       
     }
    
    else if(HallA_1_state==1 && HallB_1_state ==0)
     {
        state_1 = 1;
      }  
       
    else if(HallA_1_state==0 && HallB_1_state ==1)
     {
        state_1 = 2;
      } 
      
    else if(HallA_1_state==1 && HallB_1_state ==1)
     {
        state_1 = 3;
      }  
    //   end state determine  of 1 //
    
    //  foward  and backward determine  of 1// 
    
    switch(state_1)
    {
        case 0 :
        
        if(state_1_old == 1)
        {
            speed_count_1 ++;    // foward
        }
        
        else if(state_1_old == 2)
        {
            speed_count_1 --;  //        reverse
        }
        else{}
        
        break;
        case 1 :
        
        if(state_1_old == 3)
        {
            speed_count_1 ++;    // foward
        }
        
        else if(state_1_old == 0)
        {
            speed_count_1 --;  //        reverse
        }
        
        else{}
        
        break;
        case 2 :
        
        if(state_1_old == 0)
        {
            speed_count_1 ++;    // foward
        }
        
        else if(state_1_old == 3)
        {
            speed_count_1 --;  //        reverse
        }
        
        else{}
        
        break;
        case 3 :
        
        if(state_1_old == 2)
        {
            speed_count_1 ++;    // foward
        }
        
        else if(state_1_old == 1)
        {
            speed_count_1 --;  //        reverse
        }
        
        else{}
        
        break;
        
     }
    
    
    
    //  end  foward  and reverse determine  of 1 //
     
    //  update//
    state_1_old = state_1;
    // end update //
    
    
    
    
    // state determine (hall2) //
    
    
    if(HallA_2_state==0 && HallB_2_state ==0)
    {
      state_2 = 0;  
       
     }
    
    else if(HallA_2_state==1 && HallB_2_state ==0)
     {
        state_2 = 1;
      }  
       
    else if(HallA_2_state==0 && HallB_2_state ==1)
     {
        state_2 = 2;
      } 
      
    else if(HallA_2_state==1 && HallB_2_state ==1)
     {
        state_2 = 3;
      }  
    //   end state determine  of 2//
    
    //  foward  and reverse determine  of 2// 
    
    switch(state_2)
    {
        case 0 :
        
        if(state_2_old == 1)
        {
            speed_count_2 ++;    // foward
        }
        
        else if(state_2_old == 2)
        {
            speed_count_2 --;  //        reverse
        }
        else{}
        
        break;
        case 1 :
        
        if(state_2_old == 3)
        {
            speed_count_2 ++;    // foward
        }
        
        else if(state_2_old == 0)
        {
            speed_count_2 --;  //        reverse
        }
        
        else{}
        
        break;
        case 2 :
        
        if(state_2_old == 0)
        {
            speed_count_2 ++;    // foward
        }
        
        else if(state_2_old == 3)
        {
            speed_count_2 --;  //        reverse
        }
        
        else{}
        
        break;
        case 3 :
        
        if(state_2_old == 2)
        {
            speed_count_2 ++;    // foward
        }
        
        else if(state_2_old == 1)
        {
            speed_count_2 --;  //        reverse
        }
        
        else{}
        
        break;
        
     }
    
    
    
    //  end  foward  and backward determine  of 1 //
     
    //  update//
    state_2_old = state_2;
    // end update //
    
    
    


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

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

void timer1_ITR()
{
    // servo motor
    ///code for sevo motor///
    
    
    
    /////////////////////////
    
    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 = speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    pc.printf("%d\r\n",speed_count_1);
    
    speed_count_1 = 0;

    ///PI controller for motor1///
    err_1 = Kp *(rotation_speed_ref_1 - rotation_speed_1);
    ierr_1 = ierr_1 + Ki*(rotation_speed_ref_1- rotation_speed_1)*0.01;
    
    PI_out_1 = (err_1 + ierr_1)*0.002;
    
    
    //////////////////////////////
    
    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 = float(Kp *(rotation_speed_ref_2 - rotation_speed_2));
    ierr_2 = float(ierr_1 + Ki*(rotation_speed_ref_2- rotation_speed_2)*0.01);
    
    PI_out_2 = float((rotation_speed_2+err_2 + ierr_2)*0.002);
    
    
    //////////////////////////////
    
    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;
    
    
    
}