File content as of revision 0:b124e4e3a1e4:

#include "mbed.h"
#include "hallsensor_software_decoder.h"

#define Ts 0.01f    // f to specify float type

Ticker timer1;

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

void init_TIMER();
void init_PWM(); 
void timer1_ITR();

/* wheel 1 */
float rotation_speed_1  = 0.0;
float pwm1_duty         = 0.5;
double PI_out_1         = 0.0;
float err_1             = 0.0;
float ierr_1            = 0.0;

/* wheel 2 */
float rotation_speed_2  = 0.0;
float pwm2_duty         = 0.5;
float PI_out_2          = 0.0;
float err_2             = 0.0;
float ierr_2            = 0.0;

float r = 0.03;                     // wheel radius (m)
float L = 0.27;                     // car width (m)

float rotation_speed_ref_1 = 25.0;
float rotation_speed_ref_2 = -25.0;

float V_ref = 0.0;                  // (m/s) max: 0.25
float V     = 0.0;
float W_ref = 0.0;                  // (rad/s)
float W     = 0.0;


float Kp    = 0.008;
float Ki    = 0.02;

//Serial pc(USBTX, USBRX);


int main()
{
    //pc.baud(9600);
    init_TIMER();
    init_PWM();
    init_CN();
        
    while(1) {
        V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f;
        W = (-2.0f * r * (rotation_speed_1 - rotation_speed_2) / L) * 2.0f * 3.14f / 60.0f;
    /*    pc.printf("motor_1 = %f, motor_2 = %f\r\n", rotation_speed_1,rotation_speed_2);
        pc.printf("V = %f, W = %f\r\n", V,W);*/
        
        wait_ms(100);
    }
}



void init_TIMER() {
    /********************************************************
     * function to be attached (timer1_ITR) and 
     * the interval (10000 micro-seconds) = 0.01 second
     *********************************************************/
    timer1.attach_us(&timer1_ITR, 10000.0); 
}


void init_PWM(){
    /* setting pwm width */
    
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;

    pwm2.period_us(50);
    pwm2.write(0.5);
    TIM1->CCER |= 0x40;
}


void timer1_ITR()
{
    
    //rotation_speed_ref_1 = (V_ref / r - L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f;
    //rotation_speed_ref_2 = (V_ref / r + L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f;
    
    
    /********** MOTOR1 **********/
    
    //measure
    rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f;   //unit: rpm
    // reset
    speed_count_1 = 0;
    
    /************ PI controller for motor1 *************/
    
    err_1 = rotation_speed_ref_1 - rotation_speed_1;
    ierr_1 = ierr_1 + Ts * err_1;
    
    // restrict integrating part
    if(ierr_1 > 50.0f){
        ierr_1 = 50.0;
    } else if(ierr_1 < -50.0f){
        ierr_1 = -50.0;
    }
    
    PI_out_1 = (Kp * err_1 + Ki * ierr_1);
    
    // staturation
    if(PI_out_1 >= 0.5f){
        PI_out_1 = 0.5;
    } else if(PI_out_1 <= -0.5f){
        PI_out_1 = -0.5; 
    }
    /*************************************************/
    
    /* output to acuator */
    pwm1_duty = PI_out_1 + 0.5f;   
    pwm1.write(pwm1_duty);
    TIM1->CCER |= 0x4;
    
   
    
    
   /********* MOTOR2 ***********/
  

    rotation_speed_2 = (float)speed_count_2 * 100.0f / 48.0f * 60.0f / 56.0f;   //unit: rpm
    speed_count_2 = 0;

    /************* PI controller for motor2 ****************/
    err_2 = rotation_speed_ref_2 - rotation_speed_2;
    ierr_2 = ierr_2 + Ts * err_2;
    
    // restrict integration output
    if(ierr_2 > 50.0f){
        ierr_2 = 50.0;
    } else if(ierr_2 < -50.0f){
        ierr_2 = -50.0;
    }
    PI_out_2 = -(Kp * err_2 + Ki * ierr_2) ;
  
    // staturation
    if(PI_out_2 >= 0.5f){
        PI_out_2 = 0.5;
    } else if(PI_out_2 <= -0.5f) {
        PI_out_2 = -0.5;
    }
    /******************************************************/
    /* output to acuator */
    pwm2_duty = PI_out_2 + 0.5f;
    pwm2.write(pwm2_duty);
    TIM1->CCER |= 0x40;
    

}