Diff: source/Mapping.cpp

Revision:

49:652438112348

Parent:

48:4078877669e4

Child:

50:2e2bf0815fd9

--- a/source/Mapping.cpp	Thu Apr 13 09:18:58 2017 +0000
+++ b/source/Mapping.cpp	Thu Apr 13 22:45:22 2017 +0000
@@ -6,9 +6,12 @@
 #include "Mapping.h"
 
 uint8_t obstacle_list[row][col] = { 0 };
+uint8_t target_list[row][col] = {0};
 uint8_t list_time_value = 60;
+uint8_t list_target_value = 1;
+uint8_t list_boundry_value = 255;
 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},
+uint8_t 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},
@@ -18,30 +21,53 @@
 };
 
 position old_pos = {0};
-position mapping_pos_right = {0};
-position mapping_pos_left = {0};
+float old_heading = 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);
-        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);
+        float distance0 = getDistanceIR(0);
+        if(distance0 < 0.75f && distance0 > 0.1f) {
+            set_servo_position(0, 50);
+            float distance = getDistanceIR(1);
+            int object = 0;
+            if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance0 && distance + 0.05f > distance0) {
+                object = 0;
+            } else {
+                object = 1;
+            }
+            position mapping_pos = position_calculation(distance0, 50, 0.12, 0.12, new_heading);
+            draw_to_map(mapping_pos, object);
         }
         //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);
+        float distance4 = getDistanceIR(0);
+        if(distance4 < 0.75f && distance4 > 0.1f) {
+            float distance = getDistanceIR(1);
+            int object = 0;
+            if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance4 && distance + 0.05f > distance4) {
+                object = 0;
+            } else {
+                object = 1;
+            }
+            position mapping_pos = position_calculation(distance4, -50, -0.12, 0.12, new_heading);
+            draw_to_map(mapping_pos, object);
+        }
+        //check_sensor(center);
+        float distance2 = getDistanceIR(0);
+        if(distance2 < 0.75f && distance2 > 0.1f) {
+            float distance = getDistanceIR(1);
+            int object = 0;
+            if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance2 && distance + 0.05f > distance2) {
+                object = 0;
+            } else {
+                object = 1;
+            }
+            position mapping_pos = position_calculation(distance2, 0, -0.12, 0.12, new_heading);
+            draw_to_map(mapping_pos, object);
         }
 
         old_pos = new_pos;
@@ -49,7 +75,8 @@
     }
 }
 
-void draw_to_map(position pos)
+//******************************************************************************
+void draw_to_map(position pos, int object)
 {
     uint8_t x_negative_offset = 0;
     uint8_t x_positive_offset = 0;
@@ -64,7 +91,7 @@
     }
 
     if (pos.y < 5) {
-        y_negative_offset = 3 - pos.y; 
+        y_negative_offset = 3 - pos.y;
     }
     if (pos.y > (row-5)) {
         y_positive_offset = pos.y - (row - 4);
@@ -72,13 +99,29 @@
 
     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.y - 3 + y][pos.x - 3 + x] = obstacle_list_superpos[y][x];
+            switch(object) {
+                case 0:
+                    a = list_time_value;
+                    obstacle_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
+                    break;
+                case 1:
+                    a = list_target_value;
+                    target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
+                    break;
+                case 2:
+                    a = list_boundry_value;
+                    target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
+                    break;
+                default:
+                    break;
+            }
         }
     }
 }
 
-void draw_map() {
+//******************************************************************************
+void print_map()
+{
     for (int y = 0; y < col; y++) {
         for (int x = 0; x < row; x++) {
             printf("%d ", obstacle_list[y][x]);
@@ -88,6 +131,7 @@
     printf("\n");
 }
 
+//******************************************************************************
 position position_calculation(float distance,float degree,float offsetx,float offsety, float heading)
 {
 
@@ -96,6 +140,7 @@
     return pos;
 }
 
+//******************************************************************************
 int select_target()
 {
     position myPos = get_current_pos();
@@ -125,51 +170,9 @@
     }
 }
 
-
+//******************************************************************************
 int switch_target_red()
 {
     obstacle_list[target.y][target.x] = 0;
     return 0;
-}
-
-
-
-
-
-/*
-void chack_sensor(sensor)
-{
-    get_distance();
-    if(sensor < 80cm || sensor > 10cm) {
-        calculate_position(sensor, distance);
-        check_map(position.this, obstacle);
-        if(map.no-entry){
-            check_if_obstacle(distance);
-            update_map();
-        }
-    }
-}
-
-void get_distance(sensor)
-{
-    sensor.value;
-    return value;
-}
-
-void calcualte_position(sensor, distance)
-{
-    do math;
-    return position;
-}
-
-void check_if_obstacle(distance)
-{
-    move_scanner();
-    distance2 = get_distance(sensor);
-    reset_scanner();
-    if(distance -5cm < distance2 && distance +5cm > distance2) {
-        return 1;
-    } else {
-        return 0;
-    }
-}*/
\ No newline at end of file
+}
\ No newline at end of file