Diff: source/Mapping.cpp

Revision:

129:0f60bf9640bb

Parent:

128:6bde4483ce7b

Child:

130:670a954495bf

--- a/source/Mapping.cpp	Sun May 14 14:40:12 2017 +0000
+++ b/source/Mapping.cpp	Mon May 15 11:48:05 2017 +0000
@@ -52,7 +52,7 @@
         //check_sensor(right);
         float distance4 = getDistanceIR(4);
         if(distance4 < 0.75f && distance4 > 0.1f) {
-            set_servo_position(1, -servo_angle);
+            set_servo_position(2, -servo_angle);
             float distance = getDistanceIR(5);
             int object = 0;
             if(distance < 0.75f && distance > 0.1f && (distance -0.05f < distance4 || distance + 0.05f > distance4)) {
@@ -82,7 +82,13 @@
     }
 }
 
-//******************************************************************************
+/**
+ * draws something on the map, object defines what to draw
+ * 0 = obstacle
+ * 1 = target
+ * 2 = 
+ * by Claudio Citterio
+**/
 void draw_to_map(position pos, int object)
 {
     uint8_t x_negative_offset = 0;
@@ -127,12 +133,22 @@
 }
 
 //******************************************************************************
-void print_map()
+/**
+ * prints map defined by list
+ * 0 = obstacle_list
+ * 1 = open_list
+ * 2 = target_list
+ * very slow, shouldn't be called in final code
+ * by Claudio Citterio
+**/
+void print_map(int list)
 {
     // Debug function for printing the obstacle matrix on putty. 
     for (int y = 0; y < col; y++) {
         for (int x = 0; x < row; x++) {
-            printf("%d ", obstacle_list[y][x]);
+            if(list == 0)printf("%d ", obstacle_list[y][x]);
+            if(list == 1)printf("%d ", open_list[y][x]);
+            if(list == 2)printf("%d ", target_list[y][x]);
         }
         printf("\r\n");
     }
@@ -140,6 +156,11 @@
 }
 
 //******************************************************************************
+/**
+ * calculates a position given by numerouse distances and angels
+ * is called by mapping to define where to object is
+ * by Claudio Citterio
+**/
 position position_calculation(float distance, float degree, float offsetx, float offsety, float heading, position current_pos)
 {
     distance *= 100;
@@ -208,7 +229,12 @@
 
     return coord;
 }
-//******************************************************************************
+
+/**
+ * selects closest target from current position
+ * targets ar printed as 1 in target_list
+ * by Claudio Citterio
+**/
 int select_target()
 {
     position myPos = get_current_pos();
@@ -219,7 +245,7 @@
 
     for(int i = 0; i < row; i++) {
         for(int j = 0; j < col; j++) {
-            if(obstacle_list[i][j] == 2) {
+            if(target_list[i][j] == 1) {
                 diff.x = abs(myPos.x - j);
                 diff.y = abs(myPos.y - i);
                 current_dist = diff.x * diff.y;
@@ -236,14 +262,15 @@
     } else {
         start = get_current_pos();
         printf("start: %d || %d\r\n", start.x, start.y);
-        printf("Target: &d || %d\r\n", target.x, target.y);
+        printf("Target: %d || %d\r\n", target.x, target.y);
         
         return 36; // Target found
     }
 }
 
 int generate_fake_target(){
-        obstacle_list[50][50] = 2;
+        position pos = {50,50};
+        draw_to_map(pos,1);
     return 35;
 }