Diff: main.cpp

Revision:

16:a1ede21a963b

Parent:

12:d5e21f71c2a9

Parent:

14:a394e27b8cb2

Child:

17:e32b4b54fc04

--- a/main.cpp	Tue Jan 05 15:55:56 2016 +0100
+++ b/main.cpp	Thu Jan 07 13:56:38 2016 +0100
@@ -1,4 +1,5 @@
 #include "Odometry.h"
+#include "Map/Map.h"
 
 #define dt 10000
 #define ATTENTE 0
@@ -24,16 +25,27 @@
 /** Debut du programme */
 int main(void)
 {
+    double angle_v = 2*PI/5;
+    double distance_v = 200.0;
+    std::vector<SimplePoint> path;
+    Map map;
+    
+    map.build();
     init();
-    //roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, -116878, accel_angle, vitesse_angle, deccel_angle, 116878, 1);
-    odo.GotoXY(1800,1500);
-    odo.GotoXY(2500,500);
-    odo.GotoXY(2000,300);
-    odo.GotoXYT(300,1000,0);
-    //for(int n=0; n<40; n++) odo.setTheta();
-    odo.GotoThet(-PI);
-    odo.GotoThet(0);
+
+    map.Astar(0, 1000, 2000, 1000, 1);
+    path = map.path;
+    
+    for(int i=0; i<path.size();i++)
+        odo.GotoXYT(path[i].x, path[i].y, 0);
+        
+    //odo.GotoXYT(500, 50, 0);
+    //odo.GotoXYT(200, 0, 0);
+    //odo.GotoXYT(0, 0, 0);
+
+    //odo.GotoThet(-PI/2);
     wait(2000);
+    //odo.GotoXYT(2250,500,0);
     while(1);
 }
 
@@ -43,7 +55,7 @@
     pc.printf("Hello from main !\n\r");
     wait_ms(500);
     
-    odo.setPos(300, 1000, 0);
+    odo.setPos(0, 0, 0);
     roboclaw.ForwardM1(ADR, 0);
     roboclaw.ForwardM2(ADR, 0);
     
@@ -58,8 +70,6 @@
 {
     odo.update_odo();
     //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-    //pc.printf("Theta : %3.2f\n\r", odo.getTheta()*180/PI);
-    pc.printf("X : %4.2f\n\r", odo.getX());
     //if(pc.readable()) if(pc.getc()=='l') led = !led;
 }