Diff: main.cpp.orig

Revision:

25:6c5c453602ff

Parent:

17:e32b4b54fc04

diff -r 8e7f24c3d05f -r 6c5c453602ff main.cpp.orig
--- a/main.cpp.orig	Wed Jan 20 17:13:01 2016 +0100
+++ b/main.cpp.orig	Wed Jan 20 17:14:29 2016 +0100
@@ -1,4 +1,15 @@
 #include "Odometry.h"
+/** Dependencies :
+    Map/Point.h
+    Map/define.h
+    Map/Obstacles/Obstacle.h
+
+    Test en cours :
+    * Un obstacle seulement
+    * Radius du robot réduit à 1 (defines.cpp)
+    * Nombreux
+*/ 
+#include "Map/Map.h"
 
 #define dt 10000
 #define ATTENTE 0
@@ -24,16 +35,28 @@
 /** Debut du programme */
 int main(void)
 {
+    double angle_v = 2*PI/5;
+    double distance_v = 200.0;
+    std::vector<SimplePoint> path;
+    Map map;
+    
     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);
+    //Construction des obstacles
+    map.build();
+    map.Astar(0, 1000, 2000, 1000);
+    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 +66,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 +81,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;
 }