Diff: main.cpp

Revision:

123:55e5e9acc541

Parent:

122:3d059ad76dd7

--- a/main.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/main.cpp	Mon May 11 20:32:11 2015 +0000
@@ -196,7 +196,7 @@
     // *--------------------------* //
     //     Tirrette + couleur       //
     
-    //while(tirette); // La tirrette
+    while(tirette); // La tirrette
     
     if(couleur == COULEUR_JAUNE)
     {
@@ -204,20 +204,38 @@
         
         odometry.setTheta(PI/2);
         odometry.setX(1000);
-        odometry.setY(400);
+        odometry.setY(177);
+        
+        objectifs.push_back( new Obj_depot(1000, 350, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT_PC);
         
-        //objectifs.push_back(new Obj_clap(0, 0, 0, &ax12_brasG,&ax12_brasD));
-        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, 350+100, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT_P);
+        objectifs.push_back( new Obj_depot(1000, 350+2*100, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT_P);
+        objectifs.push_back( new Obj_depot(1000, 350+3*100, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT_P);
+        objectifs.push_back( new Obj_depot(1100, 330+3*100, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT_P);
+        objectifs.push_back( new Obj_depot(900, 330+3*100, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_DEPOT_P);
         
-        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, 700, PI/2, &ax12_pince) );
+        //objectifs.back()->setId(IDO_PC1);
+        objectifs.push_back( new Obj_pince(1750, 250+350, 1750, 250+190, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_PC2);
+        objectifs.push_back( new Obj_pince(1750, 90+350, 1750, 90+190, -PI/2, &ax12_pince) );
+        objectifs.back()->setId(IDO_P5);
+        objectifs.push_back( new Obj_pince(1850-247, 90+247, 1850-130, 90+130, -PI/4, &ax12_pince) );
+        objectifs.back()->setId(IDO_P6);
         
-        objectifs.push_back( new Obj_pince(800, 620, 800, 710, PI/2, &ax12_pince) );
-        objectifs.back()->setId(IDO_PC2);
+        
+        objectifs.push_back( new Obj_clap(1750, 192, -PI/4, 1750, 400, PI/2, &ax12_brasG, &ax12_brasD) );
         
-        objectifs.push_back( new Obj_depot(1000, 500, -PI/2, &ax12_pince) );
-        objectifs.back()->setId(IDO_DEPOT);
+        objectifs.push_back( new Obj_pince(1770-247, 1100-247, 1770-135, 1100-135, PI/4, &ax12_pince) );
+        objectifs.back()->setId(IDO_P7);
+        objectifs.push_back( new Obj_pince(1355-247, 870-247, 1355-135, 870-135, PI/4, &ax12_pince) );
+        objectifs.back()->setId(IDO_P4);
     }
     else
     {
@@ -233,32 +251,23 @@
     // *--------------------------* //
     //              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
     
     while(continuer)
     {
+        if(terrain.getHeight(odometry.getX(), odometry.getY()) >= 32000)
+        {
+            logger.printf("Collision, echappement.....");
+            
+            // Réduction de la taille des obstacles
+            for(unsigned int i=0;i < terrain.obstacles.size();i++)
+                while(terrain.obstacles[i]->height(odometry.getX(), odometry.getY()) >= 32000)
+                    terrain.obstacles[i]->setRobotRadius(terrain.obstacles[i]->getRobotRadius()-1);
+            
+            logger.printf("[done]\r\n");
+        }
+        
         logger.printf("Recherche d'objectif.......");
         int objAct = 0;
         bool newObj = false;
@@ -272,8 +281,6 @@
             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;
             }
             
@@ -298,19 +305,23 @@
         
         logger.printf("[done]\r\n");
         
+        // On remet la taille des obstacles
+        for(unsigned int i=0;i < terrain.obstacles.size();i++)
+            terrain.obstacles[i]->setRobotRadius(ROBOTRADIUS);
+        
         if(newObj)
         {
             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);
+                logger.printf("Prochain point : %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("Dernier point : %.3f\t%.3f\r\n",objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta()*180/PI);
             asserv.setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
             while(!asserv.isArrived())wait(0.1);
             
@@ -324,8 +335,22 @@
         }
         else
         {
-            logger.printf("-> Nothind to be done ... :( \r\n");
+            logger.printf("-> Nothind to be done ... :D \r\n");
+            
+            ax12_brasG.setMaxTorque(MAX_TORQUE);
+            ax12_brasD.setMaxTorque(MAX_TORQUE);
+            
+            ax12_brasG.setGoal(130);
+            ax12_brasD.setGoal(BRASD_FERME);
             wait(1);
+            ax12_brasG.setGoal(BRASG_FERME);
+            ax12_brasD.setGoal(215);
+            wait(1);
+            ax12_brasD.setGoal(BRASD_FERME);
+            wait(1);
+            
+            ax12_brasG.setMaxTorque(0);
+            ax12_brasD.setMaxTorque(0);
         }
     }
     
@@ -394,31 +419,31 @@
         asserv.setGoal(1,5);
         logger.printf("Attente\r\n");
         while(!asserv.isArrived())wait(0.1);
-        logger.printf("Arrivé\r\n");
+        logger.printf("Arrive\r\n");
         asserv.setGoal(1,10);
         logger.printf("Attente\r\n");
         while(!asserv.isArrived())wait(0.1);
-        logger.printf("Arrivé\r\n");
+        logger.printf("Arrive\r\n");
         asserv.setGoal(1,15);
         logger.printf("Attente\r\n");
         while(!asserv.isArrived())wait(0.1);
-        logger.printf("Arrivé\r\n");
+        logger.printf("Arrive\r\n");
         asserv.setGoal(1,20);
         logger.printf("Attente\r\n");
         while(!asserv.isArrived())wait(0.1);
-        logger.printf("Arrivé\r\n");
+        logger.printf("Arrive\r\n");
         asserv.setGoal(1,25);
         logger.printf("Attente\r\n");
         while(!asserv.isArrived())wait(0.1);
-        logger.printf("Arrivé\r\n");
+        logger.printf("Arrive\r\n");
         asserv.setGoal(1,30);
         logger.printf("Attente\r\n");
         while(!asserv.isArrived())wait(0.1);
-        logger.printf("Arrivé\r\n");
+        logger.printf("Arrive\r\n");
         asserv.setGoal(0,0,PI/2);
         logger.printf("Attente\r\n");
         while(!asserv.isArrived())wait(0.1);
-        logger.printf("Arrivé\r\n");
+        logger.printf("Arrive\r\n");
         while(1);
         /*logger.printf("Arrivé\r\n");
         wait(1);