File content as of revision 0:e27261bd5773:

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

#define Ts 0.01f 

Ticker timer1;

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

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


float rotation_speed_1 = 0.0;
float rotation_speed_ref_1 = 0;
float pwm1_duty = 0.5;
double PI_out_1 = 0.0;
float err_1 = 0.0;
float ierr_1 = 0.0;
float rotation_speed_2 = 0.0;
float rotation_speed_ref_2 = -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 V_ref = 0; // (m/s) max: 0.25
float V = 0;
float W_ref = 0;  // (rad/s)
float W = 0;


float Kp = 0.008;
float Ki = 0.02;

//Serial pc(USBTX, USBRX);

//rosserial
ros::NodeHandle nh;

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

void messageCallback(const geometry_msgs::Twist &msg_receive)
{
    V_ref = 0.25*msg_receive.linear.x;
    W_ref = msg_receive.angular.z;

}

ros::Subscriber<geometry_msgs::Twist> s("cmd_vel",messageCallback);

int main()
{
    //pc.baud(9600);
    init_TIMER();
    init_PWM();
    init_CN();
    
    nh.initNode();
    nh.subscribe(s);
    nh.advertise(p);
    
    while(1) {
        V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f;
        W = (-1.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);*/
        
        vel_msg.linear.x = V;
        vel_msg.angular.z = W;
        
        p.publish(&vel_msg);
        
        nh.spinOnce();
        
        wait_ms(100);
    }
}



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


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;
}


void timer1_ITR()
{
    
    rotation_speed_ref_1 = (V_ref / r - L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f;
    rotation_speed_ref_2 = (V_ref / r + L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f;
    
    
    // motor1

    rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f;   //unit: rpm
    speed_count_1 = 0;
    
    ///PI controller for motor1///
    
    err_1 = rotation_speed_ref_1 - rotation_speed_1;
    ierr_1 = ierr_1 + Ts * err_1;
    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);
  
    //////////////////////////////
    
    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;   
    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;
    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) ;
  
    
    //////////////////////////////
    
    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;
    pwm2.write(pwm2_duty);
    TIM1->CCER |= 0x40;
    

}