Diff: main.cpp

Revision:

114:be06d518b4a7

Parent:

111:c8a1129691da

Child:

116:73d7d87e0299

Child:

117:f8c147141a0c

--- a/main.cpp	Tue May 05 16:55:12 2015 +0000
+++ b/main.cpp	Wed May 06 06:50:05 2015 +0000
@@ -218,7 +218,7 @@
     #define IA
     #ifdef IA
     
-        logger.printf("Pathfinding ... ");
+        /*logger.printf("Pathfinding ... ");
         Timer t;
         t.start();
         int i = terrain.AStar(1000,300,1750,450,10);
@@ -227,18 +227,18 @@
         
         logger.printf("Return : %d in %.3fms\r\n",i,t.read()*1000);
         
-        for(i=0;i<terrain.path.size();i++)
+        for(i=0;i<terrain.path.size()-1;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);
             while(!asserv.isArrived())wait(0.1);
         }
+        asserv.setGoal(terrain.path[i].x,terrain.path[i].y);
+        while(!asserv.isArrived())wait(0.1);
         
         logger.printf("Finit !! \r\n");
         
-        while(1);
-    
-    #endif
+        while(1);*/
     
     while(continuer)
     {
@@ -261,17 +261,27 @@
         
         if(newObj)
         {
+            /* Déplacement vers le nouvel objectif */
+            for(i=0;i<terrain.path.size()-1;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);
+                while(!asserv.isArrived())wait(0.1);
+            }
+            asserv.setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
+            while(!asserv.isArrived())wait(0.1);
             
-            
+            objectifs[objAct]->run();
         }
         else
         {
             logger.printf("Nothind to be done ... :( \r\n");
             wait(1);
         }
-        
     }
     
+    #endif
+    
     #ifdef PLAN_A
         asserv.setGoal(300.0f,200.0f,0.0f);
         logger.printf("GOAL SET... RUNNING!\r\n");