Diff: source/Mapping.cpp

Revision:

132:8ae08f41bb43

Parent:

130:670a954495bf

--- a/source/Mapping.cpp	Mon May 15 13:03:16 2017 +0000
+++ b/source/Mapping.cpp	Mon May 15 14:58:27 2017 +0000
@@ -30,8 +30,8 @@
 //******************************************************************************
 void mapping()
 {
-   printf("mapping...\r\n"); 
-    
+    printf("mapping...\r\n");
+
     position current_pos = get_current_pos();
     float current_heading = get_current_heading();
     if(old_heading != current_heading || old_pos.x != current_pos.x || old_pos.y != current_pos.y) {
@@ -86,7 +86,7 @@
  * draws something on the map, object defines what to draw
  * 0 = obstacle
  * 1 = target
- * 2 = 
+ * 2 =
  * by Claudio Citterio
 **/
 void draw_to_map(position pos, int object)
@@ -114,15 +114,15 @@
         for (int x = 0 + x_negative_offset; x < 7 - x_positive_offset; x++) {
             switch(object) {
                 case 0:
-                    a = list_time_value;
+                    //a = list_time_value;
                     obstacle_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
                     break;
                 case 1:
-                    a = list_target_value;
+                    //a = list_target_value;
                     target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
                     break;
                 case 2:
-                    a = list_boundry_value;
+                    //a = list_boundry_value;
                     target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
                     break;
                 default:
@@ -224,32 +224,42 @@
 
     for(int i = 0; i < row; i++) {
         for(int j = 0; j < col; j++) {
-            if(target_list[i][j] == 1) {
+            //printf("tl: %d\r\n",target_list[i][j]);
+            if(target_list[i][j] > 0) {
                 diff.x = abs(myPos.x - j);
                 diff.y = abs(myPos.y - i);
-                current_dist = diff.x * diff.y;
+                current_dist = diff.x*diff.x * diff.y*diff.y;
+                
+                target.x = 25;
+                target.y = 25;
+                closest_dist = 1;
                 if(current_dist < closest_dist) {
-                    closest_dist = current_dist;
-                    target.x = j;
-                    target.y = i;
+                    //closest_dist = current_dist;
+                    // target.x = j;
+                    //target.y = i;
+
                 }
             }
         }
     }
     if(target.x == 0) {
+        printf("no target found\r\n");
+        print_map(2);
         return 48; // No Target found
     } else {
         start = get_current_pos();
+        printf("Target found\r\n");
         printf("start: %d || %d\r\n", start.x, start.y);
         printf("Target: %d || %d\r\n", target.x, target.y);
-        
+
         return 36; // Target found
     }
 }
 
-int generate_fake_target(){
-        position pos = {50,50};
-        draw_to_map(pos,1);
+int generate_fake_target()
+{
+    position pos = {25,25};
+    draw_to_map(pos,1);
     return 35;
 }