Diff: main.cpp

Revision:

2:611a5eb132a1

Parent:

1:58d1caed28d4

diff -r 58d1caed28d4 -r 611a5eb132a1 main.cpp
--- a/main.cpp	Thu Oct 25 12:14:32 2018 +0000
+++ b/main.cpp	Sun Sep 13 04:30:54 2020 +0000
@@ -1,4 +1,13 @@
 #include "mbed.h"
+
+#include "ros.h"
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+
 #include "actiondrv.h"
 
 #include "millis.h"
@@ -18,7 +27,7 @@
 ///////////////////////////
 //Serial Action(D8,D2 ); // tx, rx
 Serial Action(PB_6,  PB_7 );
-Serial pc(USBTX, USBRX);
+//Serial pc(USBTX, USBRX);
 
 
 
@@ -41,13 +50,13 @@
 bool newDataArrived=false;
 
 char buffer[8];
-/////////////////////////
+
 
 //Serial pc(USBTX, USBRX);
 char counter = 0;
-actionDrv action1(1);
-actionDrv action2(2);
-actionDrv action3(3);
+actionDrv action1(3); //1
+actionDrv action2(1); //2
+actionDrv action3(2); //3
 
 
 int motor1 = 0;
@@ -64,6 +73,10 @@
 Ticker motor_updater;
 
 Ticker odom_updater;
+
+Ticker ros_updater;
+
+
 ////////////////////////////////////
 float now_x=0;
 float now_y=0;
@@ -85,36 +98,16 @@
 
 /////////////////////////////////////
 const float RATE = 0.18;
-
-///////////////////////////////////////
-int point_counter=0;
-
-struct point_info
-{
-   float required_x;
-   float required_y;
-   float required_w;
-   float required_tolerance_x;
-   float required_tolerance_y;
-   float required_tolerance_w;
-   float required_speed_max_x;
-   float required_speed_max_y;
-   float required_speed_max_w;
-};
-
-struct point_info points[100];
-
-
-
+//////////////////////////
 
 ///////////////////////////
-float encoder_2_global_angle = 30;         //encoder coordinate system + 30 degree    =>  global coordinate system
+float encoder_2_global_angle = -90;         //encoder coordinate system + 30 degree    =>  global coordinate system
 //float encoder_2_global_x     =   0.34;    //encoder to center distance  in x   (tung)
 //float encoder_2_global_y     =   -0.235;     //encoder to center distance  in y   (tung)
 
 
-float encoder_2_global_x     = 0.125;//0.125;// -0.13 ;    //encoder to center distance  in x   (tung)
-float encoder_2_global_y     = 0.37; //0.35;     //encoder to center distance  in y   (tung)
+float encoder_2_global_x     = 0.33;//0.125;    //encoder to center distance  in x   (tung)
+float encoder_2_global_y     = 0.24;//0.37;    //encoder to center distance  in y   (tung)
 ////////////////////TUNG////////////////
 
 
@@ -132,8 +125,8 @@
 float encoder_to_center = sqrt( ( encoder_2_global_x  * encoder_2_global_x )  + ( encoder_2_global_y  *  encoder_2_global_y ) );
 
 //#########################//
-float encoder2local_angle = 30 *pi/float(180);
-float encoder_position_angle =( ( 180 + 18.666914)  ) / float(180) * pi ;   //90 +  angle to encoder location
+float encoder2local_angle = -90 *pi/float(180);
+float encoder_position_angle =( ( 360-36.02737)  ) / float(180) * pi ;   //90 +  angle to encoder location
 float r = sqrt( ( encoder_2_global_x  * encoder_2_global_x )  + ( encoder_2_global_y  *  encoder_2_global_y ) );   //encoder to center radius
 
 
@@ -154,11 +147,11 @@
     float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) )  + ( y_bias ) * ( cos( encoder_position_angle ) )    ;
     
     
-   // transformed_real_now_x = tx - x_tbias   - startup_offset_x_encoder;
-   // transformed_real_now_y = ty - y_tbias   - startup_offset_y_encoder;
+   transformed_real_now_x = tx + y_tbias   - startup_offset_x_encoder; //-
+   transformed_real_now_y = ty - x_tbias   - startup_offset_y_encoder;
     
-    transformed_real_now_x = tx + y_tbias   - startup_offset_x_encoder;
-    transformed_real_now_y = ty - x_tbias   - startup_offset_y_encoder;
+    //transformed_real_now_x = tx + y_tbias   - startup_offset_x_encoder;  //+
+    //transformed_real_now_y = ty - x_tbias   - startup_offset_y_encoder;
     
     
     transformed_real_now_w=   _A *pi/float(180)                                                                        -  startup_offset_w_encoder;
@@ -198,11 +191,11 @@
     float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) )  + ( y_bias ) * ( cos( encoder_position_angle ) )    ;
     
     
-  //  startup_offset_x_encoder = tx - x_tbias ;
-  //  startup_offset_y_encoder = ty - y_tbias ;
+    startup_offset_x_encoder = tx + y_tbias ;  //-
+    startup_offset_y_encoder = ty - x_tbias ;
     
-    startup_offset_x_encoder = tx + y_tbias ;
-    startup_offset_y_encoder = ty - x_tbias ;
+   //startup_offset_x_encoder = tx + y_tbias ;
+   //startup_offset_y_encoder = ty - x_tbias ;  //+
     
     
     startup_offset_w_encoder =  _A *pi/float(180);    //degree 2 rad
@@ -210,12 +203,6 @@
     
 }
 
-
-
-
-
-
-
 void ActionEncoder_init()
 {
     count=0;
@@ -300,82 +287,37 @@
     return false;
 }
 
-bool updated()
-{
-    if (done) {
-        done=false;
-        return true;
-    } else {
-        return false;
-    }
 
-}
 
-float getXangle()
-{
-    return xangle;
-}
 
-float getYangle()
-{
-    return yangle;
-}
-
-float getZangle()
+//void inverse(float global_x_vel, float global_y_vel, float w_vel) const std_msgs::UInt16& cmd_msg
+void inverse(const geometry_msgs::Twist& cmd_vel)  
 {
-    return zangle;
-}
-
-float getXpos()
-{
-    return pos_x;
-}
-
-float getYpos()
-{
-    return pos_y;
+    float global_vel_x   =  cmd_vel.linear.x;
+    float global_vel_y   =  cmd_vel.linear.y;
+    float w_vel          =  cmd_vel.angular.z;
+    
+    float local_x_vel = global_vel_x * cos( -transformed_real_now_w  )  -  global_vel_y * sin( -transformed_real_now_w ); 
+    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)
+  
+    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   );
+    motor2  =  int(   (    (-1) *  local_y_vel + d * w_vel                                                      )  * 60 / (wheelR * 2 * pi)  * gear   ); 
+    motor3  =  int(   (           sin(pi_div_3) * local_x_vel   +  cos(pi_div_3) * local_y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
+    
 }
 
-float getAngleSpeed()
-{
-    return angleSpeed;
-}
-
-bool isAlive()
-{
-    if ((millis()-LastRead)<100) {
-        return true;
-    } else {
-        return false;
-    }
-}
+/////////////////////////
+ros::NodeHandle nh;
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &inverse );
 
-bool newDataAvailable()
-{
-    if (newDataArrived) {
-        newDataArrived=false;
-        return true;
-    } else return false;
-}
-
-char* reset()
-{
-    return "ACT0";
-}
-
-char* calibrate()
-{
-    return "ACTR";
-}
+nav_msgs::Odometry odom;
+geometry_msgs::TransformStamped odom_trans;
+ 
+ros::Publisher odom_publisher("odom", &odom);
+tf::TransformBroadcaster odom_broadcaster;
 
 
-void inverse(float x_vel, float y_vel, float w_vel)
-{
-    motor1  =  int(   (    (-1) * sin(pi_div_3) * x_vel   +  cos(pi_div_3) * y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
-    motor2  =  int(   (    (-1) *  y_vel + d * w_vel                                                )  * 60 / (wheelR * 2 * pi)  * gear   ); 
-    motor3  =  int(   (           sin(pi_div_3) * x_vel   +  cos(pi_div_3) * y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
-    
-}
+/////////////////////////
 
 
 
@@ -387,111 +329,69 @@
     wait(0.005);
 }
 
+void ros_update()
+{
+
+odom_publisher.publish( &odom );
+odom_broadcaster.sendTransform(odom_trans);
+
+
+wait(0.005);
+nh.spinOnce();
+}
+
+
 void odom_update()
 {
-
-   
     calculatePos(now_x,now_y,now_w);
     
-    
-    /*sprintf(buffer, "%f", transformed_real_now_x);
-    pc.printf(buffer);
-    pc.printf("  ");
-    sprintf(buffer, "%f", transformed_real_now_y);
-    pc.printf(buffer);
-    pc.printf("  ");
-    sprintf(buffer, "%f", transformed_real_now_w);
-    pc.printf(buffer);
-    pc.printf("\r\n");
-    */
-    
-    
-    if ((    (fabs(target_x - transformed_real_now_x)) < tolerance_x ) && (   (fabs(target_y - transformed_real_now_y)) < tolerance_y )  && (   (fabs(target_w - transformed_real_now_w)) < tolerance_w )      )
-    {
-        point_counter+=1;
+    //geometry_msgs::Quaternion odom_quat = createQuaternionMsg(transformed_real_now_w );
+    geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(transformed_real_now_w);  // *57.2957795
 
-       tolerance_x = points[point_counter].required_tolerance_x;
-       tolerance_y = points[point_counter].required_tolerance_x;
-       tolerance_w = points[point_counter].required_tolerance_x;
-       
-       target_x    = points[point_counter].required_x                    ; //+    startup_offset_x_encoder;
-       target_y    = points[point_counter].required_y                    ; //+    startup_offset_y_encoder;
-       target_w    = points[point_counter].required_w  *pi/float(180);       ;//-    startup_offset_w_encoder;
-       
-       inverse( 0    ,     0         ,     0   );
-       return;
-     
-    }
-    
 
-  
-    float local_vel_x = (fabs(target_x - transformed_real_now_x) > tolerance_x ) ?   constrain(  (x_PID_P * (target_x - transformed_real_now_x)    ), -speed_max_x,    speed_max_x)  : 0 ;
-    float local_vel_y = (fabs(target_y - transformed_real_now_y) > tolerance_y ) ?   constrain(  (y_PID_P * (target_y - transformed_real_now_y)    ), -speed_max_y,    speed_max_y)  : 0 ;
-    float local_vel_w = (fabs(target_w - transformed_real_now_w) > tolerance_w ) ?   constrain(  (w_PID_P * (target_w - transformed_real_now_w)    ), -speed_max_w,    speed_max_w)  : 0 ;
-    
-    
-    
-    float global_vel_x = local_vel_x * cos( -transformed_real_now_w  )  -  local_vel_y * sin( -transformed_real_now_w ); 
-    float global_vel_y = local_vel_x * sin( -transformed_real_now_w  )  +  local_vel_y * cos( -transformed_real_now_w );  //local to global transformation   (angle only)
-    
-    /*
-    pc.printf("X: ");
-    sprintf(buffer, "%f", transformed_real_now_x);
-    pc.printf(buffer);
-    pc.printf("  Y: ");
-    sprintf(buffer, "%f", transformed_real_now_y);
-    pc.printf(buffer);
-    pc.printf("  W: ");
-    sprintf(buffer, "%f", transformed_real_now_w);
-    pc.printf(buffer);
-    
-    pc.printf(" | Global: ");
-    sprintf(buffer, "%f", global_vel_x);
-    pc.printf(buffer);
-    pc.printf("  ");
-    sprintf(buffer, "%f", global_vel_y);
-    pc.printf(buffer);
-    pc.printf("  ");
-    sprintf(buffer, "%f", local_vel_w);
-    pc.printf(buffer);*/
-    
-    
-  
-    inverse( global_vel_x   ,  global_vel_y       ,     local_vel_w   );
-    
-    /*
-    pc.printf(" | Motor: ");
-    sprintf(buffer, "%d", motor1);
-    pc.printf(buffer);
-    pc.printf("  ");
-    sprintf(buffer, "%d", motor2);
-    pc.printf(buffer);
-    pc.printf("  ");
-    sprintf(buffer, "%d", motor3);
-    pc.printf(buffer);
-    pc.printf("\r\n");*/
-    
-}
+    //first, we'll publish the transform over tf
+    odom_trans.header.stamp = nh.now();
+    odom_trans.header.frame_id = "odom";
+    odom_trans.child_frame_id = "base_link";
 
-int main() {
-    //while(1){
-////////////////////////////
-    points[0] = (point_info){.required_x = 0,.required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[1] = (point_info){.required_x = 0.5,.required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[2] = (point_info){.required_x = 0,  .required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[3] = (point_info){.required_x = 0,  .required_y = 0.2, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[4] = (point_info){.required_x = 0,  .required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[5] = (point_info){.required_x = 0,  .required_y = 0, .required_w = 90, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[6] = (point_info){.required_x = 0,  .required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[7] = (point_info){.required_x = 0,  .required_y = 0, .required_w = -90, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
-    points[8] = (point_info){.required_x = 0,  .required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
+    odom_trans.transform.translation.x = transformed_real_now_x ;
+    odom_trans.transform.translation.y = transformed_real_now_y ;
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation = odom_quat;
+
+    //send the transform
     
 
 
 
+    //next, we'll publish the odometry message over ROS
+    //nav_msgs::Odometry odom;
+    odom.header.stamp = nh.now();
+    odom.header.frame_id = "odom";
+    odom.child_frame_id = "base_link";
 
-////////////////////
-        
+    //set the position
+    odom.pose.pose.position.x = transformed_real_now_x;
+    odom.pose.pose.position.y = transformed_real_now_y ;
+    odom.pose.pose.position.z = 0.0;
+    odom.pose.pose.orientation = odom_quat;
+
+    //set the velocity
+    //odom.child_frame_id = "base_link";
+    //odom.twist.twist.linear.x = vx;
+    //odom.twist.twist.linear.y = vy;
+    //odom.twist.twist.angular.z = vth;
+
+    //publish the message    
+    //
+    
+            
+}
+    
+
+
+int main() {
+    
     millisStart();
     
     
@@ -499,6 +399,34 @@
     Action.baud(115200);
     Action.format(8,SerialBase::None,1); 
     ActionEncoder_init();
+    
+    nh.initNode();
+    odom_broadcaster.init(nh);
+    nh.advertise(odom_publisher);
+    nh.subscribe(sub);
+    
+    while (!nh.connected() )   {nh.spinOnce();}
+        odom_trans.header.stamp = nh.now();
+        odom_trans.header.frame_id = "odom";
+        odom_trans.child_frame_id = "base_link";
+    
+        odom_trans.transform.translation.x = 0.0;
+        odom_trans.transform.translation.y = 0.0 ;
+        odom_trans.transform.translation.z = 0.0;
+        odom_trans.transform.rotation = tf::createQuaternionFromYaw(0);
+        
+        odom.header.stamp = nh.now();
+        odom.header.frame_id = "odom";
+        odom.child_frame_id = "base_link";
+    
+        //set the position
+        odom.pose.pose.position.x = transformed_real_now_x;
+        odom.pose.pose.position.y = transformed_real_now_y ;
+        odom.pose.pose.position.z = 0.0;
+        odom.pose.pose.orientation = tf::createQuaternionFromYaw(0);;
+        
+    ros_updater.attach(&ros_update, 0.3);
+    
     while(1) 
     {
         if (Action.readable())
@@ -518,10 +446,6 @@
         }
     }    //start first to take offset from encoder... in case already moved
     
- 
-   target_x    = points[0].required_x;  //  +  startup_offset_x_encoder;
-   target_y    = points[0].required_y;  //  +  startup_offset_y_encoder;
-   target_w    = points[0].required_w *pi/float(180);   // -  startup_offset_w_encoder;
     
     
     for( int a = 1; a < 2; a++ ){
@@ -541,6 +465,9 @@
           
     motor_updater.attach(&motor_update, RATE);  
     //odom_updater.attach(&odom_update, RATE);
+    ros_updater.attach(&ros_update, 0.5);
+    
+
     
         
     while(1) 
@@ -554,38 +481,5 @@
         
     }
 
- 
-        
-/*
-        while (Action.readable()==1 ) 
-        {
-            char c = Action.getc();   
-            readEncoder(c);
-           
-        }
-*/
-    
-    
-/*
-    while(1)
-    {
-         
-         inverse(0.2,0,0);
-         wait(1);
-         inverse(-0.2,0,0);
-         wait(1);
-         
-         inverse(0,0,0.25);
-         wait(1);
-         inverse(0,0,-0.25);
-         wait(1);
-         
-    }
-    
-*/
-         
-
-    
-
        
 }