Diff: main.cpp

Revision:

20:30942f018252

Parent:

18:0f1fefe78266

--- a/main.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ b/main.cpp	Thu Jan 07 18:22:38 2016 +0000
@@ -4,7 +4,7 @@
     Map/define.h
     Map/Obstacles/Obstacle.h
 */ 
-#include "Map/Map.h"
+#include "Map.h"
 
 #define dt 10000
 #define ATTENTE 0
@@ -15,7 +15,7 @@
 DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
 Ticker ticker;
-//Serial pc(USBTX, USBRX);
+Serial logger(PA_9, PA_10);
 Serial pc(PA_9, PA_10);
 RoboClaw roboclaw(460800, PA_11, PA_12);
 Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
@@ -38,11 +38,14 @@
     init();
     //Construction des obstacles
     map.build();
-    map.Astar(0, 1000, 2000, 1000, 1);
+    pc.printf("map built\n\r");
+    map.AStar(1000, 1000+1000/20, 1000+2000/20, 1000+1000/20, 1);
+    pc.printf("Astar done\n\r");
     path = map.path;
-    
+    pc.printf("Hello from main : taille=%f\n\r", path.size());
     for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, 0);
+        pc.printf("Hello from GotoXY : x=%f, y=%f\n\r", path[i].x, path[i].y);
+        odo.GotoXYT(20*(path[i].x-1000), 20*(path[i].y-1000), 0);
     }
         
     //odo.GotoXYT(500, 50, 0);
@@ -58,7 +61,7 @@
 void init(void)
 {
     pc.baud(9600);
-    pc.printf("Hello from main !\n\r");
+    pc.printf("Hello from init !\n\r");
     wait_ms(500);
     
     odo.setPos(0, 0, 0);
@@ -70,6 +73,7 @@
     mybutton.fall(&pressed);
     mybutton.rise(&unpressed);
     ticker.attach_us(&update_main,dt); // 100 Hz
+    pc.printf("Leave from init !\n\r");
 }
 
 void update_main(void)