File content as of revision 2:fcf8a7739eed:

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

// ============= =============
        float motor_rpm_r, motor_rpm_l;
// ============= =============
        
    ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
    ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
    int seq = 0; // monitor times of callbacks
    int old_seq = 0; // monitor times of the command
    
    
    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");
}


int myabs( int a ){
    if ( a < 0 ){
        return -a;
        }
        return a;
        }
        
        
        
void TwistToMotors()
{
    old_seq = seq;
    seq = seq + 1;    
    //int lower_bound = 100;
    //int upper_bound = 300;
    float right,left;
//    float motor_rpm_r, motor_rpm_l;
    //double vel_data[2];
    float vel_data[2];
    float motor_rpm_vx, motor_rpm_theta, motor_rpm_v;

    // prevent agv receive weird 1.0 command from cmd_vel

    right = ( 1.0 * dx ) + (dr * w /2);
    left = ( 1.0 * dx ) - (dr * w /2);
    motor_rpm_v = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
    
    if((motor_rpm_v!=0) && (myabs(motor_rpm_v)<100)){
        if (motor_rpm_v > 0)  motor_rpm_v=100;
        else motor_rpm_v = -100;
        }

    motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);


    //motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
    //motor_rpm_l =  left*rate/(Dimeter/2)*60/(2*3.1416);

    motor_rpm_r = motor_rpm_v + motor_rpm_theta;
    motor_rpm_l = motor_rpm_v - motor_rpm_theta;
    
        
        if (myabs(motor_rpm_r)<100 || myabs(motor_rpm_l)<100){
            
        if(dx == 0){
            if(dr > 0){
                motor_rpm_r = 100;
                motor_rpm_l = -100;
                }else if( dr < 0 ){
                    motor_rpm_r = -100;
                    motor_rpm_l = 100;
                    }else{
                        motor_rpm_r = 0;
                        motor_rpm_l = 0;
                        }
            }
            else if( dx > 0 ){
                if( myabs(motor_rpm_r) < 100 ){
                    motor_rpm_r = 100;
                    motor_rpm_l=motor_rpm_l+(100-motor_rpm_r);
                }
                if ( myabs(motor_rpm_l) < 100 ){
                    motor_rpm_l = 100;
                    motor_rpm_r=motor_rpm_r+(100-motor_rpm_l);
                    }
            }else{
                    if ( myabs(motor_rpm_r) < 100 ){
                        motor_rpm_r = -100;
                        motor_rpm_l=motor_rpm_l+(-100-motor_rpm_r);
                    }
                    
                    if (myabs(motor_rpm_l) < 100 ){
                        motor_rpm_l = -100;
                        motor_rpm_r=motor_rpm_r+(-100-motor_rpm_l);
                    }
            }
    }
                                 
    /*
    if ( myabs(motor_rpm_r) == myabs(motor_rpm_l) && myabs(motor_rpm_r) < 100){
        int s = myabs(motor_rpm_r);
        
        motor_rpm_r = motor_rpm_r / s * 101;
        motor_rpm_l = motor_rpm_l / s * 101;
        
        }
*/

/*
    if ( myabs(vel_data[0]) < lower_bound )
    { // if right wheel rpm < lower_bound
            int a = 0;
            a = lower_bound - myabs( vel_data[0] );
        
        if (vel_data[0] > 0)
        {
            vel_data[0] = lower_bound;
        }
        else
        {
            vel_data[0] = -lower_bound;
        }
            
            
        if (vel_data[1] > 0)
        {
            vel_data[1] = vel_data[1] + a;
        }
        else
        {
            vel_data[1] = vel_data[1] - a;
        } //
        
    }
        
    else if ( myabs(vel_data[0]) > upper_bound )
    {  // if right wheel rpm > upper_bound
            int a = 0;
            a = myabs( vel_data[0] ) - upper_bound;
        
        if (vel_data[0] > 0)
        {
            vel_data[0] = upper_bound;
        }
        else
        {
            vel_data[0] = -upper_bound;
        }
            
            
        if (vel_data[1] > 0)
        {
            vel_data[1] = vel_data[1] - a;
        }
        else
        {
            vel_data[1] = vel_data[1] + a;
        }
    }
        
               
    if ( myabs(vel_data[1]) < lower_bound )
    { // if left wheel rpm < lower_bound
            int a = 0;
            a = lower_bound - myabs( vel_data[1] );
        
        if (vel_data[1] > 0)
        {
            vel_data[1] = lower_bound;
        }
        else
        {
            vel_data[1] = -lower_bound;
        }
            
            
        if (vel_data[0] > 0)
        {
            vel_data[0] = vel_data[0] + a;
        }
        else
        {
            vel_data[0] = vel_data[0] - a;
        } //
    }
        
        
    else if (myabs(vel_data[1]) > upper_bound )
    { // if left wheel rpm > upper_bound
            int a = 0;
            a = myabs( vel_data[1] ) - upper_bound;
        
        if (vel_data[1] > 0)
        {
            vel_data[1] = upper_bound;
        }
        else
        {
            vel_data[1] = -upper_bound;
        }
            
            
        if (vel_data[0] > 0)
        {
            vel_data[0] = vel_data[0] - a;
        }
        else
        {
            vel_data[0] = vel_data[0] + a;
        }
            
    }
               
        */
        
        /*
        if ( myabs(motor_rpm_r) < 100 || myabs(motor_rpm_l) < 100 ){
            float a = 0.0;
        
            if ( myabs(motor_rpm_r) < myabs(motor_rpm_l) ){
                
                if ( motor_rpm_r < 0 ){
                    a = -100 - motor_rpm_r;
                }else if( motor_rpm_r > 0) {
                    a = 100 - motor_rpm_r;
                    }else {
                        a = 0;
                }
                
                motor_rpm_r = motor_rpm_r + a;
                motor_rpm_l = motor_rpm_l + a;
                
                    
                
            }else if ( myabs(motor_rpm_r) > myabs(motor_rpm_l) ){
                
                if ( motor_rpm_l < 0 ){
                    a = -100 - motor_rpm_l;
                }else if( motor_rpm_l > 0) {
                    a = 100 - motor_rpm_l;
                    }else {
                        a = 0;
                }
                
                motor_rpm_l = motor_rpm_l + a;
                motor_rpm_r = motor_rpm_r + a;
                
            }
            
            // go straight < 100
            if ( motor_rpm_r == motor_rpm_l ){
                
                if ( motor_rpm_l < 0 ){
                    motor_rpm_l = -100;
                    motor_rpm_r = -100;
                
                
                }else if( motor_rpm_l > 0) {
                    motor_rpm_l = 100;
                    motor_rpm_r = 100;
                    }else {
                        motor_rpm_l = 0;
                        motor_rpm_r = 0;
                    }           
                
                
                }
                
                // turn < 100
                
                if ( motor_rpm_r == -motor_rpm_l ){
                
                if ( motor_rpm_l < 0 ){
                motor_rpm_l = -100;
                motor_rpm_r = 100;
                }else if( motor_rpm_l > 0) {
                motor_rpm_l = 100;
                motor_rpm_r = -100;
                }else {
                    motor_rpm_l = 0;
                    motor_rpm_r = 0;
                }
                               
            }
            //
            
            } 
            

            
        if ( myabs(motor_rpm_r) > 600 ){
               
               if ( motor_rpm_r < 0 ){
               motor_rpm_r = -100 * log( float(myabs(motor_rpm_r)) );
               }else{
                   motor_rpm_r = 100 * log( float(myabs(motor_rpm_r)) );
               }
               

               
        }
        
        if ( myabs(motor_rpm_l) > 600) {
        
               if ( motor_rpm_l < 0 ){
               motor_rpm_l = -100 * log( float(myabs(motor_rpm_l)) );
               }else{
                   motor_rpm_l = 100 * log( float(myabs(motor_rpm_l)) );
               }
        }
        
        */
        vel_data[0] = motor_rpm_r;
        vel_data[1] = motor_rpm_l;
        
        
        
        

//===================================================================
    //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 (true)
   {
     //if (t.read_ms() > vel_timer)
     //{
     //  vel_timer = t.read_ms() + 50;
     //  TwistToMotors();
     // }
     

     nh.spinOnce();
     
     
     if (seq +50 < old_seq){
     motor_rpm_r = 0;
     motor_rpm_l = 0;
     }
     
//     Sendmessage(-motor_rpm_r,-motor_rpm_l);
     Sendmessage(-motor_rpm_l,-motor_rpm_r);
     
     old_seq = old_seq + 1;
    
     wait_ms(10);
    
   }
}