Diff: main.cpp

Revision:

119:c45efcd706d9

Parent:

118:09bfc62d4867

Child:

120:06643cbec98a

Child:

121:0cc17ba879cb

--- a/main.cpp	Wed May 06 11:23:08 2015 +0000
+++ b/main.cpp	Wed May 06 15:17:16 2015 +0000
@@ -207,9 +207,15 @@
         odometry.setY(400);
         
         //objectifs.push_back(new Obj_clap(0, 0, 0, &ax12_brasG,&ax12_brasD));
-        objectifs.push_back( new Obj_pince(1750, 550, -PI/2, &ax12_pince) );
+        objectifs.push_back( new Obj_pince(1750, 600, 1750, 450, -PI/2, &ax12_pince) );
         objectifs.back()->setId(IDO_PC1);
         
+        objectifs.push_back( new Obj_depot(1000, 400, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT);
+        
+        objectifs.push_back( new Obj_pince(800, 620, 800, 710, PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_PC2);
+        
         objectifs.push_back( new Obj_depot(1000, 500, -PI/2, &ax12_pince) );
         objectifs.back()->setId(IDO_DEPOT);
     }
@@ -227,31 +233,30 @@
     // *--------------------------* //
     //              IA              //
     
+    /*while(1)
+    {
+        if(logger.readable())
+        {
+            char c = logger.getc();
+            logger.printf("%c",c);
+            if(c == 'a')
+            {
+                motorL.setSpeed(motorL.getSpeed()+0.05);
+                motorR.setSpeed(motorR.getSpeed()+0.05);
+                logger.printf("%.2f\r\n",motorL.getSpeed());
+            }
+            if(c == 'z')
+            {
+                motorL.setSpeed(motorL.getSpeed()-0.05);
+                motorR.setSpeed(motorR.getSpeed()-0.05);
+                logger.printf("%.2f\r\n",motorL.getSpeed());
+            }
+        }
+    }*/
+    
     #define IA
     #ifdef IA
     
-        logger.printf("Pathfinding ... ");
-        Timer t;
-        t.start();
-        int i = terrain.AStar(1000,400,1750,600,10);
-        t.stop();
-        logger.printf("[done]\r\n");
-        
-        logger.printf("Return : %d in %.3fms\r\n",i,t.read()*1000);
-        
-        for(i=1;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);
-    
     while(continuer)
     {
         logger.printf("Recherche d'objectif.......");
@@ -266,7 +271,18 @@
             if(!objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
                 continue;
             //logger.printf("Actif\r\n");
-            int retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getX(), 5);
+            int retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 10);
+            if(retourAStar == 1) // L'objectif est atteignable !!
+            {
+                newObj = true;
+                for(unsigned int i=0;i<terrain.path.size();i++)
+                    logger.printf("%d : %.3f %.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
+                break;
+            }
+            
+            if(retourAStar == 3) // Overload memoire
+                retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 25);
+            
             if(retourAStar == 1) // L'objectif est atteignable !!
             {
                 newObj = true;
@@ -274,9 +290,7 @@
             }
             
             if(retourAStar == 3) // Overload memoire
-            {
-                retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getX(), 50);
-            }
+                retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 50);
             
             if(retourAStar == 1) // L'objectif est atteignable !!
             {
@@ -290,7 +304,7 @@
         if(newObj)
         {
             
-            logger.printf("-> Objectif : %d\r\n",objectifs[objAct]->getId());
+            logger.printf("-> Objectif : %d %d\r\n",objectifs[objAct]->getId(),terrain.path.size());
             
             // Déplacement vers le nouvel objectif //
             for(int i=1;i<terrain.path.size()-1;i++)