File content as of revision 1:9238d61200e0:

#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Twist.h>

#define Ts 0.01f    //period of timer1 (s)

Serial pc(USBTX, USBRX);
 
DigitalOut led1(PC_1);

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_TIMER(void);
void timer1_interrupt(void);
// 函式宣告
void init_IO();
void init_CN();
void CN_ITR();
void init_PWM();

// servo motor
float servo_duty = 0.069;  // 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 = 0.0;//150rpm
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 = 0.0;//-80rpm
float pwm2_duty = 0.5;
float PI_out_2 = 0.0;
float err_2 = 0.0;
float ierr_2 = 0.0;
 
//set parameters
float kp = 0.015;
float ki = 0.11;
float pi = 3.14159;

ros::NodeHandle nh;
 
geometry_msgs::Twist vel_msg;
ros::Publisher p("feedback_wheel_angularVel", &vel_msg);

void messageCb(const geometry_msgs::Vector3 &msg){
    //receive velocity command from PC to motor 
    
    geometry_msgs::Vector3 vector3 = msg;
     
    rotation_speed_ref_2 = (vector3.x)/1000*2*pi/60;
    rotation_speed_ref_1 = (vector3.y)/1000*2*pi/60;
    
}
ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel", messageCb);

int main() {
    led1 = 0;
    init_TIMER();
    init_PWM();
    init_CN();
    
    nh.initNode();
    nh.subscribe(s);
    nh.advertise(p);
    
    while(1) {
       vel_msg.linear.x = rotation_speed_ref_2;   //command1
       vel_msg.linear.y = rotation_speed_2;      //respond1
       vel_msg.linear.z = 0;
       
       vel_msg.angular.x = rotation_speed_ref_1;   //command1
       vel_msg.angular.y = rotation_speed_1;      //respond1
       vel_msg.angular.z = 0;
       
       p.publish(&vel_msg);
       nh.spinOnce();
       wait_ms(50);
    }   
}

void timer1_interrupt(){
    //do motor control here
    
    //pc.printf("hello timer1_ITR\n");
    // servo motor
    ///code for sevo motor///
    /////////////////////////
    /////////////////////////   
    //rotating angle for every call
    angle = 15;
    
    //servo_duty output for every call
    servo_duty = servo_duty + 0.088/180*angle/100;
    /////////////////////////
    /////////////////////////
    
    //protection code for servo
    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 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    speed_count_1 = 0;
 
    ///PI controller for motor1///
    //////////////////////////////
    //////////////////////////////
    
    //PI control
    err_1 = rotation_speed_ref_1 - rotation_speed_1;
    ierr_1 += err_1*Ts;
    PI_out_1 = kp * err_1 + ki * ierr_1;
    
    //////////////////////////////
    //////////////////////////////
    
    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;
    //pc.printf("pwm1_duty = %d \n\r", pwm1_duty);
    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///
    //////////////////////////////
    //////////////////////////////
    
    //PI control
    err_2 = rotation_speed_ref_2 - rotation_speed_2;
    ierr_2 += err_2 * Ts;
    PI_out_2 = kp * err_2 + ki * ierr_2;
  
    //////////////////////////////
    //////////////////////////////
    
    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;
    //pc.printf("pwm2_duty = %d \n\r", pwm2_duty);
    pwm2.write(pwm2_duty);
    TIM1->CCER |= 0x40;    
}

void init_TIMER(){
    timer1.attach_us(&timer1_interrupt, 10000);    
}

void init_PWM()
{
    servo_cmd.period_ms(20);
    servo_cmd.write(servo_duty);
 
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
 
    pwm2.period_us(50);
    pwm2.write(0.5);
    TIM1->CCER |= 0x40;
}
void init_CN()
{
    //pc.printf("hello init_CN \n");
    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();
    //led1 != led1; 
 
    ///code for state determination///
    //////////////////////////////////
    //////////////////////////////////
    //determine the state
    if((HallA_1_state == 0)&&(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;
    }
    else if((HallA_1_state == 1)&&(HallB_1_state ==0))
    {
        state_1 = 4;
    }
    
    //forward or backward
    int direction_1 = 0;
    direction_1 = state_1 - state_1_old;
    if((direction_1 == -1) || (direction_1 == 3))
    {
        //forward
        speed_count_1 = speed_count_1 + 1;
    }
    else if((direction_1 == 1) || (direction_1 == -3))
    {
        //backward
        speed_count_1 = speed_count_1 - 1;
    }
    else
    {
        //prevent initializing error
        state_1_old = state_1;
    }
    
    //change old state
    state_1_old = state_1;
    //////////////////////////////////
    //////////////////////////////////
    //forward : speed_count_1 + 1
    //backward : speed_count_1 - 1
 
    // motor2
    HallA_2_state = HallA_2.read();
    HallB_2_state = HallB_2.read();
 
    ///code for state determination///
    //////////////////////////////////
    /////////////////////////////////
    //determine the state
    if((HallA_2_state == 0)&&(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;
    }
    else if((HallA_2_state == 1)&&(HallB_2_state ==0))
    {
        state_2 = 4;
    }
    
    //forward or backward
    int direction_2 = 0;
    direction_2 = state_2 - state_2_old;
    if((direction_2 == 1) || (direction_2 == -3))
    {
        //forward
        speed_count_2 = speed_count_2 + 1;
    }
    else if((direction_2 == -1) || (direction_2 == 3))
    {
        //backward
        speed_count_2 = speed_count_2 - 1;
    }
    else
    {
        //prevent initializing error
        state_2_old = state_2;
    }
    
    //change old state
    state_2_old = state_2;
    //////////////////////////////////
    //////////////////////////////////
    //forward : speed_count_2 + 1
    //backward : speed_count_2 - 1
}