File content as of revision 3:75845d88357a:

/* LAB DCMotor */
#include "mbed.h"
 
//****************************************************************************** Define
//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)
 
//****************************************************************************** End of Define
 
//****************************************************************************** I/O
//PWM
//Dc motor
PwmOut pwm1(D7);
PwmOut pwm1n(D11);
PwmOut pwm2(D8);
PwmOut pwm2n(A3);
 
//Motor1 sensor
InterruptIn HallA_1(A1);
InterruptIn HallB_1(A2);
//Motor2 sensor
InterruptIn HallA_2(D13);
InterruptIn HallB_2(D12);
 
//LED
DigitalOut led1(A4);
DigitalOut led2(A5);
 
//Timer Setting
Ticker timer;
//****************************************************************************** End of I/O
 
//****************************************************************************** Functions
void init_timer(void);
void init_CN(void);
void init_PWM(void);
void timer_interrupt(void);
void CN_interrupt(void);
//****************************************************************************** End of Functions
 
//****************************************************************************** Variables
// Servo
float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree

// motor 1
int8_t HallA_state_1 = 0;
int8_t HallB_state_1 = 0;
int8_t motor_state_1 = 0;
int8_t motor_state_old_1 = 0;
int count_1 = 0;
float speed_1 = 0.0f;
float v_ref_1 = 80.0f;
float v_err_1 = 0.0f;     //   v_err_old_1 = v_err_1 ;  v_err_1 = v_ref_1 - speed_1 ;
float v_ierr_1 = 0.0f;   //integral error : v_ierr_1 = v_err_old_1 + v_err_1;
float ctrl_output_1 = 0.0f;
float pwm1_duty = 0.0f;

float Kp_1 = 50;  //need to be tested
float Ki_1 = 100;  //need to be tested
float ctrl_output_old_1 = 0.0f;


//motor 2
int8_t HallA_state_2 = 0;
int8_t HallB_state_2 = 0;
int8_t motor_state_2 = 0;
int8_t motor_state_old_2 = 0;
int count_2 = 0;
float speed_2 = 0.0f;
float v_ref_2 = 150.0f;
float v_err_2 = 0.0f;
float v_ierr_2 = 0.0f;
float ctrl_output_2 = 0.0f;
float pwm2_duty = 0.0f;


float Kp_2 = 50;
float Ki_2 = 100;
float ctrl_output_old_2 = 0.0f;


//****************************************************************************** End of Variables
 
 
//****************************************************************************** Main
int main()
{
    init_timer();
    init_PWM();
    init_CN();
    while(1)
    {
    }
}
//****************************************************************************** End of Main
 
//****************************************************************************** timer_interrupt
void timer_interrupt()
{   
    // Motor1
    speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm of output wheel (period=0.01 sec, each period has 12 segments, reduction ratio 29)
    count_1 = 0;
    // Code for PI controller //
    
      
    v_err_1 = v_ref_1 - speed_1 ;
    
    ctrl_output_1 = (ctrl_output_old_1) + (Kp_1+Ki_1*Ts*0.5)*(v_err_1) + (-Kp_1+Ki_1)*0.5*(v_ierr_1);
    
    v_ierr_1 =  v_err_1 ;
    ctrl_output_old_1 = ctrl_output_1;

    
    ///////////////////////////
    
    if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
    else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
    pwm1_duty = ctrl_output_1 + 0.5f;
    pwm1.write(pwm1_duty);
    TIM1->CCER |= 0x4;
    
    
    
    // Motor2
    speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
    count_2 = 0;
    // Code for PI controller //
      
    v_err_2 = v_ref_2 - speed_2 ;
    
    ctrl_output_2 = (ctrl_output_old_2) + (Kp_2+Ki_2*Ts*0.5)*(v_err_2) + (-Kp_2+Ki_2)*0.5*(v_ierr_2);
    
    v_ierr_2 =  v_err_2 ;
    ctrl_output_old_2 = ctrl_output_2;
    
    ///////////////////////////    
    
    if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
    else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
    pwm2_duty = ctrl_output_2 + 0.5f;
    pwm2.write(pwm2_duty);
    TIM1->CCER |= 0x40;
    
}
//****************************************************************************** End of timer_interrupt
 
//****************************************************************************** CN_interrupt
void CN_interrupt()
{
    // Motor1
    // Read the current status of hall sensor
    HallA_state_1 = HallA_1.read();
    HallB_state_1 = HallB_1.read();
     
   ///code for state determination///
    if(HallA_state_1==0)
    {
        if(HallB_state_1==0)
        {
        motor_state_1 =1;
        }
        else
        {
         motor_state_1 =2;   
            }
        }      
    else
    {
        if(HallB_state_1==0)
        {
            motor_state_1 = 3;
            }
            else
            {
                motor_state_1 = 4;
                }
        }
         
    
    //////////////////////////////////
    switch(motor_state_1)
    {
        case '1':
            if(motor_state_old_1 == 4) 
            count_1--;           
            else if(motor_state_old_1 == 2) 
            count_1++;  
            break; 
        case '2':
            if(motor_state_old_1 == 1) 
            count_1--;                 
            else if(motor_state_old_1 == 3) 
            count_1++;  
            break; 
        case '3':
            if(motor_state_old_1== 2)  
            count_1--;                 
            else if(motor_state_old_1 == 4) 
            count_1++;  
            break; 
        case '4':
            if(motor_state_old_1 == 3) 
            count_1--;  
            else if(motor_state_old_1 == 1) 
            count_1++;  
            break; 
    }
    motor_state_old_1 = motor_state_1;
    //Forward
    //v1Count +1
    //Inverse
    //v1Count -1
        
    // Motor2
    // Read the current status of hall sensor
    HallA_state_2 = HallA_2.read();
    HallB_state_2 = HallB_2.read();
     
    ///code for state determination///
   if(HallA_state_2==0)
    {
        if(HallB_state_2==0)
        {
        motor_state_2 =1;
        }
        else
        {
         motor_state_2 =2;   
            }
        }        
    else
    {
        if(HallB_state_2==0)
        {
            motor_state_2 = 3;
            }
            else
            {
                motor_state_2 = 4;
                }
        }
      
    //////////////////////////////////
        switch(motor_state_2)
    {
        case 1:
            if(motor_state_old_2 == 4) 
            count_2--;           
            else if(motor_state_old_2 == 2) 
            count_2++;  
            break; 
        case 2:
            if(motor_state_old_2 == 1) 
            count_1--;                 
            else if(motor_state_old_2 == 3) 
            count_2++;  
            break; 
        case 3:
            if(motor_state_old_2== 2)  
            count_2--;                 
            else if(motor_state_old_2 == 4) 
            count_2++;  
            break; 
        case 4:
            if(motor_state_old_2 == 3) 
            count_2--;  
            else if(motor_state_old_2 == 1) 
            count_2++;  
            break; 
    }
    motor_state_old_2 = motor_state_2;
    
    //Forward
    //v2Count +1
    //Inverse
    //v2Count -1
}
//****************************************************************************** End of CN_interrupt
 
//****************************************************************************** init_timer
void init_timer()
{
     timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
}
//****************************************************************************** End of init_timer
 
//****************************************************************************** init_PWM
void init_PWM()
{
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
    
    pwm2.period_us(50);
    pwm2.write(0.5);
    TIM1->CCER |= 0x40;
}
//****************************************************************************** End of init_PWM
 
//****************************************************************************** init_CN
void init_CN()
{
    // Motor1
    HallA_1.rise(&CN_interrupt);
    HallA_1.fall(&CN_interrupt);
    HallB_1.rise(&CN_interrupt);
    HallB_1.fall(&CN_interrupt);
    
    HallA_state_1 = HallA_1.read();
    HallB_state_1 = HallB_1.read();
    
    // Motor2
    HallA_2.rise(&CN_interrupt);
    HallA_2.fall(&CN_interrupt);
    HallB_2.rise(&CN_interrupt);
    HallB_2.fall(&CN_interrupt);
    
    HallA_state_2 = HallA_2.read();
    HallB_state_2 = HallB_2.read();
}
//****************************************************************************** End of init_CN