Diff: main.cpp

Revision:

110:7e71e5cd8197

Parent:

109:53918ba98306

Parent:

108:890094ee202a

Child:

111:c8a1129691da

--- a/main.cpp	Tue May 05 16:35:53 2015 +0000
+++ b/main.cpp	Tue May 05 16:52:09 2015 +0000
@@ -149,10 +149,16 @@
     
     logger.printf("Appuyer sur le bouton pour mettre en position\r\n");
     
-    //while(!bp); // On attend le top de mise en position
+    while(!bp); // On attend le top de mise en position
     
     logger.printf("MIP........................");
     
+    odometry.update(1);
+    
+    odometry.setTheta(PI/2);
+    odometry.setX(1000);
+    odometry.setY(400);
+    
     ax12_pince.setMaxTorque(MAX_TORQUE);
     ax12_brasG.setMaxTorque(MAX_TORQUE);
     ax12_brasD.setMaxTorque(MAX_TORQUE);
@@ -209,35 +215,30 @@
     // *--------------------------* //
     //              IA              //
     
-    logger.printf("Pathfinding ... ");
-    Timer t;
-    t.start();
-    int i = terrain.AStar(1000,300,1750,250,10);
-    t.stop();
-    logger.printf("[done]\r\n");
-    
-    logger.printf("Return : %d in %.3fms\r\n",i,t.read()*1000);
-    
-    for(i=0;i<terrain.path.size();i++)
-    {
-        logger.printf("%d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
-        //asserv.setGoal(terrain.path[i].x,terrain.path[i].y
-    }
+    #define IA
+    #ifdef IA
     
-    while(1);
-    
-    for(int x=-10;x<2000;x+=20)
-    {
-        for(int y=-10;y<3000;y+=20)
+        logger.printf("Pathfinding ... ");
+        Timer t;
+        t.start();
+        int i = terrain.AStar(1000,300,1750,450,10);
+        t.stop();
+        logger.printf("[done]\r\n");
+        
+        logger.printf("Return : %d in %.3fms\r\n",i,t.read()*1000);
+        
+        for(i=0;i<terrain.path.size();i++)
         {
-            if(terrain.getHeight(x,y) >= 32000)
-                logger.printf("x");
-            else
-                logger.printf(" ");
+            logger.printf("%d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
+            asserv.setGoal(terrain.path[i].x,terrain.path[i].y);
+            while(!asserv.isArrived())wait(0.1);
         }
-        logger.printf("\r\n");
-    }
+        
+        logger.printf("Finit !! \r\n");
+        
+        while(1);
     
+    #endif
     
     while(continuer)
     {
@@ -311,7 +312,27 @@
             #endif
         }
     #else
-        asserv.setGoal(45,45,0);
+        asserv.setGoal(1,300,0);
+        logger.printf("Attente\r\n");
+        while(!asserv.isArrived())wait(0.1);
+        logger.printf("Arrivé\r\n");
+        wait(1);
+        asserv.setGoal(-300,300,0);
+        while(!asserv.isArrived())wait(0.1);
+        wait(1);
+        asserv.setGoal(-300,-1,0);
+        while(!asserv.isArrived())wait(0.1);
+        wait(1);
+        asserv.setGoal(-300,-300,0);
+        while(!asserv.isArrived())wait(0.1);
+        wait(1);
+        asserv.setGoal(1,-300,0);
+        while(!asserv.isArrived())wait(0.1);
+        wait(1);
+        asserv.setGoal(300,-300,0);
+        while(!asserv.isArrived())wait(0.1);
+        wait(1);
+        asserv.setGoal(0,0,0);
     #endif
     
     while(1);