File content as of revision 0:68d5289b45cc:

/* 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 PI    3.145926

// PID parameter
#define Kp_1  2.0f
#define Ki_1  0.0f
#define Kd_1  0.0f

#define Kp_2  3.0f
#define Ki_2  0.0f
#define Kd_2  0.0f

//****************************************************************************** End of Define
 
//****************************************************************************** I/O
//PWM
//Dc motor
PwmOut pwm1(D11);
PwmOut pwm1n(D7);
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
// decoder
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;

// velocity
int count_1 = 0;
float speed_1 = 0.0f;

// control

// u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
float Ka_1 = Kp_1 + Ki_1 + Kd_1;
float Kb_1 = -Kp_1 - 2*Kd_1;
float Kc_1 = Kd_1;

float v_ref_1 = 0.0f;           // r(k)
float v_err_1 = 0.0f;           // e(k) = r(k) - v(k)
float v_err_old_1 = 0.0f;       // e(k-1)
float v_err_older_1 = 0.0f;     // e(k-2)
float ctrl_output_1 = 0.0f;     // u(k)
float ctrl_output_old_1 = 0.0f; // u(k-1)
float pwm1_duty = 0.0f;         // duty = u(k) + 0.5


// motor 2
// decoder
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;

// velocity
int count_2 = 0;
float speed_2 = 0.0f;

// control

// u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
float Ka_2 = Kp_2 + Ki_2 + Kd_2;
float Kb_2 = -Kp_2 - 2*Kd_2;
float Kc_2 = Kd_2;

float v_ref_2 = 0.0f;           // r(k)
float v_err_2 = 0.0f;           // e(k) = r(k) - v(k)
float v_err_old_2 = 0.0f;       // e(k-1)
float v_err_older_2 = 0.0f;     // e(k-2)
float ctrl_output_2 = 0.0f;     // u(k)
float ctrl_output_old_2 = 0.0f; // u(k-1)
float pwm2_duty = 0.0f;         // duty = u(k) + 0.5




//****************************************************************************** End of Variables
 
//****************************************************************************** ROS
/*
 * **System Structure**
 *
 *  remote_car_node
 *    pkg : robotics
 *    pub : "/cmd_ang_vel"                  (Vector3)
 *    sub : "/turtlebot_teleop/cmd_vel"     (Twist)
 *
 *  turtlebot_teleop_key
 *    pkg : turtlebot_teleop
 *    pub : "/turtlebot_teleop/cmd_vel"     (Twist)
 *    sub : --
 *
 *  STM
 *    pkg :
 *    pub : "/feedback_wheel_ang_vel"       (Twist)
 *    sub : "/cmd_ang_vel"                  (Vector3)
 *
 */


// create a ROS node handle  
ros::NodeHandle n;

// pub a topic "/feedback_wheel_ang_vel"
// with type "Twist"
geometry_msgs::Twist wheel_ang_vel;
ros::Publisher pub("/feedback_wheel_ang_vel", &wheel_ang_vel);

// sub a topic "/cmd_ang_vel"
// with type "Vector3"
void messageCb(const geometry_msgs::Vector3 &cmd_received)
{
    v_ref_1 = cmd_received.x;
    v_ref_2 = cmd_received.y;
}
ros::Subscriber<geometry_msgs::Vector3> sub("/cmd_ang_vel", messageCb);


//****************************************************************************** End of ROS
 
//****************************************************************************** Main
int main()
{
    init_PWM();
    init_timer();
    init_CN();
    
    n.initNode();
    n.subscribe(sub);
    n.advertise(pub);
    
    while(1)
    {
        wheel_ang_vel.linear.x = v_ref_1;
        wheel_ang_vel.linear.y = speed_1;
        wheel_ang_vel.linear.z = 0.0f;
        
        wheel_ang_vel.angular.x = v_ref_2;
        wheel_ang_vel.angular.y = speed_2;
        wheel_ang_vel.angular.z = 0.0f;
    
        pub.publish(&wheel_ang_vel);
        n.spinOnce();
        wait_ms(500);
    }
}
//****************************************************************************** End of Main
 
 
 //***************************************************************************** init_timer
void init_timer()
{
     timer.attach_us(&timer_interrupt, 100000);//100ms interrupt period (10 Hz)
}
//****************************************************************************** End of init_timer


//****************************************************************************** timer_interrupt
void timer_interrupt()
{
    
    // Motor1
    // (period=0.01 sec, each period has 12 segments, reduction ratio 29)
    speed_1 = (float)count_1 * 100.0f / 12.0f * 2 * PI / 29.0f;  // rad/s
    count_1 = 0;
    
    
    // Code for PID controller //
    
    
    v_err_1 = v_ref_1 - speed_1 ;       // e(k) = r(k) - v(k)
    
    //    u(k)    =       u(k-1)      +   Ka*e(k)    +   Kb*e(k-1)      +    Kc*e(k-2)
    ctrl_output_1 = ctrl_output_old_1 + Ka_1*v_err_1 + Kb_1*v_err_old_1 + Kc_1*v_err_older_1;
    
    v_err_older_1 = v_err_old_1;        // e(k-1)
    v_err_old_1 = v_err_1;              // e(k-2)
    ctrl_output_old_1 = ctrl_output_1;  // u(k-1)
   
 
    // limitter of ctrl_output
    
    if(ctrl_output_1 >= 0.5f)
    {
        ctrl_output_1 = 0.5f;
    }
    else if(ctrl_output_1 <= -0.5f)
    {
        ctrl_output_1 = -0.5f;
    }
    
    // convert crtl_output to PWM signal
    pwm1_duty = ctrl_output_1 + 0.5f;
    pwm1.write(pwm1_duty);
    TIM1->CCER |= 0x4;
    
    
    // Motor2
    // (period=0.01 sec, each period has 12 segments, reduction ratio 29)
    speed_2 = (float)count_2 * 100.0f / 12.0f * 2 * PI / 29.0f;  // rad/s
    count_2 = 0;
    
    
    // Code for PID controller //
    
    
    v_err_2 = v_ref_2 - speed_2 ;       // e(k) = r(k) - v(k)
    
    //    u(k)    =       u(k-1)      +   Ka*e(k)    +   Kb*e(k-1)      +    Kc*e(k-2)
    ctrl_output_2 = ctrl_output_old_2 + Ka_2*v_err_2 + Kb_2*v_err_old_2 + Kc_2*v_err_older_2;
    
    v_err_older_2 = v_err_old_2;        // e(k-1)
    v_err_old_2 = v_err_2;              // e(k-2)
    ctrl_output_old_2 = ctrl_output_2;  // u(k-1)
   
 
    // limitter of ctrl_output
    
    if(ctrl_output_2 >= 0.5f)
    {
        ctrl_output_2 = 0.5f;
    }
    else if(ctrl_output_2 <= -0.5f)
    {
        ctrl_output_2 = -0.5f;
    }
    
    // convert crtl_output to PWM signal
    pwm2_duty = ctrl_output_2 + 0.5f;
    pwm2.write(pwm2_duty);
    TIM1->CCER |= 0x40;
    
}
//****************************************************************************** End of timer_interrupt
 
//****************************************************************************** CN_interrupt
void CN_interrupt_1()
{
    // 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)
        {// (0,0) -> 1
            motor_state_1 = 1;
        }
        else
        {// (0,1) -> 2
            motor_state_1 = 2;   
        }
    }      
    else
    {
        if(HallB_state_1 == 0)
        {// (1,0) -> 4
            motor_state_1 = 4;
        }
        else
        {// (1,1) -> 3
            motor_state_1 = 3;
        }
    }
         
    
    // determind direction and count hall sensor changed //
    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;
    
}

void CN_interrupt_2()
{
    // 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)
        {// (0,0) -> 1
            motor_state_2 = 1;
        }
        else
        {// (0,1) -> 2
            motor_state_2 = 2;
        }
    }
    else
    {
        if(HallB_state_2 == 0)
        {// (1,0) -> 4
            motor_state_2 = 4;
        }
        else
        {// (1,1) -> 3
            motor_state_2 = 3;
        }
    }
      
    // determind direction and count hall sensor changed //
    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_2++;
            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;
    
}
//****************************************************************************** End of CN_interrupt
 

 
//****************************************************************************** init_PWM
void init_PWM()
{
    // motor 1
    pwm1.period_us(50);
    pwm1.pulsewidth_us(25);
    
    TIM1->CCER |= 0x4;
    
    // motor 2
    pwm2.period_us(50);
    pwm2.pulsewidth_us(25);
   
    TIM1->CCER |= 0x40;
}
//****************************************************************************** End of init_PWM
 
//****************************************************************************** init_CN
void init_CN()
{
    // Motor1
    HallA_1.rise(&CN_interrupt_1);
    HallA_1.fall(&CN_interrupt_1);
    HallB_1.rise(&CN_interrupt_1);
    HallB_1.fall(&CN_interrupt_1);
    
    HallA_state_1 = HallA_1.read();
    HallB_state_1 = HallB_1.read();
    
    // Motor2
    HallA_2.rise(&CN_interrupt_2);
    HallA_2.fall(&CN_interrupt_2);
    HallB_2.rise(&CN_interrupt_2);
    HallB_2.fall(&CN_interrupt_2);
    
    HallA_state_2 = HallA_2.read();
    HallB_state_2 = HallB_2.read();

}
//****************************************************************************** End of init_CN