Diff: source/Mapping.cpp

Revision:

47:728502df3cb6

Parent:

46:8b52c7b34d34

Child:

48:4078877669e4

--- a/source/Mapping.cpp	Tue Apr 11 19:43:39 2017 +0000
+++ b/source/Mapping.cpp	Wed Apr 12 13:30:44 2017 +0000
@@ -4,16 +4,19 @@
 **/
 
 #include "Mapping.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}};
- 
+uint8_t obstacle_list[row][col] = { 0 };
+uint8_t list_time_value = 60;
+uint8_t a = list_time_value; //Substitution um Matrix übersichtlich zu halten
+uint8_t obstacle_list_superpos[7][7] = {{0,0,a,a,a,0,0},
+    {0,a,a,a,a,a,0},
+    {a,a,a,a,a,a,a},
+    {a,a,a,a,a,a,a},
+    {a,a,a,a,a,a,a},
+    {0,a,a,a,a,a,0},
+    {0,0,a,a,a,0,0}
+};
+
 position old_pos = {0};
 position mapping_pos_right = {0};
 position mapping_pos_left = {0};
@@ -26,33 +29,59 @@
 {
     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);
-    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);
+    if(old_heading != new_heading || old_pos.x != new_pos.x || old_pos.y != new_pos.y) {
+        //pseudo code
+        //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;
     }
-    //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);
+}
+
+void draw_to_map(position pos)
+{
+    uint8_t x_negative_offset = 0;
+    uint8_t x_positive_offset = 0;
+    uint8_t y_negative_offset = 0;
+    uint8_t y_positive_offset = 0;
+
+    if(pos.x < 5) {
+        x_negative_offset = 5-pos.x;
+    }
+    if(pos.x > 195) {
+        x_positive_offset = pos.x-195;
+    }
+
+    if(pos.y < 5) {
+        y_negative_offset = 5-pos.y;
+    }
+    if(pos.y > 195) {
+        y_positive_offset = pos.y-195;
     }
     
-    old_pos = new_pos;
-    old_heading = new_heading;
-    }   
+    for(int y = 0 + y_negative_offset; y < 7-y_positive_offset; y++) {
+        for(int x = 0 + x_negative_offset; x < 7-x_positive_offset; x++) {
+            
+            obstacle_list[pos.x-3+x][pos.y-3+y] = obstacle_list_superpos[x][y];
+        }
+    }
 }
 
-void draw_to_map(position pos){
-    
-    }
-    
-position position_calculation(float distance,float degree,float ofsettx,float ofsetty, float heading){
-    
-    
+position position_calculation(float distance,float degree,float offsetx,float offsety, float heading)
+{
+
+
     position pos = {0};
     return pos;
 }
@@ -86,16 +115,11 @@
     }
 }
 
-int remove_target()
-{
-    obstacle_list[target.y][target.x] = 0;
-    return 0;
-}
 
 int switch_target_red()
 {
     obstacle_list[target.y][target.x] = 0;
-    return 0;  
+    return 0;
 }