Diff: source/Positioning.cpp

Revision:

70:922cbbfebf02

Parent:

69:1fdcef6a7577

Child:

71:ddf4eb5c3081

--- a/source/Positioning.cpp	Thu Apr 20 05:52:22 2017 +0000
+++ b/source/Positioning.cpp	Thu Apr 20 07:36:50 2017 +0000
@@ -17,6 +17,8 @@
 int deg_l = 0;
 float last_dist_r = 0;
 float last_dist_l = 0;
+float direction_r = 0;
+float direction_l = 0;
 
 
 coordinates get_current_coord()
@@ -56,10 +58,21 @@
     return current_heading;
 }
 
+void clamp_heading()
+{
+
+    while (direction_l >= 360) direction_l -= 360;
+    while (direction_l < 0) direction_l += 360;
+
+}
 
 void positioning()
 {
     printf("positioning...\r\n");
+    
+    
+    
+    clamp_heading();
 }
 
 
@@ -76,6 +89,8 @@
 
         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();
         init = true;
     } else {
@@ -91,21 +106,60 @@
                 last_dist_r = dist_r;
                 deg_r += 5;
                 set_servo_position(2,-deg_r);
-            } else {
-                  
+            } else if(direction_r == 0) {
+
+                dist_r *= 100;
+                float offsetx = 12;
+                float offsety = 12;
 
-                //    current_coord.y =
+                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 = 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) {
+
+                dist_r *= 100;
+                float offsetx = -12;
+                float offsety = 12;
+
+                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 = 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;
+
+                clamp_heading();
+                printf("heading: %f \r\n", current_heading);
+
+                //finished
+                return 11;
+
+            }
         }
-
     }
-    //finished
-    //return 11
-
-        // not finished*/
+    // not finished
     return 16;
 }