Diff: source/Mapping.cpp

Revision:

46:8b52c7b34d34

Parent:

41:462d379e85c4

Child:

47:728502df3cb6

--- a/source/Mapping.cpp	Tue Apr 11 18:01:36 2017 +0000
+++ b/source/Mapping.cpp	Tue Apr 11 19:43:39 2017 +0000
@@ -4,16 +4,58 @@
 **/
 
 #include "Mapping.h"
-#include "Pathfinding.h"
+//#include "Pathfinding.h"
 
+uint8_t obstacle_list_superpos[7][7] = {{0,0,2,2,2,0,0},
+                                        {0,2,2,2,2,2,0},
+                                        {2,2,2,2,2,2,2},
+                                        {2,2,2,1,2,2,2},
+                                        {2,2,2,2,2,2,2},
+                                        {0,2,2,2,2,2,0},
+                                        {0,0,2,2,2,0,0}};
+ 
+position old_pos = {0};
+position mapping_pos_right = {0};
+position mapping_pos_left = {0};
+
+float old_heading = {0};
+float distance_left = 0;
+float distance_right = 0;
 
 void mapping()
 {
+    position new_pos = get_current_pos();
+    float new_heading = get_current_heading();
+    if(old_heading != new_heading || old_pos.x != new_pos.x || old_pos.y != new_pos.y){
     //pseudo code
-    // check_sensor(left);
+    //check_sensor(left);
+    distance_left = getDistanceIR(0);
+    if(distance_left < 0.75f && distance_left > 0.1f){
+        mapping_pos_left = position_calculation(distance_left, 50, 0.12, 0.12, new_heading);
+        draw_to_map(mapping_pos_left);
+    }
     //check_sensor(right);
+    distance_right = getDistanceIR(4);
+    if(distance_right < 0.75f && distance_right > 0.1f){
+        mapping_pos_right = position_calculation(distance_right, -50, -0.12, 0.12, new_heading);
+        draw_to_map(mapping_pos_right);
+    }
+    
+    old_pos = new_pos;
+    old_heading = new_heading;
+    }   
 }
 
+void draw_to_map(position pos){
+    
+    }
+    
+position position_calculation(float distance,float degree,float ofsettx,float ofsetty, float heading){
+    
+    
+    position pos = {0};
+    return pos;
+}
 
 int select_target()
 {