File content as of revision 0:4ff8854a49e8:

#include "mbed.h"
#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include<geometry_msgs/Twist.h> //set buffer larger than 50byte
#include <std_msgs/Float32.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <iostream>
#define Start 0xAA
#define Address 0x7F
#define ReturnType 0x00
#define Clean 0x00
#define Reserve 0x00
#define End 0x55 

    Serial RS232 (PB_10, PB_11,115200);  // This one works
    std_msgs::String str_msg;
    std_msgs::Float32 VelAngular_L;
    std_msgs::Float32 VelAngular_R;
    ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
    ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
    
    ros::NodeHandle nh;
    geometry_msgs::Twist vel;
    //float left;
    //float right;
    float Lrpm,Rrpm;
    float ticks_since_target;
    double timeout_ticks;

    double w;
    double rate;
    double Dimeter;
    float dx,dy,dr;
    void init_variables();
void Sendmessage(float Rrpm,float Lrpm);

unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
{
    unsigned int i, j;
    //#define wPolynom 0xA001
    unsigned int wCrc = 0xffff;
    unsigned int wPolynom = 0xA001;
    /*---------------------------------------------------------------------------------*/
    for (i = 0; i < iBufLen; i++)
    {
        wCrc ^= cBuffer[i];
        for (j = 0; j < 8; j++)
        {
            if (wCrc &0x0001)
            {
                wCrc = (wCrc >> 1) ^ wPolynom;
            }
            else
            { 
                wCrc = wCrc >> 1;
            }
        }
    }
    return wCrc;
}

 /*
 起始碼(1Byte):                            AA
地址碼(1Byte):                             01
返回數據類型(1Byte):從站驅動器不返回數據       00
故障清除(1Byte)默認發送:                     00
保留位(1Byte)                              00
A 馬達過載控制(1Byte)驅動器使能狀態           01
B 馬達控制(1Byte)                           01
A 運行方向(1Byte)00:正轉,01反轉             00
B 運行方向(1Byte)00:正轉,01反轉             00
運行轉速(2Byte)是100–3000r/min,            12 34
運行轉速(2Byte)是100–3000r/min,            12 34
第 1 個 Byte:速度值高 8 位;                
第 2 個 Byte:速度值低 8 位;
無效(2Byte)00:無效數據位。                   00
結束碼(1Byte)55:通訊結束編碼識別。            55
校驗碼(2Byte)CRC16 
 */

 //   void spinOnce();
 //   void twistCallback(const geometry_msgs::Twist &twist_aux);


void init_variables()
{
    dx = dy = dr =0;
    w = 0.302;//0.2 ;//m
    rate = 20;//50;
    timeout_ticks = 2;
    Dimeter = 0.127;//0.15;
}
void Sendmessage(float Rrpm,float Lrpm)
{
    //RS232.printf("Wr = %.1f\n",Rrpm);
    //RS232.printf("Wl = %.1f\n",Lrpm);
unsigned char sendData[16];
unsigned int tmpCRC;
int motor1,motor2;
   
    sendData[0] = Start;
    sendData[1] = Address;
    sendData[2] = ReturnType;
    sendData[3] = Clean;
    sendData[4] = Reserve;
    sendData[5]  = 0x01;//motor1Sevro ON
    sendData[6] = 0x01;//motor2Sevro ON
if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
   motor1 =  abs(Rrpm);
   motor2 =  abs(Lrpm); 
    
    sendData[9] = (motor1>>8);//motor1speedH
    sendData[10] = (motor1 & 0xFF);//motor1speedL
    sendData[11] = (motor2>>8);//motor2speedH
    sendData[12] = (motor2 & 0xFF);//motor2speedL
    sendData[13] = End;
    tmpCRC = CRC_Verify(sendData, 14);
    sendData[14] = (tmpCRC & 0xFF);
    sendData[15] = (tmpCRC>>8);
    int i;
    for (i=0;i<16;i++)
    {
        RS232.printf("%c",sendData[i]);
    }
    RS232.printf("\r\n");
}
void TwistToMotors()
{
    float right,left;
    float motor_rpm_r, motor_rpm_l;
    double vel_data[2];
    right = ( 1.0 * dx ) + (dr * w /2);
    /*
    left = ( 1.0 * dx ) - (dr * w /2);
    vel_data[0] = right*rate/Dimeter/60*1000;
    vel_data[1] =  left*rate/Dimeter/60*1000;
    */
   if (dx!=0)
    {
     if (dx>0)
     { 
       if (dr >=0)
       {
           motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
           motor_rpm_l=300;
       }
       else
       {
           motor_rpm_r=300;
           motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
       }
     }
     else
     {
         if(dr>=0)
         {
          motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
          motor_rpm_l=(-300);
         }
         else
         {
             motor_rpm_r=(-300);
             motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
         }
     }
    }
    else
    {
        if(dr>=0)
        {
            motor_rpm_r=100;
            motor_rpm_l=-100;
        }
        else
        {
            motor_rpm_r=-100;
            motor_rpm_l=100;
        }
    }
    vel_data[0]=motor_rpm_r;
    vel_data[1]=motor_rpm_l; 
    //================================================================ Original Version
    /*if (dr>=0)
    {
        right = ( 1.0 * dx ) + (dr * w /2);
        left = ( 1.0 * dx );
    }
    else
    {
        right = ( 1.0 * dx );
        left = ( 1.0 * dx ) - (dr * w /2);
    }
        vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60;
        vel_data[1] =  left*rate/(Dimeter/2)/(2*3.1416)*60;*/
//===================================================================
    Sendmessage(vel_data[0],vel_data[1]);
    VelAngular_R.data = vel_data[0];
    VelAngular_L.data = vel_data[1];
    //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
    //}
    //else{
    pub_rmotor.publish( &VelAngular_R );
    pub_lmotor.publish( &VelAngular_L );
    //}
    //RS232.printf("Wr = %.1f\n",vel_data[0]);
    //RS232.printf("Wl = %.1f\n",vel_data[1]);
    ticks_since_target += 1;

}

void messageCb(const geometry_msgs::Twist &msg)
{
    ticks_since_target = 0;
    dx = msg.linear.x;
    dy = msg.linear.y;
    dr = msg.angular.z;
    //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr);
    TwistToMotors();
}

void ACT( const std_msgs::String& ACT) {
char sendData[16];
//sendData = ACT;
int motor1,motor2;
   
    sendData[0] = Start;
    sendData[1] = Address;
    sendData[2] = ReturnType;
    sendData[3] = Clean;
    sendData[4] = Reserve;
    sendData[5]  = 0x00;//motor1Sevro OFF
    sendData[6] = 0x00;//motor2Sevro OFF
   motor1 =  0;
   motor2 =  0; 
    sendData[9] = (motor1>>8);//motor1speedH
    sendData[10] = (motor1 & 0xFF);//motor1speedL
    sendData[11] = (motor2>>8);//motor2speedH
    sendData[12] = (motor2 & 0xFF);//motor2speedL
    sendData[13] = End;
    
    
    sendData[14] = Start;
    sendData[15] = Start;
    int i;
    for (i=0;i<16;i++)
    {
        RS232.printf("%c",sendData[i]);
    }
    RS232.printf("\r\n");
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT );

 Timer t;
int main(int argc, char **argv)
{
    long vel_timer = 0;
    init_variables();
    t.start();
    nh.initNode();
    nh.advertise(pub_lmotor);
    nh.advertise(pub_rmotor);
    nh.subscribe(cmd_vel_sub);
    nh.subscribe(sub_action);

   while (1)
   {
     if (t.read_ms() > vel_timer)
     {
       vel_timer = t.read_ms() + 500;
     }
     nh.spinOnce();

     wait_ms(10);
   }
}