main.cpp

00001 #include "mbed.h"
00002 
00003 #include "ros.h"
00004 #include <nav_msgs/Odometry.h>
00005 #include <geometry_msgs/Twist.h>
00006 #include <geometry_msgs/Quaternion.h>
00007 #include <geometry_msgs/TransformStamped.h>
00008 #include <tf/tf.h>
00009 #include <tf/transform_broadcaster.h>
00010 
00011 #include "actiondrv.h"
00012 
00013 #include "millis.h"
00014 
00015 /*
00016  * ActionEncoder.cpp
00017  *
00018  *  Created on: 7 Mar 2018
00019  *      Author: tung
00020  */
00021 
00022 #include "ActionEncoder.hpp"
00023 #include "Timer.h"
00024 
00025 
00026 
00027 ///////////////////////////
00028 //Serial Action(D8,D2 ); // tx, rx
00029 Serial Action(PB_6,  PB_7 );
00030 //Serial pc(USBTX, USBRX);
00031 
00032 
00033 
00034 union {
00035     uint8_t data[24];
00036     float val[6];
00037 } posture;
00038 int count=0;
00039 int i=0;
00040 int done=0;
00041 float xangle=0;
00042 float yangle=0;
00043 float zangle=0;
00044 float d_angle=0;
00045 float pos_x=0;
00046 float pos_y=0;
00047 float angleSpeed=0;
00048 float temp_zangle=0;
00049 int   LastRead=0;
00050 bool newDataArrived=false;
00051 
00052 char buffer[8];
00053 
00054 
00055 //Serial pc(USBTX, USBRX);
00056 char counter = 0;
00057 actionDrv action1(3); //1
00058 actionDrv action2(1); //2
00059 actionDrv action3(2); //3
00060 
00061 
00062 int motor1 = 0;
00063 int motor2 = 0;
00064 int motor3 = 0;
00065 int motor4 = 0;
00066 
00067 float pi = 3.14159265;
00068 double pi_div_3 = 1.04719755;
00069 float d = 0.525;//0.43;
00070 float wheelR = 0.0508; //4 inch wheel
00071 float gear = 10;
00072 
00073 Ticker motor_updater;
00074 
00075 Ticker odom_updater;
00076 
00077 Ticker ros_updater;
00078 
00079 
00080 ////////////////////////////////////
00081 float now_x=0;
00082 float now_y=0;
00083 float now_w=0;
00084 
00085 float target_x=0;
00086 float target_y=0;
00087 float target_w=0;
00088 
00089 float tolerance_x=0.02;
00090 float tolerance_y=0.02;
00091 float tolerance_w=0.01;
00092 
00093 float speed_max_x=1;
00094 float speed_max_y=1;
00095 float speed_max_w=1;
00096 
00097 long odom_last_read= millis();
00098 
00099 /////////////////////////////////////
00100 const float RATE = 0.18;
00101 //////////////////////////
00102 
00103 ///////////////////////////
00104 float encoder_2_global_angle = -90;         //encoder coordinate system + 30 degree    =>  global coordinate system
00105 //float encoder_2_global_x     =   0.34;    //encoder to center distance  in x   (tung)
00106 //float encoder_2_global_y     =   -0.235;     //encoder to center distance  in y   (tung)
00107 
00108 
00109 float encoder_2_global_x     = 0.33;//0.125;    //encoder to center distance  in x   (tung)
00110 float encoder_2_global_y     = 0.24;//0.37;    //encoder to center distance  in y   (tung)
00111 ////////////////////TUNG////////////////
00112 
00113 
00114 float transformed_real_now_x=0;
00115 float transformed_real_now_y=0;
00116 float transformed_real_now_w=0;
00117 
00118 
00119 float startup_offset_x_encoder =0;
00120 float startup_offset_y_encoder =0;
00121 float startup_offset_w_encoder=0;
00122 
00123 
00124 
00125 float encoder_to_center = sqrt( ( encoder_2_global_x  * encoder_2_global_x )  + ( encoder_2_global_y  *  encoder_2_global_y ) );
00126 
00127 //#########################//
00128 float encoder2local_angle = -90 *pi/float(180);
00129 float encoder_position_angle =( ( 360-36.02737)  ) / float(180) * pi ;   //90 +  angle to encoder location
00130 float r = sqrt( ( encoder_2_global_x  * encoder_2_global_x )  + ( encoder_2_global_y  *  encoder_2_global_y ) );   //encoder to center radius
00131 
00132 
00133 void calculatePos(float _X,float _Y,float _A)
00134 {
00135     float zangle  =  _A-   360 * int(_A / 360);
00136     float zrangle =  zangle *pi/float(180);    //degree 2 rad
00137     
00138     float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) )  -  (  ( _Y / float(1000) )   *  sin( -encoder2local_angle) );
00139     float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) )  +  (  ( _Y / float(1000) )   *  cos( -encoder2local_angle) );
00140     
00141     float s  = copysign( sqrt(  2*( r*r )  -  2*(r*r)*cos(zrangle)  )    , zrangle );
00142     
00143     float x_bias = s * cos( zrangle / 2 );
00144     float y_bias = s * sin( zrangle / 2 );
00145     
00146     float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) )  - ( y_bias ) * ( sin( encoder_position_angle ) )    ;
00147     float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) )  + ( y_bias ) * ( cos( encoder_position_angle ) )    ;
00148     
00149     
00150    transformed_real_now_x = tx + y_tbias   - startup_offset_x_encoder; //-
00151    transformed_real_now_y = ty - x_tbias   - startup_offset_y_encoder;
00152     
00153     //transformed_real_now_x = tx + y_tbias   - startup_offset_x_encoder;  //+
00154     //transformed_real_now_y = ty - x_tbias   - startup_offset_y_encoder;
00155     
00156     
00157     transformed_real_now_w=   _A *pi/float(180)                                                                        -  startup_offset_w_encoder;
00158     
00159     
00160 }
00161 
00162 
00163 
00164 
00165 
00166 ///////////////////////
00167 
00168 
00169 
00170 float x_PID_P = 0.5;
00171 float y_PID_P = 0.5;
00172 float w_PID_P = 1;
00173 
00174 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
00175 
00176 //////////////////////////////
00177 void start_calculatePos(float _X,float _Y,float _A)
00178 {
00179     float zangle  =  _A-   360 * int(_A / 360);
00180     float zrangle =  zangle *pi/float(180);    //degree 2 rad
00181     
00182     float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) )  -  (  ( _Y / float(1000) )   *  sin( -encoder2local_angle) );
00183     float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) )  +  (  ( _Y / float(1000) )   *  cos( -encoder2local_angle) );
00184     
00185     float s  = copysign( sqrt(  2*( r*r )  -  2*(r*r)*cos(zrangle)  )    , zrangle );
00186     
00187     float x_bias = s * cos( zrangle / 2 );
00188     float y_bias = s * sin( zrangle / 2 );
00189     
00190     float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) )  - ( y_bias ) * ( sin( encoder_position_angle ) )    ;
00191     float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) )  + ( y_bias ) * ( cos( encoder_position_angle ) )    ;
00192     
00193     
00194     startup_offset_x_encoder = tx + y_tbias ;  //-
00195     startup_offset_y_encoder = ty - x_tbias ;
00196     
00197    //startup_offset_x_encoder = tx + y_tbias ;
00198    //startup_offset_y_encoder = ty - x_tbias ;  //+
00199     
00200     
00201     startup_offset_w_encoder =  _A *pi/float(180);    //degree 2 rad
00202     
00203     
00204 }
00205 
00206 void ActionEncoder_init()
00207 {
00208     count=0;
00209     i=0;
00210     done=0;
00211     xangle=0;
00212     yangle=0;
00213     zangle=0;
00214     d_angle=0;
00215     pos_x=0;
00216     pos_y=0;
00217     angleSpeed=0;
00218     temp_zangle=0;
00219     LastRead=0;
00220     newDataArrived=false;
00221 
00222 }
00223 
00224 bool readEncoder(char c)
00225 {
00226     //sprintf(buffer,"%02X",c);
00227     //sprintf(buffer,"%X",c);
00228     //pc.printf(buffer);
00229     //pc.printf("\r\n");
00230     
00231     //sprintf(buffer,"%d",count);
00232     //pc.printf(buffer);
00233     //pc.printf("\r\n");
00234     switch(count) {
00235         case 0:
00236             if (c==0x0d) count++;
00237             else count=0;
00238             break;
00239         case 1:
00240             if(c==0x0a) {
00241                 i=0;
00242                 count++;
00243             } else if(c==0x0d) {}
00244             else count=0;
00245             break;
00246         case 2:
00247             posture.data[i]=c;
00248             i++;
00249             if(i>=24) {
00250                 i=0;
00251                 count++;
00252             }
00253             break;
00254         case 3:
00255             if(c==0x0a)count++;
00256             else count=0;
00257             break;
00258         case 4:
00259             if(c==0x0d) {
00260                 d_angle=posture.val[0]-temp_zangle;
00261                 if (d_angle<-180) {
00262                     d_angle=d_angle+360;
00263                 } else if (d_angle>180) {
00264                     d_angle=d_angle-360;
00265                 }
00266                 
00267                 now_w+=d_angle;
00268                 temp_zangle=posture.val[0];
00269                 //xangle=posture.val[1];
00270                 //yangle=posture.val[2];
00271                 now_x=posture.val[3];
00272                 now_y=posture.val[4];
00273                 //angleSpeed=posture.val[5];
00274                 newDataArrived=true;
00275                 
00276             }
00277             count=0;
00278             done=1;
00279             LastRead=millis();
00280             return true;
00281             //break;
00282         default:
00283             count=0;
00284             break;
00285     }
00286     
00287     return false;
00288 }
00289 
00290 
00291 
00292 
00293 //void inverse(float global_x_vel, float global_y_vel, float w_vel) const std_msgs::UInt16& cmd_msg
00294 void inverse(const geometry_msgs::Twist& cmd_vel)  
00295 {
00296     float global_vel_x   =  cmd_vel.linear.x;
00297     float global_vel_y   =  cmd_vel.linear.y;
00298     float w_vel          =  cmd_vel.angular.z;
00299     
00300     float local_x_vel = global_vel_x * cos( -transformed_real_now_w  )  -  global_vel_y * sin( -transformed_real_now_w ); 
00301     float local_y_vel = global_vel_x * sin( -transformed_real_now_w  )  +  global_vel_y * cos( -transformed_real_now_w );  //local to global transformation   (angle only)
00302   
00303     motor1  =  int(   (    (-1) * sin(pi_div_3) * local_x_vel   +  cos(pi_div_3) * local_y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
00304     motor2  =  int(   (    (-1) *  local_y_vel + d * w_vel                                                      )  * 60 / (wheelR * 2 * pi)  * gear   ); 
00305     motor3  =  int(   (           sin(pi_div_3) * local_x_vel   +  cos(pi_div_3) * local_y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
00306     
00307 }
00308 
00309 /////////////////////////
00310 ros::NodeHandle nh;
00311 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &inverse );
00312 
00313 nav_msgs::Odometry odom;
00314 geometry_msgs::TransformStamped odom_trans;
00315  
00316 ros::Publisher odom_publisher("odom", &odom);
00317 tf::TransformBroadcaster odom_broadcaster;
00318 
00319 
00320 /////////////////////////
00321 
00322 
00323 
00324 void motor_update()
00325 {
00326     action1.SetVelocity_mod(motor1  * -1 );
00327     action2.SetVelocity_mod(motor2  * -1 );
00328     action3.SetVelocity_mod(motor3  * -1 );
00329     wait(0.005);
00330 }
00331 
00332 void ros_update()
00333 {
00334 
00335 odom_publisher.publish( &odom );
00336 odom_broadcaster.sendTransform(odom_trans);
00337 
00338 
00339 wait(0.005);
00340 nh.spinOnce();
00341 }
00342 
00343 
00344 void odom_update()
00345 {
00346     calculatePos(now_x,now_y,now_w);
00347     
00348     //geometry_msgs::Quaternion odom_quat = createQuaternionMsg(transformed_real_now_w );
00349     geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(transformed_real_now_w);  // *57.2957795
00350 
00351 
00352     //first, we'll publish the transform over tf
00353     odom_trans.header.stamp = nh.now();
00354     odom_trans.header.frame_id = "odom";
00355     odom_trans.child_frame_id = "base_link";
00356 
00357     odom_trans.transform.translation.x = transformed_real_now_x ;
00358     odom_trans.transform.translation.y = transformed_real_now_y ;
00359     odom_trans.transform.translation.z = 0.0;
00360     odom_trans.transform.rotation = odom_quat;
00361 
00362     //send the transform
00363     
00364 
00365 
00366 
00367     //next, we'll publish the odometry message over ROS
00368     //nav_msgs::Odometry odom;
00369     odom.header.stamp = nh.now();
00370     odom.header.frame_id = "odom";
00371     odom.child_frame_id = "base_link";
00372 
00373     //set the position
00374     odom.pose.pose.position.x = transformed_real_now_x;
00375     odom.pose.pose.position.y = transformed_real_now_y ;
00376     odom.pose.pose.position.z = 0.0;
00377     odom.pose.pose.orientation = odom_quat;
00378 
00379     //set the velocity
00380     //odom.child_frame_id = "base_link";
00381     //odom.twist.twist.linear.x = vx;
00382     //odom.twist.twist.linear.y = vy;
00383     //odom.twist.twist.angular.z = vth;
00384 
00385     //publish the message    
00386     //
00387     
00388             
00389 }
00390     
00391 
00392 
00393 int main() {
00394     
00395     millisStart();
00396     
00397     
00398         
00399     Action.baud(115200);
00400     Action.format(8,SerialBase::None,1); 
00401     ActionEncoder_init();
00402     
00403     nh.initNode();
00404     odom_broadcaster.init(nh);
00405     nh.advertise(odom_publisher);
00406     nh.subscribe(sub);
00407     
00408     while (!nh.connected() )   {nh.spinOnce();}
00409         odom_trans.header.stamp = nh.now();
00410         odom_trans.header.frame_id = "odom";
00411         odom_trans.child_frame_id = "base_link";
00412     
00413         odom_trans.transform.translation.x = 0.0;
00414         odom_trans.transform.translation.y = 0.0 ;
00415         odom_trans.transform.translation.z = 0.0;
00416         odom_trans.transform.rotation = tf::createQuaternionFromYaw(0);
00417         
00418         odom.header.stamp = nh.now();
00419         odom.header.frame_id = "odom";
00420         odom.child_frame_id = "base_link";
00421     
00422         //set the position
00423         odom.pose.pose.position.x = transformed_real_now_x;
00424         odom.pose.pose.position.y = transformed_real_now_y ;
00425         odom.pose.pose.position.z = 0.0;
00426         odom.pose.pose.orientation = tf::createQuaternionFromYaw(0);;
00427         
00428     ros_updater.attach(&ros_update, 0.3);
00429     
00430     while(1) 
00431     {
00432         if (Action.readable())
00433         {
00434             char c = Action.getc();
00435             if (readEncoder(c))
00436             {
00437                 //startup_offset_x_encoder = now_x/1000;
00438                 //startup_offset_y_encoder = now_y/1000;
00439                 //startup_offset_w_encoder = now_w/float(180)*pi;
00440                 
00441                 start_calculatePos(  (now_x),(now_y), now_w  );   //global
00442                 break;
00443             
00444             }
00445             
00446         }
00447     }    //start first to take offset from encoder... in case already moved
00448     
00449     
00450     
00451     for( int a = 1; a < 2; a++ ){
00452       action1.Enable();
00453       action2.Enable();
00454       action3.Enable();
00455       wait(0.1);
00456       action1.SetOperationalMode();
00457       action2.SetOperationalMode();
00458       action3.SetOperationalMode();
00459       wait(0.1);
00460       action1.Configvelocity(100000, 100000);
00461       action2.Configvelocity(100000, 100000);
00462       action3.Configvelocity(100000, 100000);  
00463       wait(0.1);
00464    }
00465           
00466     motor_updater.attach(&motor_update, RATE);  
00467     //odom_updater.attach(&odom_update, RATE);
00468     ros_updater.attach(&ros_update, 0.5);
00469     
00470 
00471     
00472         
00473     while(1) 
00474     {
00475         if (Action.readable())
00476         {
00477             //pc.putc('a');
00478             char c = Action.getc();
00479             if(readEncoder(c)) odom_update();
00480         }
00481         
00482     }
00483 
00484        
00485 }