Diff: source/positioning.cpp

Revision:

33:8a98f8b9d859

Parent:

18:a82994e67297

--- a/source/positioning.cpp	Thu Mar 16 12:52:41 2017 +0000
+++ b/source/positioning.cpp	Mon Mar 20 14:11:46 2017 +0000
@@ -2,14 +2,57 @@
  * Positioning function library
  * Handels position of the Robot on the map
 **/
-/*
+
 #include "mbed.h"
+#include "pathfinding.h"
+#include "IRsensor.h"
+
+position current_pos;
 
-double time_counter = 0.0f;
-double deltatime = 0.0f;
+void get_current_pos()
+{
+    return current_pos;
+}
+
+void initial_positioning()
+{
+    float last_dist_r = 1.0f;
+    float last_dist_f = 1.0f;
+
+    int deg_r = 0;
+    int deg_l = 0;
+
+    turn_straight_right();
+    turn_straight_left();
 
-void positioning()
-{
-    //call positioning functions
+    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
+        wait(0.1f)
+        deg_l += 1;
+        last_dist_l = sensors[l];
+    }
+
+    int deg_l_2=0;
+    turn_straight_left();
+    last_dist_l = 0;
+
+    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];
+    }
+    
+    turn_straight_right();
+    turn_straight_left();
+    
+    wait(0.2f);
+    
 }
-*/
\ No newline at end of file