Diff: source/Positioning.cpp

Revision:

120:cdf7a6751f9e

Parent:

118:fbd6d41f6ce8

Child:

121:af16ffc05593

--- a/source/Positioning.cpp	Fri May 12 08:33:04 2017 +0000
+++ b/source/Positioning.cpp	Fri May 12 15:20:45 2017 +0000
@@ -107,8 +107,8 @@
         deg_r = 0;
         deg_l = 0;
 
-        set_servo_position(0,-deg_l);    //servo sensor left
-        set_servo_position(2,-deg_r);   //servo sensor right
+        set_servo_position(0,deg_l);    //servo sensor left
+        set_servo_position(2,deg_r);   //servo sensor right
         current_coord.x = 0;
         current_coord.y = 0;
         t2.start();
@@ -127,62 +127,59 @@
                 last_dist_r = dist_r;
 
 
-                deg_r += 5;
-                set_servo_position(2,-deg_r);
-            } else if(direction_r == 0) {
-
+                deg_r += 3;
+                set_servo_position(2,deg_r);
+                
+            } else if(current_coord.y == 0) {
+                deg_r -= 5; //compensate overturning
+                //deg_r -= 3; // compensate not straight looking
                 dist_r *= 100;
-                float offsetx = 12;
-                float offsety = 12;
+                float offsetx = 11.5;
+                float offsety = 11;
 
-                float x = (offsetx + sin(deg_r/180*(float)M_PI)*dist_r)/4;
-                float y = (-offsety - cos(deg_r/180*(float)M_PI)*dist_r)/4;
-                float hyp = sqrt(x*x+y*y);                     // y pos
-                direction_r = asin(x/hyp)/(float)M_PI*180;     // winkel
+                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 = hyp;
 
-                while (direction_r >= 360) direction_r -= 360;
-                while (direction_r < 0) direction_r += 360;
             }
 
             //left
             if(dist_l < last_dist_l) {
                 last_dist_l = dist_l;
                 deg_l -= 5;
-                set_servo_position(0,-deg_l);
-            } else if(direction_l == 0) {
-
+                set_servo_position(0,deg_l);
+                
+            } else if(current_coord.x == 0) {
+                deg_l += 3; //compensate overturning
+                deg_l -= 3; //compensate not straight looking
                 dist_r *= 100;
-                float offsetx = -12;
-                float offsety = 12;
+                float offsetx = 11.5;
+                float offsety = 11;
 
-                float x = (offsetx + sin(deg_l/180*(float)M_PI)*dist_l)/4;
-                float y = (-offsety - cos(deg_l/180*(float)M_PI)*dist_l)/4;
-                float hyp = sqrt(x*x+y*y);                     // y pos
-                direction_l = asin(x/hyp)/(float)M_PI*180;     // winkel
+                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 = hyp;
 
-                while (direction_l >= 360) direction_l -= 360;
-                while (direction_l < 0) direction_l += 360;
             }
 
             if(current_coord.x != 0 && current_coord.y != 0) {
-                printf("directions: %f || %f \r\n", direction_l, direction_r);
-                current_heading = (360-direction_r + 270-direction_l)/2;
+                
+                printf("degs: %d || %d \r\n", deg_l , deg_r);
+                printf("coords : %f || %f \r\n", current_coord.x, current_coord.y);
+                current_heading = (360-deg_r + 270-deg_l)/2;
 
                 clamp_heading();
                 printf("heading: %f \r\n", current_heading);
+                
+                current_coord.x = 20;
+                current_coord.y = 20;
 
                 //finished
-                return 17;
+                return 11;
 
             }
         }
     }
     // not finished
-    return 11;
+    return 17;
 }