Diff: source/Positioning.cpp

Revision:

64:c2fcf3b349e9

Parent:

61:b57577b0072f

Child:

66:8cfca8fad65d

--- a/source/Positioning.cpp	Tue Apr 18 17:33:22 2017 +0000
+++ b/source/Positioning.cpp	Wed Apr 19 08:34:24 2017 +0000
@@ -5,12 +5,44 @@
 
 
 #include "Positioning.h"
-position current_pos;
+coordinates current_coord;  // Updated
+position current_pos;       // Generated from current_coord
 float current_heading;
 position next_pos;
 
+bool init = false;
+Timer t2 = 0;
+
+int deg_r = 0;
+int deg_l = 0;
+float last_dist_r = 0;
+float last_dist_l = 0;
+
+
+coordinates get_current_coord()
+{
+    return current_coord;
+}
+
+coordinates pos_to_coord(position pos)
+{
+    coordinates coord = {0};
+    coord.x = pos.x + 0.5f;
+    coord.y = pos.y + 0.5f;
+    return coord;
+}
+
+position coord_to_pos(coordinates coord)
+{
+    position pos = {0};
+    pos.x = rint(coord.x -0.5f);
+    pos.y = rint(coord.y -0.5f);
+    return pos;
+}
+
 position get_current_pos()
 {
+    current_pos = coord_to_pos(current_coord);
     return current_pos;
 }
 
@@ -31,47 +63,84 @@
 }
 
 
-/*
-void initial_positioning()
+
+int initial_positioning()
 {
-    float last_dist_r = 1.0f;
-    float last_dist_f = 1.0f;
+    if(init == false) {
+
+        last_dist_r = 100;
+        last_dist_l = 100;
 
-    int deg_r = 0;
-    int deg_l = 0;
+        deg_r = -50;
+        deg_l = 50;
+
+        set_servo_position(0,deg_l);    //servo sensor left
+        set_servo_position(2,-deg_r);   //servo sensor right
+        t2.start();
+        init = true;
+    } else {
 
-    turn_straight_right();
-    turn_straight_left();
+        if(t2 > 0.2f) {
+            t2 = 0;
+
+            float dist_r = getDistanceIR(4);
+            float dist_l = getDistanceIR(0);
 
-    while(last_dist_r > sensors[r]) {
-        turn_sensor_right(1); //turn sensor + 1 deg
-        wait(0.1f)
-        deg_r += 1;
-        last_dist_r = sensors[r];
-    }
+            //right
+            if(dist_r < last_dist_r) {
+                last_dist_r = dist_r;
+                deg_r += 5;
+                set_servo_position(2,-deg_r);
+            } else {
+                  
+
+                //    current_coord.y =
+            }
+
+
+            //left
+        }
 
-    while(last_dist_l > sensors[l]) {
-        turn_sensor_left(-1); //turn sensor - 1 deg
-        wait(0.1f)
-        deg_l += 1;
-        last_dist_l = sensors[l];
     }
+    //finished
+    //return 11
 
-    int deg_l_2=0;
-    turn_straight_left();
-    last_dist_l = 0;
+        // not finished*/
+    return 16;
+}
+
+
+turn_straight_right();
+turn_straight_left();
+
+while(last_dist_r > sensors[r]) {
+    turn_sensor_right(1); //turn sensor + 1 deg
+    wait(0.1f)
+    deg_r += 1;
+    last_dist_r = sensors[r];
+}
 
-    while(last_dist_l < sensors[l]) {
-        turn_sensor_left(1); //turn sensor +1 deg (positiv = uhrzeigersinn)
-        wait(0.1f)
-        deg_l_2 += 1;
-        last_dist_l = sensors[l];
-    }
+while(last_dist_l > sensors[l]) {
+    turn_sensor_left(-1); //turn sensor - 1 deg
+    wait(0.1f)
+    deg_l += 1;
+    last_dist_l = sensors[l];
+}
+
+int deg_l_2=0;
+turn_straight_left();
+last_dist_l = 0;
 
-    turn_straight_right();
-    turn_straight_left();
+while(last_dist_l < sensors[l]) {
+    turn_sensor_left(1); //turn sensor +1 deg (positiv = uhrzeigersinn)
+    wait(0.1f);
+    deg_l_2 += 1;
+    last_dist_l = sensors[l];
+}
 
-    wait(0.2f);
+turn_straight_right();
+turn_straight_left();
 
-}
-*/
\ No newline at end of file
+wait(0.2f);
+*/
+