File content as of revision 0:e5d268f797d7:

#include "mbed.h"
#include "QEI.h"
#include "ros/time.h"
#include "sensor_msgs/JointState.h"
#include "std_msgs/Float32.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include "tf/transform_broadcaster.h"
#include "tf/tf.h"
#include "PID.h"

//    
//    1 \\-----// 2                    
//        |   |                        
//        |   | <--robot               
//        |   |                        
//    4 //-----\\ 3             
//

//-----------------------define pin-----------------------//

#define RATE 0.01

#define kp 2.3
#define ki 0.0
#define kd 0.0

DigitalOut dir1(PB_4);
DigitalOut dir2(PB_5);
DigitalOut dir3(PB_3);
DigitalOut dir4(PA_10);

PwmOut pul1(PC_7);
PwmOut pul2(PA_9);
PwmOut pul3(PA_8);
PwmOut pul4(PB_10);

DigitalOut led(LED1);
//------------------------------------------------------//

//-----------------------define value-----------------------//
long time_loop = 0;

float l     = 0.2;
float w     = 0.1;
float rad   = 0.03;

float u1 = 0.0;
float u2 = 0.0;
float u3 = 0.0;
float u4 = 0.0;

float speed1;
float speed2;
float speed3;
float speed4;

double th =0;

//----------------------------------------------------------//


//-----------------------cal omega wheel----------------------//

void messageCb(const geometry_msgs::Twist& msg)
{
    led = !led;
    
    u1 = ( -((l+w)*msg.angular.z) + msg.linear.x - msg.linear.y )/rad;  //omega wheel in rad/sec
    u2 = (  ((l+w)*msg.angular.z) + msg.linear.x + msg.linear.y )/rad;
    u3 = (  ((l+w)*msg.angular.z) + msg.linear.x - msg.linear.y )/rad;
    u4 = ( -((l+w)*msg.angular.z) + msg.linear.x + msg.linear.y )/rad;
    
    if(u1>=0)   {dir1 = 1;}     //dir1-->go forward
    if(u1<0)    {dir1 = 0;}     //dir1-->go backward
    if(u2>=0)   {dir2 = 1;}
    if(u2<0)    {dir2 = 0;}
    if(u3>=0)   {dir3 = 1;}
    if(u3<0)    {dir3 = 0;}
    if(u4>=0)   {dir4 = 1;}
    if(u4<0)    {dir4 = 0;}
    
    u1 = abs(u1);
    u2 = abs(u2);
    u3 = abs(u3);
    u4 = abs(u4);
}

//------------------------------------------------------------//

//-----------------------encoder----------------------//

QEI en1 (PB_8, PB_9, NC, 748.44);
QEI en2 (PA_7, PB_6, NC, 748.44);
QEI en3 (PA_4, PB_0, NC, 748.44);
QEI en4 (PC_1, PC_0, NC, 748.44);

//----------------------------------------------------//

//-----------------------PID Controller----------------------//

PID controller1( kp, ki, kd, RATE);
PID controller2( kp, ki, kd, RATE);
PID controller3( kp, ki, kd, RATE);
PID controller4( kp, ki, kd, RATE);

//-----------------------------------------------------------//

//-----------------------drive----------------------//

void drive()
{
    controller1.setSetPoint(u1);
    controller2.setSetPoint(u2);
    controller3.setSetPoint(u3);
    controller4.setSetPoint(u4);
    
    controller1.setProcessValue(abs(en1.getVelosity()));
    speed1 = controller1.compute();
    pul1.pulsewidth_us(speed1);
    
    controller2.setProcessValue(abs(en2.getVelosity()));
    speed2 = controller2.compute();
    pul2.pulsewidth_us(speed2);
    
    controller3.setProcessValue(abs(en3.getVelosity()));
    speed3 = controller3.compute();
    pul3.pulsewidth_us(speed3);
    
    controller4.setProcessValue(abs(en4.getVelosity()));
    speed4 = controller4.compute();
    pul4.pulsewidth_us(speed4);
}

//--------------------------------------------------//

ros::NodeHandle nh;
geometry_msgs::Twist msg;
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
nav_msgs::Odometry _odometry_msg;
ros::Publisher odom("odom", &_odometry_msg);

sensor_msgs::JointState _joints_state_msg;
ros::Publisher joint_states("joint_states", &_joints_state_msg);

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;

Ticker time_up;

//-----------------------main function----------------------//

int main()
{
    nh.getHardware()->setBaud(250000);
    nh.initNode();
    nh.subscribe(sub);
    
    nh.advertise(odom);
    nh.advertise(joint_states);
    broadcaster.init(nh);
    
    _joints_state_msg.header.frame_id = "base_footprint";
    _joints_state_msg.name_length = 4;
    char* names[4];
    _joints_state_msg.name = names;
    _joints_state_msg.name[0] = "wheel_front_left_joint";
    _joints_state_msg.name[1] = "wheel_front_right_joint";
    _joints_state_msg.name[2] = "wheel_rear_left_joint";
    _joints_state_msg.name[3] = "wheel_rear_right_joint";
    _joints_state_msg.position_length = 4;
    double positions[4];
    _joints_state_msg.position = positions;
    
    t.header.frame_id = "odom";
    t.child_frame_id = "base_footprint";
    
    _odometry_msg.header.frame_id = "odom";
    _odometry_msg.child_frame_id = "base_footprint";
    
    pul1.period_us(1000);
    pul2.period_us(1000);
    pul3.period_us(1000);
    pul4.period_us(1000);
  
    controller1.setInputLimits(0.0, 36.56);
    controller2.setInputLimits(0.0, 36.56);
    controller3.setInputLimits(0.0, 36.56);
    controller4.setInputLimits(0.0, 36.56);

    controller1.setOutputLimits(0.0, 1000.0);
    controller2.setOutputLimits(0.0, 1000.0);
    controller3.setOutputLimits(0.0, 1000.0);
    controller4.setOutputLimits(0.0, 1000.0);

    controller1.setBias(1.0);
    controller2.setBias(1.0);
    controller3.setBias(1.0);
    controller4.setBias(1.0);
  
    controller1.setMode(0);
    controller2.setMode(0);
    controller3.setMode(0);
    controller4.setMode(0);
    
    time_up.attach(&drive, RATE);
    
    while (1)
    {
        th = ((-en1.getPulses()+en2.getPulses()+en3.getPulses()-en4.getPulses())*0.00839504209)*(rad/(4*(l+w)));
        
        _joints_state_msg.header.stamp = nh.now(); 
        _joints_state_msg.position[0] = en1.getPulses()*0.00839504209;//rad
        _joints_state_msg.position[1] = en2.getPulses()*0.00839504209;//rad
        _joints_state_msg.position[2] = en3.getPulses()*0.00839504209;//rad
        _joints_state_msg.position[3] = en4.getPulses()*0.00839504209;//rad
        joint_states.publish(&_joints_state_msg); 
        
        _odometry_msg.header.stamp = nh.now();
        _odometry_msg.pose.pose.position.x = (( en1.getPulses()+en2.getPulses()+en3.getPulses()+en4.getPulses())*0.00839504209)*(rad/4);
        _odometry_msg.pose.pose.position.y = ((-en1.getPulses()+en2.getPulses()-en3.getPulses()+en4.getPulses())*0.00839504209)*(rad/4);
        _odometry_msg.pose.pose.position.z = 0;
        _odometry_msg.pose.pose.orientation = tf::createQuaternionFromYaw(th);
        odom.publish(&_odometry_msg);   
        
        t.transform.translation.x = _odometry_msg.pose.pose.position.x;
        t.transform.translation.y = _odometry_msg.pose.pose.position.y;
        t.transform.rotation = _odometry_msg.pose.pose.orientation;
        t.header.stamp = nh.now();
        broadcaster.sendTransform(t);      
        
        nh.spinOnce();
        
        wait_ms(400);
    }
}
//----------------------------------------------------------//