File content as of revision 1:4a6acf69fde9:

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

#define MOTOR_PWM_PERIOD 50  //us
#define MOTOR_INITIAL_DUTYCYCLE 0.5f
#define Kp 0.01f
#define Ki 0.08f

enum STATE{ONE, TWO, THREE, FOUR, DEFAULT};

class HaseHardware : public MbedHardware
{
public:
    HaseHardware(): MbedHardware(D10, D2, 115200){};            
};
ros::NodeHandle_<HaseHardware> nh;

Ticker motorTimer;
PwmOut motor_cmd_1(D7);
PwmOut motor_cmd_1N(D11);
PwmOut motor_cmd_2(D8);
PwmOut motor_cmd_2N(A3);
InterruptIn hallA_1(A1);
InterruptIn hallB_1(A2);
InterruptIn hallA_2(D13);
InterruptIn hallB_2(D12);


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


int v1Count = 0;
int v2Count = 0;
float DESIRE_REV_1 = 0.0f;
float DESIRE_REV_2 = 0.0f;
float rev_1 = 0.0;
float rev_2 = 0.0;

void init_motor();
void motorTimer_interrupt();
void init_timer();
void init_hall();
void CN_interrupt();


void messageCb(const geometry_msgs::Vector3& msg)
{
    //receive velocity command from PC to motor
    DESIRE_REV_1 = msg.x;
    DESIRE_REV_2 = msg.y; 
}
ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel", messageCb);

int main() {
    
    init_motor();
    init_timer();
    init_hall();
    
    nh.initNode();
    nh.subscribe(s);
    nh.advertise(p);
    
    while(1)
    {
        vel_msg.linear.x = DESIRE_REV_1;
        vel_msg.linear.y = rev_1;
        vel_msg.linear.z = 0.0f;
         
        vel_msg.angular.x = DESIRE_REV_2;
        vel_msg.angular.y = rev_2;
        vel_msg.angular.z = 0.0f;
        
        p.publish(&vel_msg);
        nh.spinOnce();
        wait_ms(50);
    }
}

void init_hall()
{
    hallA_1.rise(&CN_interrupt);
    hallB_1.rise(&CN_interrupt);
    hallA_2.rise(&CN_interrupt);
    hallB_2.rise(&CN_interrupt);
    
    hallA_1.fall(&CN_interrupt);
    hallB_1.fall(&CN_interrupt);
    hallA_2.fall(&CN_interrupt);
    hallB_2.fall(&CN_interrupt);
}

void init_motor()
{
    motor_cmd_1.period_us(MOTOR_PWM_PERIOD);
    motor_cmd_1.write(MOTOR_INITIAL_DUTYCYCLE);
    TIM1->CCER |= 0x4;
    motor_cmd_2.period_us(MOTOR_PWM_PERIOD);
    motor_cmd_2.write(MOTOR_INITIAL_DUTYCYCLE);
    TIM1->CCER |= 0x40;
}

void init_timer(void)
{

    motorTimer.attach_us(&motorTimer_interrupt, 10000.0); //10ms
}



void motorTimer_interrupt()
{
    
    static float output_vol_1 = 0.0;
    static float output_vol_2 = 0.0;
    static float error_rev_1 = 0.0;
    static float error_rev_2 = 0.0;
    static float intergral_1 = 0.0;
    static float intergral_2 = 0.0;
    static float duty_cycle_1 = 0.5;
    static float duty_cycle_2 = 0.5;
    
    ////motor1輸出
    
    rev_1 = (float)v1Count /12.0f *6000.0f /29.0f;  //rpm
    //在這裡把Count都歸零，因為Count是累加的，如果不歸零轉速會一直變大
    v1Count = 0;
    
    error_rev_1 = (DESIRE_REV_1 -rev_1);  // (rad/s)
    intergral_1 += error_rev_1 *0.01f;
    output_vol_1 = error_rev_1*Kp + intergral_1*Ki;
    

    if (-5.0f > output_vol_1)
    {
        output_vol_1 = -5.0f;
    }
    else if (output_vol_1 > 5.0f)
    {
        output_vol_1 = 5.0f;
    }
    duty_cycle_1 = (5.0f +output_vol_1) /10.0f;
    
    motor_cmd_1.write(duty_cycle_1);
    TIM1->CCER |= 0x4;

    
    
    ////motor2輸出
    
    rev_2 = (float)v2Count /12.0f *6000.0f /29.0f;  //rpm
    //在這裡把Count都歸零，因為Count是累加的，如果不歸零轉速會一直變大
    v2Count = 0;
    
    error_rev_2 = (DESIRE_REV_2 -rev_2);  // (rad/s)
    intergral_2 += error_rev_2 *0.01f;
    output_vol_2 = error_rev_2*Kp + intergral_2*Ki;
    

    if (-5.0f > output_vol_2)
    {
        output_vol_2 = -5.0f;
    }
    else if (output_vol_2 > 5.0f)
    {
        output_vol_2 = 5.0f;
    }
    duty_cycle_2 = (5.0f +output_vol_2) /10.0f;
    
    motor_cmd_2.write(duty_cycle_2);
    TIM1->CCER |= 0x40;
 
}

void CN_interrupt()
{
    static bool hall1A_state_1;
    static bool hall1B_state_1;
    static bool hall1A_state_2;
    static bool hall1B_state_2;
    static STATE old_state_1 = DEFAULT;
    static STATE new_state_1 = DEFAULT;
    static STATE old_state_2 = DEFAULT;
    static STATE new_state_2 = DEFAULT;


    ////////////////////motor1///////////////////////
    hall1A_state_1 = hallA_1.read();
    hall1B_state_1 = hallB_1.read();
    old_state_1 = new_state_1;
    
    if (hall1A_state_1 == 0)
    {
        if (hall1B_state_1 == 0)
        {
            new_state_1 = ONE;
        }
        else if(hall1B_state_1 == 1)
        {
            new_state_1 = TWO;
        }
    }
    else if (hall1A_state_1 == 1)
    {
        if (hall1B_state_1 == 0)
        {
            new_state_1 = FOUR;
        }
        else if(hall1B_state_1 == 1)
        {
            new_state_1 = THREE;
        }
    }
    
    ///////判斷正反轉//////
    switch (old_state_1)
    {
        case ONE:
            if(new_state_1 == TWO)
            {
                v1Count += 1;     
            }
            else if(new_state_1 == FOUR)
            {        
                v1Count -= 1;               
            }

            break;
        case TWO:
            if(new_state_1 == THREE)
            {
                v1Count += 1;     
            }
            else if(new_state_1 == ONE)
            {        
                v1Count -= 1;               
            }

            break;
        case THREE:
            if(new_state_1 == FOUR)
            {
                v1Count += 1;     
            }
            else if(new_state_1 == TWO)
            {        
                v1Count -= 1;               
            }

            break;
        case FOUR:
            if(new_state_1 == ONE)
            {
                v1Count += 1;     
            }
            else if(new_state_1 == THREE)
            {        
                v1Count -= 1;               
            }


            break;
        case DEFAULT:
        
            break ;        
    }
    
    
    
    ////////////////////motor2///////////////////////
    hall1A_state_2 = hallA_2.read();
    hall1B_state_2 = hallB_2.read();
    old_state_2 = new_state_2;
    
    if (hall1A_state_2 == 0)
    {
        if (hall1B_state_2 == 0)
        {
            new_state_2 = ONE;
        }
        else if(hall1B_state_2 == 1)
        {
            new_state_2 = TWO;
        }
    }
    else if (hall1A_state_2 == 1)
    {
        if (hall1B_state_2 == 0)
        {
            new_state_2 = FOUR;
        }
        else if(hall1B_state_2 == 1)
        {
            new_state_2 = THREE;
        }
    }
    
    ///////判斷正反轉//////
    switch (old_state_2)
    {
        case ONE:
            if(new_state_2 == TWO)
            {
                v2Count += 1;     
            }
            else if(new_state_2 == FOUR)
            {        
                v2Count -= 1;               
            }

            break;
        case TWO:
            if(new_state_2 == THREE)
            {
                v2Count += 1;     
            }
            else if(new_state_2 == ONE)
            {        
                v2Count -= 1;               
            }

            break;
        case THREE:
            if(new_state_2 == FOUR)
            {
                v2Count += 1;     
            }
            else if(new_state_2 == TWO)
            {        
                v2Count -= 1;               
            }

            break;
        case FOUR:
            if(new_state_2 == ONE)
            {
                v2Count += 1;     
            }
            else if(new_state_2 == THREE)
            {        
                v2Count -= 1;               
            }


            break;
        case DEFAULT:
        
            break ;        
    }
}