Diff: source/Mapping.cpp

Revision:

41:462d379e85c4

Parent:

40:a46667b62671

Child:

44:7118b23b0fd7

Child:

46:8b52c7b34d34

--- a/source/Mapping.cpp	Tue Apr 04 15:37:40 2017 +0000
+++ b/source/Mapping.cpp	Mon Apr 10 14:52:23 2017 +0000
@@ -4,31 +4,62 @@
 **/
 
 #include "Mapping.h"
+#include "Pathfinding.h"
 
 
 void mapping()
 {
     //pseudo code
-   // check_sensor(left);
-   //check_sensor(right);
+    // check_sensor(left);
+    //check_sensor(right);
 }
 
 
-int select_target(){
-    
+int select_target()
+{
+    position myPos = get_current_pos();
+    position diff = {0};
+    target.x = 0;
+    int closest_dist = 10000;
+    int current_dist = 0;
+
+    for(int i = 0; i < row; i++) {
+        for(int j = 0; j < col; j++) {
+            if(obstacle_list[i][j] == 2) {
+                diff.x = abs(myPos.x - j);
+                diff.y = abs(myPos.y - i);
+                current_dist = diff.x * diff.y;
+                if(current_dist < closest_dist) {
+                    closest_dist = current_dist;
+                    target.x = j;
+                    target.y = i;
+                }
+            }
+        }
+    }
+    if(target.x == 0) {
+        return 47; // No Target found
+    } else {
+        return 36; // Target found
+    }
+}
+
+int remove_target()
+{
+    obstacle_list[target.y][target.x] = 0;
     return 0;
 }
 
-int remove_target(){
-    
-    return 0;
+int switch_target_red()
+{
+    obstacle_list[target.y][target.x] = 0;
+    return 0;  
 }
 
 
 
 
 
-
 /*
 void chack_sensor(sensor)
 {