main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <ros/time.h>
00004 #include <std_msgs/Empty.h>
00005 #include <std_msgs/String.h>
00006 #include<geometry_msgs/Twist.h> //set buffer larger than 50byte
00007 #include <std_msgs/Float32.h>
00008 #include <math.h>
00009 #include <stdio.h>
00010 #include <string.h>
00011 #include <iostream>
00012 #define Start 0xAA
00013 #define Address 0x7F
00014 #define ReturnType 0x00
00015 #define Clean 0x00
00016 #define Reserve 0x00
00017 #define End 0x55 
00018 
00019     Serial RS232 (PB_10, PB_11,115200);  // This one works
00020     std_msgs::String str_msg;
00021     std_msgs::Float32 VelAngular_L;
00022     std_msgs::Float32 VelAngular_R;
00023     ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
00024     ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
00025     
00026     ros::NodeHandle nh;
00027     geometry_msgs::Twist vel;
00028     //float left;
00029     //float right;
00030     float Lrpm,Rrpm;
00031     float ticks_since_target;
00032     double timeout_ticks;
00033 
00034     double w;
00035     double rate;
00036     double Dimeter;
00037     float dx,dy,dr;
00038     void init_variables();
00039 void Sendmessage(float Rrpm,float Lrpm);
00040 
00041 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
00042 {
00043     unsigned int i, j;
00044     //#define wPolynom 0xA001
00045     unsigned int wCrc = 0xffff;
00046     unsigned int wPolynom = 0xA001;
00047     /*---------------------------------------------------------------------------------*/
00048     for (i = 0; i < iBufLen; i++)
00049     {
00050         wCrc ^= cBuffer[i];
00051         for (j = 0; j < 8; j++)
00052         {
00053             if (wCrc &0x0001)
00054             {
00055                 wCrc = (wCrc >> 1) ^ wPolynom;
00056             }
00057             else
00058             { 
00059                 wCrc = wCrc >> 1;
00060             }
00061         }
00062     }
00063     return wCrc;
00064 }
00065 
00066  /*
00067  起始碼(1Byte):                            AA
00068 地址碼(1Byte):                             01
00069 返回數據類型(1Byte):從站驅動器不返回數據       00
00070 故障清除(1Byte)默認發送:                     00
00071 保留位(1Byte)                              00
00072 A 馬達過載控制(1Byte)驅動器使能狀態           01
00073 B 馬達控制(1Byte)                           01
00074 A 運行方向(1Byte)00:正轉,01反轉             00
00075 B 運行方向(1Byte)00:正轉,01反轉             00
00076 運行轉速(2Byte)是100–3000r/min,            12 34
00077 運行轉速(2Byte)是100–3000r/min,            12 34
00078 第 1 個 Byte:速度值高 8 位;                
00079 第 2 個 Byte:速度值低 8 位;
00080 無效(2Byte)00:無效數據位。                   00
00081 結束碼(1Byte)55:通訊結束編碼識別。            55
00082 校驗碼(2Byte)CRC16 
00083  */
00084 
00085  //   void spinOnce();
00086  //   void twistCallback(const geometry_msgs::Twist &twist_aux);
00087 
00088 
00089 void init_variables()
00090 {
00091     dx = dy = dr =0;
00092     w = 0.302;//0.2 ;//m
00093     rate = 20;//50;
00094     timeout_ticks = 2;
00095     Dimeter = 0.127;//0.15;
00096 }
00097 void Sendmessage(float Rrpm,float Lrpm)
00098 {
00099     //RS232.printf("Wr = %.1f\n",Rrpm);
00100     //RS232.printf("Wl = %.1f\n",Lrpm);
00101 unsigned char sendData[16];
00102 unsigned int tmpCRC;
00103 int motor1,motor2;
00104    
00105     sendData[0] = Start;
00106     sendData[1] = Address;
00107     sendData[2] = ReturnType;
00108     sendData[3] = Clean;
00109     sendData[4] = Reserve;
00110     sendData[5]  = 0x01;//motor1Sevro ON
00111     sendData[6] = 0x01;//motor2Sevro ON
00112 if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
00113 if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
00114    motor1 =  abs(Rrpm);
00115    motor2 =  abs(Lrpm); 
00116     
00117     sendData[9] = (motor1>>8);//motor1speedH
00118     sendData[10] = (motor1 & 0xFF);//motor1speedL
00119     sendData[11] = (motor2>>8);//motor2speedH
00120     sendData[12] = (motor2 & 0xFF);//motor2speedL
00121     sendData[13] = End;
00122     tmpCRC = CRC_Verify(sendData, 14);
00123     sendData[14] = (tmpCRC & 0xFF);
00124     sendData[15] = (tmpCRC>>8);
00125     int i;
00126     for (i=0;i<16;i++)
00127     {
00128         RS232.printf("%c",sendData[i]);
00129     }
00130     RS232.printf("\r\n");
00131 }
00132 void TwistToMotors()
00133 {
00134     float right,left;
00135     float motor_rpm_r, motor_rpm_l;
00136     double vel_data[2];
00137     right = ( 1.0 * dx ) + (dr * w /2);
00138     /*
00139     left = ( 1.0 * dx ) - (dr * w /2);
00140     vel_data[0] = right*rate/Dimeter/60*1000;
00141     vel_data[1] =  left*rate/Dimeter/60*1000;
00142     */
00143    if (dx!=0)
00144     {
00145      if (dx>0)
00146      { 
00147        if (dr >=0)
00148        {
00149            motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00150            motor_rpm_l=300;
00151        }
00152        else
00153        {
00154            motor_rpm_r=300;
00155            motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00156        }
00157      }
00158      else
00159      {
00160          if(dr>=0)
00161          {
00162           motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00163           motor_rpm_l=(-300);
00164          }
00165          else
00166          {
00167              motor_rpm_r=(-300);
00168              motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00169          }
00170      }
00171     }
00172     else
00173     {
00174         if(dr>=0)
00175         {
00176             motor_rpm_r=100;
00177             motor_rpm_l=-100;
00178         }
00179         else
00180         {
00181             motor_rpm_r=-100;
00182             motor_rpm_l=100;
00183         }
00184     }
00185     vel_data[0]=motor_rpm_r;
00186     vel_data[1]=motor_rpm_l; 
00187     //================================================================ Original Version
00188     /*if (dr>=0)
00189     {
00190         right = ( 1.0 * dx ) + (dr * w /2);
00191         left = ( 1.0 * dx );
00192     }
00193     else
00194     {
00195         right = ( 1.0 * dx );
00196         left = ( 1.0 * dx ) - (dr * w /2);
00197     }
00198         vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60;
00199         vel_data[1] =  left*rate/(Dimeter/2)/(2*3.1416)*60;*/
00200 //===================================================================
00201     Sendmessage(vel_data[0],vel_data[1]);
00202     VelAngular_R.data = vel_data[0];
00203     VelAngular_L.data = vel_data[1];
00204     //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
00205     //}
00206     //else{
00207     pub_rmotor.publish( &VelAngular_R );
00208     pub_lmotor.publish( &VelAngular_L );
00209     //}
00210     //RS232.printf("Wr = %.1f\n",vel_data[0]);
00211     //RS232.printf("Wl = %.1f\n",vel_data[1]);
00212     ticks_since_target += 1;
00213 
00214 }
00215 
00216 void messageCb(const geometry_msgs::Twist &msg)
00217 {
00218     ticks_since_target = 0;
00219     dx = msg.linear.x;
00220     dy = msg.linear.y;
00221     dr = msg.angular.z;
00222     //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr);
00223     TwistToMotors();
00224 }
00225 
00226 void ACT( const std_msgs::String& ACT) {
00227 char sendData[16];
00228 //sendData = ACT;
00229 int motor1,motor2;
00230    
00231     sendData[0] = Start;
00232     sendData[1] = Address;
00233     sendData[2] = ReturnType;
00234     sendData[3] = Clean;
00235     sendData[4] = Reserve;
00236     sendData[5]  = 0x00;//motor1Sevro OFF
00237     sendData[6] = 0x00;//motor2Sevro OFF
00238    motor1 =  0;
00239    motor2 =  0; 
00240     sendData[9] = (motor1>>8);//motor1speedH
00241     sendData[10] = (motor1 & 0xFF);//motor1speedL
00242     sendData[11] = (motor2>>8);//motor2speedH
00243     sendData[12] = (motor2 & 0xFF);//motor2speedL
00244     sendData[13] = End;
00245     
00246     
00247     sendData[14] = Start;
00248     sendData[15] = Start;
00249     int i;
00250     for (i=0;i<16;i++)
00251     {
00252         RS232.printf("%c",sendData[i]);
00253     }
00254     RS232.printf("\r\n");
00255 }
00256 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
00257 ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT );
00258 
00259  Timer t;
00260 int main(int argc, char **argv)
00261 {
00262     long vel_timer = 0;
00263     init_variables();
00264     t.start();
00265     nh.initNode();
00266     nh.advertise(pub_lmotor);
00267     nh.advertise(pub_rmotor);
00268     nh.subscribe(cmd_vel_sub);
00269     nh.subscribe(sub_action);
00270 
00271    while (1)
00272    {
00273      if (t.read_ms() > vel_timer)
00274      {
00275        vel_timer = t.read_ms() + 500;
00276      }
00277      nh.spinOnce();
00278 
00279      wait_ms(10);
00280    }
00281 }