Diff: main.cpp

Revision:

118:09bfc62d4867

Parent:

116:73d7d87e0299

Parent:

117:f8c147141a0c

Child:

119:c45efcd706d9

--- a/main.cpp	Wed May 06 11:14:02 2015 +0000
+++ b/main.cpp	Wed May 06 11:23:08 2015 +0000
@@ -7,6 +7,8 @@
 
 #include "Objectif.h"
 #include "Obj_clap.h"
+#include "Obj_pince.h"
+#include "Obj_depot.h"
 
 
 #include "AX12.h"
@@ -199,7 +201,17 @@
     if(couleur == COULEUR_JAUNE)
     {
         couleurRobot = COULEUR_JAUNE;
-        objectifs.push_back(new Obj_clap(0, 0, 0, &asserv, &ax12_brasG,&ax12_brasD));
+        
+        odometry.setTheta(PI/2);
+        odometry.setX(1000);
+        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.back()->setId(IDO_PC1);
+        
+        objectifs.push_back( new Obj_depot(1000, 500, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT);
     }
     else
     {
@@ -242,27 +254,46 @@
     
     while(continuer)
     {
+        logger.printf("Recherche d'objectif.......");
         int objAct = 0;
         bool newObj = false;
         for(objAct = 0 ; objAct < objectifs.size() ; objAct++)
         {
+            //logger.printf("Obj %d\r\n",objAct);
             if(objectifs[objAct]->isDone()) // Pas la peine de le faire, il est déjà fait ;)
                 continue;
-                
-            if(objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
+            //logger.printf("Pas done\r\n"); 
+            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);
+            if(retourAStar == 1) // L'objectif est atteignable !!
+            {
+                newObj = true;
+                break;
+            }
             
-            if(terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getX(), 0.05) != 1) // L'objectif est atteignable !!
+            if(retourAStar == 3) // Overload memoire
+            {
+                retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getX(), 50);
+            }
+            
+            if(retourAStar == 1) // L'objectif est atteignable !!
             {
                 newObj = true;
                 break;
             }
         }
         
+        logger.printf("[done]\r\n");
+        
         if(newObj)
         {
-            /* Déplacement vers le nouvel objectif */
-            for(i=0;i<terrain.path.size()-1;i++)
+            
+            logger.printf("-> Objectif : %d\r\n",objectifs[objAct]->getId());
+            
+            // Déplacement vers le nouvel objectif //
+            for(int 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);
@@ -271,11 +302,14 @@
             asserv.setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
             while(!asserv.isArrived())wait(0.1);
             
+            logger.printf("Execution de l'objectif....");
+            // Exection de l'objectif //
             objectifs[objAct]->run();
+            logger.printf("[done]\r\n");
         }
         else
         {
-            logger.printf("Nothind to be done ... :( \r\n");
+            logger.printf("-> Nothind to be done ... :( \r\n");
             wait(1);
         }
     }