Diff: source/Positioning.cpp

Revision:

34:40d8d29b44b8

Parent:

33:8a98f8b9d859

Child:

38:3526c36e4c73

diff -r 8a98f8b9d859 -r 40d8d29b44b8 source/Positioning.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/Positioning.cpp	Wed Mar 29 12:10:32 2017 +0000
@@ -0,0 +1,59 @@
+/**
+ * Positioning function library
+ * Handels position of the Robot on the map
+**/
+
+#include "mbed.h"
+#include "Pathfinding.h"
+#include "IRSensor.h"
+
+position current_pos;
+
+position 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();
+
+    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