Diff: main.cpp

Revision:

30:58bfac39e701

Parent:

29:13aa1d1a1c12

Child:

31:8bcc3a0bfa8a

--- a/main.cpp	Thu Jan 21 14:58:47 2016 +0000
+++ b/main.cpp	Fri Jan 22 15:46:43 2016 +0000
@@ -45,19 +45,27 @@
     //Construction des obstacles
     map.build();
     
-    map.AStar(0, 1000, 1400, 1000, 40);
+    float x=odo.getX();
+    float y=odo.getY();
+    float the = 0;
+    
+    map.AStar(x, y, 1400, 1000, 75);
     path = map.path;
     
     for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, 0);
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
+        odo.GotoXYT(path[i].x, path[i].y, the);
     }
     
-    map.AStar(odo.getX(), odo.getY(), 0, 1000, 40);
+    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
     path = map.path;
     
     for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, PI);
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
+        odo.GotoXYT(path[i].x, path[i].y, the);
     }
+    
+    odo.GotoThet(0);
      
     //odo.GotoXYT(500, 50, 0);
     //odo.GotoXYT(200, 0, 0);