Diff: main.cpp

Revision:

1:58d1caed28d4

Parent:

0:502b364c9f1d

Child:

2:611a5eb132a1

--- a/main.cpp	Sun Oct 21 19:38:09 2018 +0000
+++ b/main.cpp	Thu Oct 25 12:14:32 2018 +0000
@@ -79,7 +79,7 @@
 
 float speed_max_x=1;
 float speed_max_y=1;
-float speed_max_w=10;
+float speed_max_w=1;
 
 long odom_last_read= millis();
 
@@ -109,77 +109,113 @@
 
 ///////////////////////////
 float encoder_2_global_angle = 30;         //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.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)
 ////////////////////TUNG////////////////
 
-float    Xshift=  encoder_2_global_x;
-float    Yshift=  encoder_2_global_y;
-float    offsetX = -Yshift;
-float    offsetY = Xshift;
-
-float Ashift  =   -30*pi/float(180);
-float offsetA =   -30;
 
 float transformed_real_now_x=0;
 float transformed_real_now_y=0;
 float transformed_real_now_w=0;
 
+
+float startup_offset_x_encoder =0;
+float startup_offset_y_encoder =0;
+float startup_offset_w_encoder=0;
+
+
+
+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 r = sqrt( ( encoder_2_global_x  * encoder_2_global_x )  + ( encoder_2_global_y  *  encoder_2_global_y ) );   //encoder to center radius
+
+
 void calculatePos(float _X,float _Y,float _A)
 {
-    float radAng=_A/float(180)*pi;
-    /*
-    posX=(float(local_posY)/1000 + self.paraX * sin(w_radian) + self.paraY * cos(w_radian) )*(1) +self.offsetX
-    posY=(float(local_posX)/1000 + self.paraX * cos(w_radian) - self.paraY * sin(w_radian) )*(-1) +self.offsetY
-    */
-    float rotatedPosX=_X*cos(Ashift)+_Y*sin(Ashift);
-    float rotatedPosY=-_X*sin(Ashift)+_Y*cos(Ashift);
-    transformed_real_now_x=(rotatedPosY/float(1000)+Xshift*sin(radAng)+Yshift*cos(radAng))+offsetX;
-    transformed_real_now_y=(rotatedPosX/float(1000)+Xshift*cos(radAng)-Yshift*sin(radAng))*(-1)+offsetY;
-    //transformed_real_now_w=_A;   //
-    transformed_real_now_w=radAng;
+    float zangle  =  _A-   360 * int(_A / 360);
+    float zrangle =  zangle *pi/float(180);    //degree 2 rad
+    
+    float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) )  -  (  ( _Y / float(1000) )   *  sin( -encoder2local_angle) );
+    float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) )  +  (  ( _Y / float(1000) )   *  cos( -encoder2local_angle) );
+    
+    float s  = copysign( sqrt(  2*( r*r )  -  2*(r*r)*cos(zrangle)  )    , zrangle );
+    
+    float x_bias = s * cos( zrangle / 2 );
+    float y_bias = s * sin( zrangle / 2 );
+    
+    float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) )  - ( y_bias ) * ( sin( encoder_position_angle ) )    ;
+    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_w=   _A *pi/float(180)                                                                        -  startup_offset_w_encoder;
+    
+    
 }
 
 
 
 
 
-
-
-
+///////////////////////
 
-///////////////////////
-float startup_offset_x_encoder = 0;
-float startup_offset_y_encoder = 0;
-float startup_offset_w_encoder = 0;
 
 
 float x_PID_P = 0.5;
 float y_PID_P = 0.5;
-float w_PID_P = 0.1;
+float w_PID_P = 1;
 
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 
 //////////////////////////////
 void start_calculatePos(float _X,float _Y,float _A)
 {
-    float radAng=_A/float(180)*pi;
-    /*
-    posX=(float(local_posY)/1000 + self.paraX * sin(w_radian) + self.paraY * cos(w_radian) )*(1) +self.offsetX
-    posY=(float(local_posX)/1000 + self.paraX * cos(w_radian) - self.paraY * sin(w_radian) )*(-1) +self.offsetY
-    */
-    float rotatedPosX=_X*cos(Ashift)+_Y*sin(Ashift);
-    float rotatedPosY=-_X*sin(Ashift)+_Y*cos(Ashift);
-    startup_offset_x_encoder  =  (rotatedPosY/float(1000)+Xshift*sin(radAng)+Yshift*cos(radAng))+offsetX;
-    startup_offset_y_encoder  =  (rotatedPosX/float(1000)+Xshift*cos(radAng)-Yshift*sin(radAng))*(-1)+offsetY;
-    //transformed_real_now_w=_A;   //
-    startup_offset_w_encoder=radAng;
+    float zangle  =  _A-   360 * int(_A / 360);
+    float zrangle =  zangle *pi/float(180);    //degree 2 rad
+    
+    float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) )  -  (  ( _Y / float(1000) )   *  sin( -encoder2local_angle) );
+    float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) )  +  (  ( _Y / float(1000) )   *  cos( -encoder2local_angle) );
+    
+    float s  = copysign( sqrt(  2*( r*r )  -  2*(r*r)*cos(zrangle)  )    , zrangle );
+    
+    float x_bias = s * cos( zrangle / 2 );
+    float y_bias = s * sin( zrangle / 2 );
+    
+    float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) )  - ( y_bias ) * ( sin( encoder_position_angle ) )    ;
+    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_w_encoder =  _A *pi/float(180);    //degree 2 rad
+    
+    
 }
 
 
 
 
 
+
+
 void ActionEncoder_init()
 {
     count=0;
@@ -357,8 +393,8 @@
    
     calculatePos(now_x,now_y,now_w);
     
-    /*
-    sprintf(buffer, "%f", transformed_real_now_x);
+    
+    /*sprintf(buffer, "%f", transformed_real_now_x);
     pc.printf(buffer);
     pc.printf("  ");
     sprintf(buffer, "%f", transformed_real_now_y);
@@ -366,8 +402,8 @@
     pc.printf("  ");
     sprintf(buffer, "%f", transformed_real_now_w);
     pc.printf(buffer);
-    pc.printf("\r\n");*/
-    
+    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 )      )
@@ -378,9 +414,9 @@
        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  /float(180)*pi     +    startup_offset_w_encoder;
+       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;
@@ -440,8 +476,15 @@
 int main() {
     //while(1){
 ////////////////////////////
-    points[0] = (point_info){.required_x = 0.2,.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,  .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[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};
     
 
 
@@ -467,7 +510,7 @@
                 //startup_offset_y_encoder = now_y/1000;
                 //startup_offset_w_encoder = now_w/float(180)*pi;
                 
-                start_calculatePos(  (now_x/1000),(now_y/1000), now_w  );   //global
+                start_calculatePos(  (now_x),(now_y), now_w  );   //global
                 break;
             
             }
@@ -476,9 +519,9 @@
     }    //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    +  startup_offset_w_encoder;
+   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++ ){
@@ -504,6 +547,7 @@
     {
         if (Action.readable())
         {
+            //pc.putc('a');
             char c = Action.getc();
             if(readEncoder(c)) odom_update();
         }