Diff: main.cpp

Revision:

109:53918ba98306

Parent:

103:6a38cc0765f5

Child:

110:7e71e5cd8197

--- a/main.cpp	Mon May 04 13:00:28 2015 +0000
+++ b/main.cpp	Tue May 05 16:35:53 2015 +0000
@@ -1,12 +1,14 @@
 #include "mbed.h"
 
-#define PLAN_B
-#define OUT_USB
-
 #include "defines.h"
 
 #include "QEI.h"
 #include "Map.h"
+
+#include "Objectif.h"
+#include "Obj_clap.h"
+
+
 #include "AX12.h"
 
 #ifdef PLAN_A
@@ -80,11 +82,17 @@
 //         Decl. Asserv         //
 
 #ifdef PLAN_A
-    Asserv<float> instanceAsserv(&odometry);
+    Asserv<float> asserv(&odometry);
 #else
     aserv_planB asserv(odometry,motorL,motorR);
 #endif
 
+//           Decl. IA           //
+
+Map terrain;
+std::vector<Objectif*> objectifs;
+char couleurRobot = COULEUR_JAUNE;
+
 //           Fin Decl.          //
 // *--------------------------* //
 
@@ -92,7 +100,7 @@
 int main()
 {
     #ifdef OUT_USB
-        logger.baud((int)921600);
+        logger.baud((int)9600);
     #endif
     
     // *--------------------------* //
@@ -127,6 +135,10 @@
         odometry.setY(0);
     #endif
     
+    //           Init. IA           //
+    
+    terrain.build();
+    
     logger.printf("[done]\r\n");
     
     //           Fin Init.          //
@@ -135,9 +147,9 @@
     // *--------------------------* //
     //              MIP             //
     
-    logger.printf("Appuyer sur le bouton pour mettre ne position\r\n");
+    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........................");
     
@@ -172,11 +184,95 @@
     
     //                              //
     // *--------------------------* //
+    
+    // *--------------------------* //
+    //     Tirrette + couleur       //
+    
+    //while(tirette); // La tirrette
+    
+    if(couleur == COULEUR_JAUNE)
+    {
+        couleurRobot = COULEUR_JAUNE;
+        objectifs.push_back(new Obj_clap(0, 0, 0, &asserv, &ax12_brasG,&ax12_brasD));
+    }
+    else
+    {
+        couleurRobot = COULEUR_VERTE;
+    }
+    
+    
+    //                              //
+    // *--------------------------* //
 
     bool continuer = true;
     
+    // *--------------------------* //
+    //              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
+    }
+    
+    while(1);
+    
+    for(int x=-10;x<2000;x+=20)
+    {
+        for(int y=-10;y<3000;y+=20)
+        {
+            if(terrain.getHeight(x,y) >= 32000)
+                logger.printf("x");
+            else
+                logger.printf(" ");
+        }
+        logger.printf("\r\n");
+    }
+    
+    
+    while(continuer)
+    {
+        int objAct = 0;
+        bool newObj = false;
+        for(objAct = 0 ; objAct < objectifs.size() ; 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
+                continue;
+            
+            if(terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getX(), 0.05) != 1) // L'objectif est atteignable !!
+            {
+                newObj = true;
+                break;
+            }
+        }
+        
+        if(newObj)
+        {
+            
+            
+        }
+        else
+        {
+            logger.printf("Nothind to be done ... :( \r\n");
+            wait(1);
+        }
+        
+    }
+    
     #ifdef PLAN_A
-        instanceAsserv.setGoal(300.0f,200.0f,0.0f);
+        asserv.setGoal(300.0f,200.0f,0.0f);
         logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
@@ -185,32 +281,32 @@
         {
             #define test
             #ifdef test            
-                if(state == 0 && instanceAsserv.isArrivedRho())
+                if(state == 0 && asserv.isArrivedRho())
                 {
                     state = 1;
                     logger.printf("Arrive en 0,0 !!!!!\r\n");
                     motorR.setSpeed(0.0f);
                     motorL.setSpeed(0.0f);
                     wait(5);
-                    instanceAsserv.setGoal(300.0f,200.0f,0.0f);
+                    asserv.setGoal(300.0f,200.0f,0.0f);
                 }
-                else if(state == 1 && instanceAsserv.isArrivedRho())
+                else if(state == 1 && asserv.isArrivedRho())
                 {
                     state = 2;
                     logger.printf("Arrive en 200,200 !!!!!\r\n");
                     motorR.setSpeed(0.0f);
                     motorL.setSpeed(0.0f);
                     wait(5);
-                    instanceAsserv.setGoal(0.0f,300.0f,0.0f);
+                    asserv.setGoal(0.0f,300.0f,0.0f);
                 }
-                else if(state == 2 && instanceAsserv.isArrivedRho())
+                else if(state == 2 && asserv.isArrivedRho())
                 {
                     state = 0;
                     logger.printf("Arrive en 0,200 !!!!!\r\n");
                     motorR.setSpeed(0.0f);
                     motorL.setSpeed(0.0f);
                     wait(5);
-                    instanceAsserv.setGoal(0.0f,0.0f,0.0f);
+                    asserv.setGoal(0.0f,0.0f,0.0f);
                 }
             #endif
         }
@@ -227,17 +323,15 @@
     t.reset();
     
     odometry.update(dt);
+    asserv.update(dt);
     
     #ifdef PLAN_A
-        instanceAsserv.update(dt);
         float phi_r = (float)instanceAsserv.getPhiR();
         float phi_l = (float)instanceAsserv.getPhiL();
         float phi_max = (float)instanceAsserv.getPhiMax();
         
         motorR.setSpeed(0.08+(phi_r/phi_max));
         motorL.setSpeed(0.08+(phi_l/phi_max));
-    #else
-        asserv.update(dt);
     #endif
 }