File content as of revision 1:d24c3384bc59:

/* LAB DCMotor */
#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Twist.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)
#define Servo_Period 20
#define pi 3.14159265f
//****************************************************************************** End of Define
 
//****************************************************************************** I/O
//PWM
 
//Dc motor
PwmOut pwm1(D7); 
PwmOut pwm1n(D11);
PwmOut pwm2(D8);
PwmOut pwm2n(A3);
PwmOut servo(A0);
//Motor1 sensor
InterruptIn HallA_R(A1);
InterruptIn HallB_R(A2);
//Motor2 sensor
InterruptIn HallA_L(D13);
InterruptIn HallB_L(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.03; // 0.025~0.113(-90~+90) 0.069->0 degree
// motor right
int8_t HallA_state_R = 0;
int8_t HallB_state_R = 0;
int8_t motor_state_R = 0;
int8_t motor_state_old_R = 0;
int count_R = 0;
float rotation_R = 0.0f;
float rotation_ref_R = 0.0f;
float rotation_err_R = 0.0f;
float rotation_ierr_R = 0.0f;
float ctrl_output_R = 0.0f;
float pwm1_duty = 0.0f;
//motor left
int8_t HallA_state_L = 0;
int8_t HallB_state_L = 0;
int8_t motor_state_L = 0;
int8_t motor_state_old_L= 0;
int count_L = 0;
float rotation_L = 0.0f;
float rotation_ref_L = 0.0f;
float rotation_err_L = 0.0f;
float rotation_ierr_L = 0.0f;
float ctrl_output_L = 0.0f;
float pwm2_duty = 0.0f;
//servo
int i = 0;
//****************************************************************************** End of Variables

//****************************************************************************** ros related function
ros:: NodeHandle nh;

geometry_msgs::Twist vel_msg;
ros::Publisher vel_feedback("feedback_wheel_angularVel", &vel_msg);
float v_right = 0.0f;
float v_left = 0.0f;
void messageCb(const geometry_msgs::Vector3& msg)
{
    v_right = msg.y; //right wheel speed m/s 
    v_left = msg.x; //left wheel speed m/s
    
    rotation_ref_R = -v_right/0.03f*60.0f/(2*pi); //  m/s to RPM
    rotation_ref_L = v_left/0.03f*60.0f/(2*pi); //  m/s to RPM
    //Limit the speed
    /*if(rotation_ref_R > 120.69f)
    {
        rotation_ref_R = 120.69f;   
    }
    else if(rotation_ref_R < -120.69f)
    {
        rotation_ref_R = -120.69;
    }
    if(rotation_ref_L > 120.69f)
    {
        rotation_ref_L = 120.69f;   
    }
    else if(rotation_ref_L < -120.69f)
    {
        rotation_ref_L = -120.69f;
    }*/
}
ros::Subscriber<geometry_msgs::Vector3> cmd_sub("cmd_wheel_angularVel", messageCb);


//****************************************************************************** End of ros related function
//****************************************************************************** Main
int main()
{
    init_timer();
    init_PWM();
    init_CN();
    
    nh.initNode();
    
    //cmd_sub = nh.subscribe("cmd_wheel_angularVel",10, messageCb);
    nh.subscribe(cmd_sub);
    nh.advertise(vel_feedback);
    
    while(1)
    {
        vel_msg.linear.x = rotation_ref_R;
        vel_msg.linear.y = rotation_R;
        vel_msg.linear.z = 0.0f;
          
        vel_msg.angular.x = rotation_ref_L;
        vel_msg.angular.y = rotation_L;
        vel_msg.angular.z = 0.0f;
        
        vel_feedback.publish(&vel_msg);
        nh.spinOnce();
        wait_ms(100);
    }
}
//****************************************************************************** End of Main
 
//****************************************************************************** timer_interrupt
void timer_interrupt()
{   
    // Motor R
    rotation_R = (float)count_R * 100.0f / 12.0f * 60.0f / 29.0f; //17.24 hall count convert to RPM
    count_R = 0;
    // Code for PI controller //
    rotation_err_R = rotation_ref_R - rotation_R;
    rotation_ierr_R += (rotation_err_R*Ts);
    ctrl_output_R = 0.01f*rotation_err_R+ 0.2f*rotation_ierr_R; 
    ///////////////////////////
    
    if(ctrl_output_R >= 0.5f)ctrl_output_R = 0.5f;
    else if(ctrl_output_R <= -0.5f)ctrl_output_R = -0.5f;
    pwm1_duty = ctrl_output_R + 0.5f;
    pwm1.write(pwm1_duty);
    TIM1->CCER |= 0x4;
    
    // Motor L
    rotation_L = (float)count_L * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
    count_L = 0;
    // Code for PI controller //
    rotation_err_L = rotation_ref_L - rotation_L;
    rotation_ierr_L += (rotation_err_L*Ts);
    ctrl_output_L = 0.01f*rotation_err_L + 0.2f*rotation_ierr_L;
    ///////////////////////////      
    if(ctrl_output_L >= 0.5f)ctrl_output_L = 0.5f;
    else if(ctrl_output_L <= -0.5f)ctrl_output_L = -0.5f;
    pwm2_duty = ctrl_output_L + 0.5f;
    pwm2.write(pwm2_duty);
    TIM1->CCER |= 0x40;
    /*
    if(rotation_ierr_R > 100 || rotation_ierr_R < -100)
    {
        rotation_ierr_R = 0;
    }
    if(rotation_ierr_L > 100 || rotation_ierr_L < -100)
    {
        rotation_ierr_L = 0;
    } */   
    //Servo
    if(i==100) 
    {
        if(servo_duty < 0.07f)
        {
            servo_duty = servo_duty+0.04f/6;
        }
        else
        {
            servo_duty = 0.07f;            
        }
        servo.write(servo_duty);
        i=0;
    }
    else
    {
        i++;
    }

}
//****************************************************************************** End of timer_interrupt
 
//****************************************************************************** CN_interrupt
void CN_interrupt()
{
    // Motor Right
    // Read the current status of hall sensor
    HallA_state_R = HallA_R.read();
    HallB_state_R = HallB_R.read();
     
   ///code for state determination///
    if(HallA_state_R == 0 && HallB_state_R == 0)
        motor_state_R = 1;
    else if(HallA_state_R == 0 && HallB_state_R == 1)
        motor_state_R = 2;
    else if(HallA_state_R == 1 && HallB_state_R == 1)
        motor_state_R = 3;
    else if(HallA_state_R == 1 && HallB_state_R == 0)
        motor_state_R = 4;
    
    if(motor_state_old_R != 0)
    {
        if(motor_state_old_R < motor_state_R)
        {
            count_R += 1;
            if(motor_state_old_R == 1 && motor_state_R == 4)
                count_R -= 2;
        }
        else if(motor_state_old_R > motor_state_R)
        {
            count_R -= 1;
            if(motor_state_old_R == 4 && motor_state_R == 1)
                count_R += 2;
        }
    }
    motor_state_old_R = motor_state_R;
    //////////////////////////////////

    // Motor Left
    // Read the current status of hall sensor
    HallA_state_L = HallA_L.read();
    HallB_state_L = HallB_L.read();
 
    ///code for state determination///
    if(HallA_state_L == 0 && HallB_state_L == 0)
        motor_state_L = 1;
    else if(HallA_state_L == 0 && HallB_state_L == 1)
        motor_state_L = 2;
    else if(HallA_state_L == 1 && HallB_state_L == 1)
        motor_state_L = 3;
    else if(HallA_state_L == 1 && HallB_state_L == 0)
        motor_state_L = 4;
    
    if(motor_state_old_L != 0)
    {
        if(motor_state_old_L < motor_state_L)
        {
            count_L += 1;
            if(motor_state_old_L == 1 && motor_state_L == 4)
                count_L -= 2;
        }
        else if(motor_state_old_L > motor_state_L)
        {
            count_L -= 1;
            if(motor_state_old_L == 4 && motor_state_L == 1)
                count_L += 2;
        } 
    }
    motor_state_old_L = motor_state_L;
    //////////////////////////////////
}
//****************************************************************************** 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;
    
    servo.period_ms(Servo_Period);
    servo.write(servo_duty);
}
//****************************************************************************** End of init_PWM
 
//****************************************************************************** init_CN
void init_CN()
{
    // Motor Right
    HallA_R.rise(&CN_interrupt);
    HallA_R.fall(&CN_interrupt);
    HallB_R.rise(&CN_interrupt);
    HallB_R.fall(&CN_interrupt);
    
    HallA_state_R = HallA_R.read();
    HallB_state_R = HallB_R.read();
    
    // Motor Left
    HallA_L.rise(&CN_interrupt);
    HallA_L.fall(&CN_interrupt);
    HallB_L.rise(&CN_interrupt);
    HallB_L.fall(&CN_interrupt);
    
    HallA_state_L = HallA_L.read();
    HallB_state_L = HallB_L.read();
}
//****************************************************************************** End of init_CN