Diff: source/Positioning.cpp

Revision:

125:573a18c2155f

Parent:

121:af16ffc05593

Child:

129:0f60bf9640bb

--- a/source/Positioning.cpp	Sun May 14 09:34:37 2017 +0000
+++ b/source/Positioning.cpp	Sun May 14 13:33:42 2017 +0000
@@ -136,8 +136,10 @@
                 dist_r *= 100;
                 float offsetx = 11.5;
                 float offsety = 11;
+                float arctan_a_b = 46.273f;
+                float c = sqrt(offsetx*offsetx+offsety*offsety);
 
-                current_coord.y = dist_r + sqrt(offsetx*offsetx+offsety*offsety)*cos((90-deg_r-atan(offsety/offsetx))/180.0f*(float)M_PI);
+                current_coord.y = dist_r + c * cos( ( (90-fabsf(deg_r)-arctan_a_b) / 180.0f)*(float)M_PI);
 
 
             }
@@ -151,12 +153,13 @@
             } else if(current_coord.x == 0) {
                 deg_l += 3; //compensate overturning
                 deg_l -= 3; //compensate not straight looking
-                dist_r *= 100;
+                dist_l *= 100;
                 float offsetx = 11.5;
                 float offsety = 11;
+                float arctan_a_b = 46.273f;
+                float c = sqrt(offsetx*offsetx+offsety*offsety);
 
-                current_coord.x = dist_r + sqrt(offsetx*offsetx+offsety*offsety)*cos((90-fabsf(deg_l)-atan(offsetx/offsety))/180.0f*(float)M_PI);
-
+                current_coord.x = dist_l + c * cos( ( (90-fabsf(deg_l)-arctan_a_b) / 180.0f)*(float)M_PI);
 
             }