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.2 ;
00093     rate = 50;
00094     timeout_ticks = 2;
00095     Dimeter = 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     double vel_data[2];
00136     right = ( 1.0 * dx ) + (dr * w /2);
00137     left = ( 1.0 * dx ) - (dr * w /2);
00138     vel_data[0] = right*rate/Dimeter/60*1000;
00139     vel_data[1] =  left*rate/Dimeter/60*1000;
00140     Sendmessage(vel_data[0],vel_data[1]);
00141     VelAngular_R.data = vel_data[0];
00142     VelAngular_L.data = vel_data[1];
00143     //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
00144     //}
00145     //else{
00146     pub_rmotor.publish( &VelAngular_R );
00147     pub_lmotor.publish( &VelAngular_L );
00148     //}
00149     //RS232.printf("Wr = %.1f\n",vel_data[0]);
00150     //RS232.printf("Wl = %.1f\n",vel_data[1]);
00151     ticks_since_target += 1;
00152 
00153 }
00154 
00155 void messageCb(const geometry_msgs::Twist &msg)
00156 {
00157     ticks_since_target = 0;
00158     dx = msg.linear.x;
00159     dy = msg.linear.y;
00160     dr = msg.angular.z;
00161     //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr);
00162     TwistToMotors();
00163 }
00164 
00165 void ACT( const std_msgs::String& ACT) {
00166 char sendData[16];
00167 //sendData = ACT;
00168 int motor1,motor2;
00169    
00170     sendData[0] = Start;
00171     sendData[1] = Address;
00172     sendData[2] = ReturnType;
00173     sendData[3] = Clean;
00174     sendData[4] = Reserve;
00175     sendData[5]  = 0x00;//motor1Sevro OFF
00176     sendData[6] = 0x00;//motor2Sevro OFF
00177    motor1 =  0;
00178    motor2 =  0; 
00179     sendData[9] = (motor1>>8);//motor1speedH
00180     sendData[10] = (motor1 & 0xFF);//motor1speedL
00181     sendData[11] = (motor2>>8);//motor2speedH
00182     sendData[12] = (motor2 & 0xFF);//motor2speedL
00183     sendData[13] = End;
00184     
00185     
00186     sendData[14] = Start;
00187     sendData[15] = Start;
00188     int i;
00189     for (i=0;i<16;i++)
00190     {
00191         RS232.printf("%c",sendData[i]);
00192     }
00193     RS232.printf("\r\n");
00194 }
00195 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
00196 ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT );
00197 
00198  Timer t;
00199 int main(int argc, char **argv)
00200 {
00201     long vel_timer = 0;
00202     init_variables();
00203     t.start();
00204     nh.initNode();
00205     nh.advertise(pub_lmotor);
00206     nh.advertise(pub_rmotor);
00207     nh.subscribe(cmd_vel_sub);
00208     nh.subscribe(sub_action);
00209 
00210    while (1)
00211    {
00212      if (t.read_ms() > vel_timer)
00213      {
00214        vel_timer = t.read_ms() + 500;
00215      }
00216      nh.spinOnce();
00217 
00218      wait_ms(10);
00219    }
00220 }