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 
00024 // ============= =============
00025         float motor_rpm_r, motor_rpm_l;
00026 // ============= =============
00027         
00028     ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
00029     ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
00030     int seq = 0; // monitor times of callbacks
00031     int old_seq = 0; // monitor times of the command
00032     
00033     
00034     ros::NodeHandle nh;
00035     geometry_msgs::Twist vel;
00036     //float left;
00037     //float right;
00038     float Lrpm,Rrpm;
00039     float ticks_since_target;
00040     double timeout_ticks;
00041 
00042     double w;
00043     double rate;
00044     double Dimeter;
00045     float dx,dy,dr;
00046     void init_variables();
00047 void Sendmessage(float Rrpm,float Lrpm);
00048 
00049 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
00050 {
00051     unsigned int i, j;
00052     //#define wPolynom 0xA001
00053     unsigned int wCrc = 0xffff;
00054     unsigned int wPolynom = 0xA001;
00055     /*---------------------------------------------------------------------------------*/
00056     for (i = 0; i < iBufLen; i++)
00057     {
00058         wCrc ^= cBuffer[i];
00059         for (j = 0; j < 8; j++)
00060         {
00061             if (wCrc &0x0001)
00062             {
00063                 wCrc = (wCrc >> 1) ^ wPolynom;
00064             }
00065             else
00066             { 
00067                 wCrc = wCrc >> 1;
00068             }
00069         }
00070     }
00071     return wCrc;
00072 }
00073 
00074  /*
00075  起始碼(1Byte):                            AA
00076 地址碼(1Byte):                             01
00077 返回數據類型(1Byte):從站驅動器不返回數據       00
00078 故障清除(1Byte)默認發送:                     00
00079 保留位(1Byte)                              00
00080 A 馬達過載控制(1Byte)驅動器使能狀態           01
00081 B 馬達控制(1Byte)                           01
00082 A 運行方向(1Byte)00:正轉,01反轉             00
00083 B 運行方向(1Byte)00:正轉,01反轉             00
00084 運行轉速(2Byte)是100–3000r/min,            12 34
00085 運行轉速(2Byte)是100–3000r/min,            12 34
00086 第 1 個 Byte:速度值高 8 位;                
00087 第 2 個 Byte:速度值低 8 位;
00088 無效(2Byte)00:無效數據位。                   00
00089 結束碼(1Byte)55:通訊結束編碼識別。            55
00090 校驗碼(2Byte)CRC16 
00091  */
00092 
00093  //   void spinOnce();
00094  //   void twistCallback(const geometry_msgs::Twist &twist_aux);
00095 
00096 
00097 void init_variables()
00098 {
00099     dx = dy = dr =0;
00100     w = 0.302;//0.2 ;//m
00101     rate = 20;//50;
00102     timeout_ticks = 2;
00103     Dimeter = 0.127;//0.15;
00104 }
00105 void Sendmessage(float Rrpm,float Lrpm)
00106 {
00107     //RS232.printf("Wr = %.1f\n",Rrpm);
00108     //RS232.printf("Wl = %.1f\n",Lrpm);
00109 unsigned char sendData[16];
00110 unsigned int tmpCRC;
00111 int motor1,motor2;
00112    
00113     sendData[0] = Start;
00114     sendData[1] = Address;
00115     sendData[2] = ReturnType;
00116     sendData[3] = Clean;
00117     sendData[4] = Reserve;
00118     sendData[5]  = 0x01;//motor1Sevro ON
00119     sendData[6] = 0x01;//motor2Sevro ON
00120 if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
00121 if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
00122    motor1 =  abs(Rrpm);
00123    motor2 =  abs(Lrpm); 
00124     
00125     sendData[9] = (motor1>>8);//motor1speedH
00126     sendData[10] = (motor1 & 0xFF);//motor1speedL
00127     sendData[11] = (motor2>>8);//motor2speedH
00128     sendData[12] = (motor2 & 0xFF);//motor2speedL
00129     sendData[13] = End;
00130     tmpCRC = CRC_Verify(sendData, 14);
00131     sendData[14] = (tmpCRC & 0xFF);
00132     sendData[15] = (tmpCRC>>8);
00133     int i;
00134     for (i=0;i<16;i++)
00135     {
00136         RS232.printf("%c",sendData[i]);
00137     }
00138     RS232.printf("\r\n");
00139 }
00140 
00141 
00142 int myabs( int a ){
00143     if ( a < 0 ){
00144         return -a;
00145         }
00146         return a;
00147         }
00148         
00149         
00150         
00151 void TwistToMotors()
00152 {
00153     old_seq = seq;
00154     seq = seq + 1;    
00155     //int lower_bound = 100;
00156     //int upper_bound = 300;
00157     float right,left;
00158 //    float motor_rpm_r, motor_rpm_l;
00159     //double vel_data[2];
00160     float vel_data[2];
00161     float motor_rpm_vx, motor_rpm_theta, motor_rpm_v;
00162 
00163     // prevent agv receive weird 1.0 command from cmd_vel
00164 
00165     right = ( 1.0 * dx ) + (dr * w /2);
00166     left = ( 1.0 * dx ) - (dr * w /2);
00167     motor_rpm_v = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
00168     
00169     if((motor_rpm_v!=0) && (myabs(motor_rpm_v)<100)){
00170         if (motor_rpm_v > 0)  motor_rpm_v=100;
00171         else motor_rpm_v = -100;
00172         }
00173 
00174     motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);
00175 
00176 
00177     //motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
00178     //motor_rpm_l =  left*rate/(Dimeter/2)*60/(2*3.1416);
00179 
00180     motor_rpm_r = motor_rpm_v + motor_rpm_theta;
00181     motor_rpm_l = motor_rpm_v - motor_rpm_theta;
00182     
00183         
00184         if (myabs(motor_rpm_r)<100 || myabs(motor_rpm_l)<100){
00185             
00186         if(dx == 0){
00187             if(dr > 0){
00188                 motor_rpm_r = 100;
00189                 motor_rpm_l = -100;
00190                 }else if( dr < 0 ){
00191                     motor_rpm_r = -100;
00192                     motor_rpm_l = 100;
00193                     }else{
00194                         motor_rpm_r = 0;
00195                         motor_rpm_l = 0;
00196                         }
00197             }
00198             else if( dx > 0 ){
00199                 if( myabs(motor_rpm_r) < 100 ){
00200                     motor_rpm_r = 100;
00201                     motor_rpm_l=motor_rpm_l+(100-motor_rpm_r);
00202                 }
00203                 if ( myabs(motor_rpm_l) < 100 ){
00204                     motor_rpm_l = 100;
00205                     motor_rpm_r=motor_rpm_r+(100-motor_rpm_l);
00206                     }
00207             }else{
00208                     if ( myabs(motor_rpm_r) < 100 ){
00209                         motor_rpm_r = -100;
00210                         motor_rpm_l=motor_rpm_l+(-100-motor_rpm_r);
00211                     }
00212                     
00213                     if (myabs(motor_rpm_l) < 100 ){
00214                         motor_rpm_l = -100;
00215                         motor_rpm_r=motor_rpm_r+(-100-motor_rpm_l);
00216                     }
00217             }
00218     }
00219                                  
00220     /*
00221     if ( myabs(motor_rpm_r) == myabs(motor_rpm_l) && myabs(motor_rpm_r) < 100){
00222         int s = myabs(motor_rpm_r);
00223         
00224         motor_rpm_r = motor_rpm_r / s * 101;
00225         motor_rpm_l = motor_rpm_l / s * 101;
00226         
00227         }
00228 */
00229 
00230 /*
00231     if ( myabs(vel_data[0]) < lower_bound )
00232     { // if right wheel rpm < lower_bound
00233             int a = 0;
00234             a = lower_bound - myabs( vel_data[0] );
00235         
00236         if (vel_data[0] > 0)
00237         {
00238             vel_data[0] = lower_bound;
00239         }
00240         else
00241         {
00242             vel_data[0] = -lower_bound;
00243         }
00244             
00245             
00246         if (vel_data[1] > 0)
00247         {
00248             vel_data[1] = vel_data[1] + a;
00249         }
00250         else
00251         {
00252             vel_data[1] = vel_data[1] - a;
00253         } //
00254         
00255     }
00256         
00257     else if ( myabs(vel_data[0]) > upper_bound )
00258     {  // if right wheel rpm > upper_bound
00259             int a = 0;
00260             a = myabs( vel_data[0] ) - upper_bound;
00261         
00262         if (vel_data[0] > 0)
00263         {
00264             vel_data[0] = upper_bound;
00265         }
00266         else
00267         {
00268             vel_data[0] = -upper_bound;
00269         }
00270             
00271             
00272         if (vel_data[1] > 0)
00273         {
00274             vel_data[1] = vel_data[1] - a;
00275         }
00276         else
00277         {
00278             vel_data[1] = vel_data[1] + a;
00279         }
00280     }
00281         
00282                
00283     if ( myabs(vel_data[1]) < lower_bound )
00284     { // if left wheel rpm < lower_bound
00285             int a = 0;
00286             a = lower_bound - myabs( vel_data[1] );
00287         
00288         if (vel_data[1] > 0)
00289         {
00290             vel_data[1] = lower_bound;
00291         }
00292         else
00293         {
00294             vel_data[1] = -lower_bound;
00295         }
00296             
00297             
00298         if (vel_data[0] > 0)
00299         {
00300             vel_data[0] = vel_data[0] + a;
00301         }
00302         else
00303         {
00304             vel_data[0] = vel_data[0] - a;
00305         } //
00306     }
00307         
00308         
00309     else if (myabs(vel_data[1]) > upper_bound )
00310     { // if left wheel rpm > upper_bound
00311             int a = 0;
00312             a = myabs( vel_data[1] ) - upper_bound;
00313         
00314         if (vel_data[1] > 0)
00315         {
00316             vel_data[1] = upper_bound;
00317         }
00318         else
00319         {
00320             vel_data[1] = -upper_bound;
00321         }
00322             
00323             
00324         if (vel_data[0] > 0)
00325         {
00326             vel_data[0] = vel_data[0] - a;
00327         }
00328         else
00329         {
00330             vel_data[0] = vel_data[0] + a;
00331         }
00332             
00333     }
00334                
00335         */
00336         
00337         /*
00338         if ( myabs(motor_rpm_r) < 100 || myabs(motor_rpm_l) < 100 ){
00339             float a = 0.0;
00340         
00341             if ( myabs(motor_rpm_r) < myabs(motor_rpm_l) ){
00342                 
00343                 if ( motor_rpm_r < 0 ){
00344                     a = -100 - motor_rpm_r;
00345                 }else if( motor_rpm_r > 0) {
00346                     a = 100 - motor_rpm_r;
00347                     }else {
00348                         a = 0;
00349                 }
00350                 
00351                 motor_rpm_r = motor_rpm_r + a;
00352                 motor_rpm_l = motor_rpm_l + a;
00353                 
00354                     
00355                 
00356             }else if ( myabs(motor_rpm_r) > myabs(motor_rpm_l) ){
00357                 
00358                 if ( motor_rpm_l < 0 ){
00359                     a = -100 - motor_rpm_l;
00360                 }else if( motor_rpm_l > 0) {
00361                     a = 100 - motor_rpm_l;
00362                     }else {
00363                         a = 0;
00364                 }
00365                 
00366                 motor_rpm_l = motor_rpm_l + a;
00367                 motor_rpm_r = motor_rpm_r + a;
00368                 
00369             }
00370             
00371             // go straight < 100
00372             if ( motor_rpm_r == motor_rpm_l ){
00373                 
00374                 if ( motor_rpm_l < 0 ){
00375                     motor_rpm_l = -100;
00376                     motor_rpm_r = -100;
00377                 
00378                 
00379                 }else if( motor_rpm_l > 0) {
00380                     motor_rpm_l = 100;
00381                     motor_rpm_r = 100;
00382                     }else {
00383                         motor_rpm_l = 0;
00384                         motor_rpm_r = 0;
00385                     }           
00386                 
00387                 
00388                 }
00389                 
00390                 // turn < 100
00391                 
00392                 if ( motor_rpm_r == -motor_rpm_l ){
00393                 
00394                 if ( motor_rpm_l < 0 ){
00395                 motor_rpm_l = -100;
00396                 motor_rpm_r = 100;
00397                 }else if( motor_rpm_l > 0) {
00398                 motor_rpm_l = 100;
00399                 motor_rpm_r = -100;
00400                 }else {
00401                     motor_rpm_l = 0;
00402                     motor_rpm_r = 0;
00403                 }
00404                                
00405             }
00406             //
00407             
00408             } 
00409             
00410 
00411             
00412         if ( myabs(motor_rpm_r) > 600 ){
00413                
00414                if ( motor_rpm_r < 0 ){
00415                motor_rpm_r = -100 * log( float(myabs(motor_rpm_r)) );
00416                }else{
00417                    motor_rpm_r = 100 * log( float(myabs(motor_rpm_r)) );
00418                }
00419                
00420 
00421                
00422         }
00423         
00424         if ( myabs(motor_rpm_l) > 600) {
00425         
00426                if ( motor_rpm_l < 0 ){
00427                motor_rpm_l = -100 * log( float(myabs(motor_rpm_l)) );
00428                }else{
00429                    motor_rpm_l = 100 * log( float(myabs(motor_rpm_l)) );
00430                }
00431         }
00432         
00433         */
00434         vel_data[0] = motor_rpm_r;
00435         vel_data[1] = motor_rpm_l;
00436         
00437         
00438         
00439         
00440 
00441 //===================================================================
00442     //Sendmessage(vel_data[0],vel_data[1]);
00443     
00444     
00445     VelAngular_R.data = vel_data[0];
00446     VelAngular_L.data = vel_data[1];
00447     //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
00448     //}
00449     //else{
00450     pub_rmotor.publish( &VelAngular_R );
00451     pub_lmotor.publish( &VelAngular_L );
00452     //}
00453     //RS232.printf("Wr = %.1f\n",vel_data[0]);
00454     //RS232.printf("Wl = %.1f\n",vel_data[1]);
00455     ticks_since_target += 1;
00456 
00457 }
00458 
00459 void messageCb(const geometry_msgs::Twist &msg)
00460 {
00461     ticks_since_target = 0;
00462     dx = msg.linear.x;
00463     dy = msg.linear.y;
00464     dr = msg.angular.z;
00465     //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr);
00466     TwistToMotors();
00467 }
00468 
00469 void ACT( const std_msgs::String& ACT) {
00470 char sendData[16];
00471 //sendData = ACT;
00472 int motor1,motor2;
00473    
00474     sendData[0] = Start;
00475     sendData[1] = Address;
00476     sendData[2] = ReturnType;
00477     sendData[3] = Clean;
00478     sendData[4] = Reserve;
00479     sendData[5]  = 0x00;//motor1Sevro OFF
00480     sendData[6] = 0x00;//motor2Sevro OFF
00481    motor1 =  0;
00482    motor2 =  0; 
00483     sendData[9] = (motor1>>8);//motor1speedH
00484     sendData[10] = (motor1 & 0xFF);//motor1speedL
00485     sendData[11] = (motor2>>8);//motor2speedH
00486     sendData[12] = (motor2 & 0xFF);//motor2speedL
00487     sendData[13] = End;
00488     
00489     
00490     sendData[14] = Start;
00491     sendData[15] = Start;
00492     int i;
00493     for (i=0;i<16;i++)
00494     {
00495         RS232.printf("%c",sendData[i]);
00496     }
00497     RS232.printf("\r\n");
00498 }
00499 
00500 
00501 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
00502 //ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT );
00503 
00504 
00505  Timer t;
00506 int main(int argc, char **argv)
00507 {   
00508     long vel_timer = 0;
00509     init_variables();
00510     t.start();
00511     nh.initNode();
00512     nh.advertise(pub_lmotor);
00513     nh.advertise(pub_rmotor);
00514     nh.subscribe(cmd_vel_sub);
00515     //nh.subscribe(sub_action);
00516 
00517    while (true)
00518    {
00519      //if (t.read_ms() > vel_timer)
00520      //{
00521      //  vel_timer = t.read_ms() + 50;
00522      //  TwistToMotors();
00523      // }
00524      
00525 
00526      nh.spinOnce();
00527      
00528      
00529      if (seq +50 < old_seq){
00530      motor_rpm_r = 0;
00531      motor_rpm_l = 0;
00532      }
00533      
00534 //     Sendmessage(-motor_rpm_r,-motor_rpm_l);
00535      Sendmessage(-motor_rpm_l,-motor_rpm_r);
00536      
00537      old_seq = old_seq + 1;
00538     
00539      wait_ms(10);
00540     
00541    }
00542 }