Revision 123:55e5e9acc541, committed 2015-05-11

Comitter:

Jagang

Date:

Mon May 11 20:32:11 2015 +0000

Parent:

122:3d059ad76dd7

Commit message:

Maj AI; Repositionnement du d?part (1000,177); Ajout de l'action sur les claps

Changed in this revision

--- a/Asserv_Plan_B/planB.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Mon May 11 20:32:11 2015 +0000
@@ -27,7 +27,7 @@
     Kd_angle = 0.2;
     
     Kp_distance = 0.0041;
-    Ki_distance = 0.00003;//0.000001
+    Ki_distance = 0.0001;//0.000001
     Kd_distance = 0.01;//0.05
     
     N = 0;
@@ -52,6 +52,7 @@
 {
     m_motorL.setSpeed(0);
     m_motorR.setSpeed(0);
+    state = 0;
 }
 
 void aserv_planB::setGoal(float x, float y)
@@ -77,12 +78,6 @@
     
     if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
     if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
-    
-    if(state == 0)
-    {
-        m_motorL.setSpeed(0);
-        m_motorR.setSpeed(0);
-    }
 
     // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
     if(state == 1)
@@ -103,7 +98,7 @@
             m_motorL.setSpeed(0);
             m_motorR.setSpeed(0);
             state = 2;
-            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
             somme_erreur_theta = 0;
         }
     }
@@ -159,6 +154,8 @@
             erreur_precedente_theta = 0;
             somme_erreur_theta = 0;
             erreur_theta = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
             if(squip == true) 
             {
                 arrived = true;
@@ -166,7 +163,7 @@
                 state = 0;
             }
             else state = 3;
-            logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
+            //logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
         }
     }
 
@@ -190,11 +187,13 @@
         else N = 0;
         if(N > 10)
         {
-            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
             somme_erreur_theta = 0;
             arrived = true;
             squip = false;
             state = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
         }
     }
 }

--- a/Map/Map.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Map.cpp	Mon May 11 20:32:11 2015 +0000
@@ -1,10 +1,17 @@
 #include "Map.h"
 
-#include "mbed.h"
 #include "Obs_circle.h"
 #include "Obs_rect.h"
 
-extern Serial logger;
+#ifdef CODEBLOCK
+    using namespace std;
+    #include <iostream>
+    #include <fstream>
+#else
+    #include "mbed.h"
+    extern Serial logger;
+#endif
+#include <math.h>
 
 Map::Map()
 {
@@ -19,14 +26,50 @@
 
 void Map::build()
 {
-    //obstacles.push_back(new Obs_circle(ROBOTRADIUS,2,2,1));
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,-100,-100,2100,0)); // MG
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MG,-100,-100,2100,0));// MG
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MB,2100,-100,2000,3100));// MB
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MH,-100,-100,0,3100));// MH
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MD,2100,3000,-100,3100));// MD
+    
+    
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M1,778,0,800,400));// M1
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M2,1200,0,1222,400));// M2
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M3,800,0,1200,70));// M3
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M4,1222,2600,1200,3000));// M4
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M5,1200,2930,800,3000));// M5
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M6,800,2600,778,3000));// M6
+    
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_E,580,967,0,2033));// E
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_S,2000,1200,1900,1800));// S
+    
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D1,70,265,0,335));// D1
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D2,70,565,0,636));// D2
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D3,70,2365,0,2435));// D3
+    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D4,70,2665,0,2735));// D4
     
     
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,778,0,800,400)); // M1
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,1200,0,1222,400)); // M2
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,800,0,1200,70)); // M3
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC1,800,910,35));// PC1
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC3,1650,1500,35));// PC3
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC2,1750,250,35));// PC2
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC4,1750,3000-250,35));// PC4
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC5,800,3000-910,35));// PC5
     
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P1,200,90,30));// P1
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P2,100,850,30));// P2
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P3,200,850,30));// P3
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P4,1355,870,30));// P4
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P5,1750,90,30));// P5
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P6,1850,90,30));// P6
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P7,1770,1100,30));// P7
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P8,1400,1300,30));// P8
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P9,200,3000-90,30));// P9
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P10,100,3000-850,30));// P10
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P11,200,3000-850,30));// P11
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P12,1355,3000-870,30));// P12
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P13,1750,3000-90,30));// P13
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P14,1850,3000-90,30));// P14
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1400,3000-1300,30));// P16
 }
 
 int Map::getHeight(float x, float y)
@@ -53,22 +96,12 @@
     //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
     
     path.clear();
-    #if LOG_LEVEL >= 3
-        logger.printf("[info - pathfinder] Start point (%.1f,%.1f)\r\n",x,y);
-        logger.printf("[info - pathfinder] Goal point (%.1f,%.1f)\r\n",goal_x,goal_y);
-    #endif
     
     Point goal(goal_x/mpc,goal_y/mpc);
     if(getHeight(goal_x,goal_y) >= 32000)
     {
         #if LOG_LEVEL >= 2
-            logger.printf("[warning - pathfinder] Unreachable point (%.1f,%.1f) %d\r\n",goal_x,goal_y,getHeight(goal_x,goal_y));
-            
-            for(unsigned int i=0;i<obstacles.size();i++)
-            {
-                logger.printf("%d %d\r\n",i,obstacles[i]->height(goal_x,goal_y));
-            }
-            
+            logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
         #endif
         return 4;
     }
@@ -122,7 +155,10 @@
         {
             for(int dy=-1;dy<=1;dy++)
             {
-                if(dx==0 && dy==0) continue;
+                if(dx==0 && dy==0)
+                    continue;
+                if(!(dx == 0 || dy == 0)) // On skype les mouvement diagoneaux
+                    continue;
                 
                 Point *p = new Point(current->getx()+dx,current->gety()+dy);
                 instanceDePoint++;
@@ -198,8 +234,22 @@
     
     if(!openList.empty())
     {
-        for(i=0;i<openList.size();i++)
-            delete openList[i];
+        #ifdef CODEBLOCK
+            ofstream f_openlist("openlist.txt");
+            for(i=0;i<openList.size();i++)
+            {
+                f_openlist << i << "," << openList[i]->getx()*mpc << "," << openList[i]->gety()*mpc << endl;
+                delete openList[i];
+            }
+            f_openlist.close();
+            
+            ofstream f_closelist("closelist.txt");
+            for(i=0;i<closeList.size();i++)
+            {
+                f_closelist << i << "," << closeList[i]->getx()*mpc << "," << closeList[i]->gety()*mpc << endl;
+            }
+            f_closelist.close();
+        #endif
         
         // On reconstruit le chemin
         #if LOG_LEVEL >= 3
@@ -213,6 +263,21 @@
             c = c->getParent();
         }
         
+        path.front().x=x;
+        path.front().y=y;
+        
+        path.back().x=goal_x;
+        path.back().y=goal_y;
+        
+        #ifdef CODEBLOCK
+            ofstream f_path("path.txt");
+            for(i=0;i<path.size();i++)
+            {
+                f_path << i << "," << path[i].x << "," <<  path[i].y << endl;
+            }
+            f_path.close();
+        #endif
+        
         for(i=0;i<closeList.size();i++)
             delete closeList[i];
         
@@ -224,105 +289,104 @@
         
         // Algo de 'tendage' du chemin
         bool continuer = true;
-        std::vector<SimplePoint> tempPath;
+        unsigned int pointValide = 0;
+        
+        #ifdef CODEBLOCK
+            ofstream f_tendage("tendage.txtt");
+        #endif // CODEBLOCK
+        
+        
         while(continuer)
         {
-            for(i=1;i<path.size();i+=2)
+            continuer = false;
+            
+            for(unsigned int i1=pointValide;i1<path.size();i1++)
             {
-                tempPath.push_back(path[i-1]);
-                int height = (getHeight(path[i-1].x*mpc,path[i-1].y*mpc)+getHeight(path[i+1].x*mpc,path[i+1].y*mpc))/2;
-                
-                #if LOG_LEVEL >= 4 & LOG_TENDEUR
-                    logger.printf("[info - tendeur] Testing (%.3f,%.3f) %d\r\n",path[i].x,path[i].y,height);
-                #endif
+                bool necessaire = false;
                 
-                if(path[i-1].x!=path[i+1].x)
-                {
-                    // y=a*x+b
-                    float a = (path[i-1].y-path[i+1].y)/(path[i-1].x-path[i+1].x);
-                    float b = path[i-1].y - a*path[i-1].x;
-                    
-                    float step = (mpc*0.5f)*cos(atan(a));
-                    
-                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
-                        logger.printf("[info - tendeur] X: a=%.2f b=%.2f\r\n",a,b);
-                        logger.printf("[info - tendeur] x [%.3f,%.3f] step=%.6f\r\n",path[i-1].x,path[i+1].x,step);
-                    #endif
-                    float x;
-                    for(x=min(path[i-1].x,path[i+1].x);x<max(path[i-1].x,path[i+1].x);x+=step)
-                    {
-                        #if LOG_LEVEL >= 5 & LOG_TENDEUR
-                            logger.printf("%.3f\t%.3f\t%.3f\r\n",getHeight(x,(a*x+b)),x,(a*x+b));
-                        #endif
-                        if(getHeight(x,(a*x+b)) > height) // Ca ne passe pas sans ce point
-                        {
-                            tempPath.push_back(path[i]);
-                            
-                            #if LOG_LEVEL >= 4 & LOG_TENDEUR
-                                logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
-                            #endif
-                            break;
-                        }
-                    }
-                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
-                        if(x >= max(path[i-1].x,path[i+1].x))
-                        {
-                            logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
-                        }
-                    #endif
-                }
-                else if(path[i-1].y!=path[i+1].y)
+                for(unsigned int i2=i1+2;i2<path.size();i2++)
                 {
-                    // x=a*y+b
-                    float a = (path[i-1].x-path[i+1].x)/(path[i-1].y-path[i+1].y);
-                    float b = path[i-1].x - a*path[i-1].y;
-                    
-                    float step = (mpc*0.5f)*cos(atan(a));
-                    
-                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
-                        logger.printf("[info - tendeur] Y: a=%.2f b=%.2f\r\n",a,b);
-                        logger.printf("[info - tendeur] y [%.3f,%.3f] step=%.6f\r\n",path[i-1].y,path[i+1].y,step);
-                    #endif
-                    float y;
-                    for(y=min(path[i-1].y,path[i+1].y);y<max(path[i-1].y,path[i+1].y);y+=step)
+                    //cout << "Entre " << i1 << " et " << i2 << endl;
+                    if(path[i1].x!=path[i2].x && atan((path[i1].y-path[i2].y)/(path[i1].x-path[i2].x)) < PI/4)
                     {
-                        #if LOG_LEVEL >= 5 & LOG_TENDEUR
-                            logger.printf("%.3f\t%.3f\t%.3f\t%.3f\r\n",getHeight((a*y+b),y),height,(a*y+b),y);
-                        #endif
-                        if(getHeight((a*y+b),y) > height) // Ca ne passe pas sans ce point
+                        float a = (path[i1].y-path[i2].y)/(path[i1].x-path[i2].x);
+                        float b = path[i1].y - a*path[i1].x;
+                        float step = (mpc*0.5f)*cos(atan(a));
+                        
+                        necessaire = false;
+                        
+                        for(x=min(path[i1].x,path[i2].x);x<max(path[i1].x,path[i2].x);x+=step)
                         {
-                            tempPath.push_back(path[i]);
+                            y=a*x+b;
+                            
+                            #ifdef CODEBLOCK
+                                f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
+                            #endif // CODEBLOCK
                             
-                            #if LOG_LEVEL >= 4 & LOG_TENDEUR
-                                logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
-                            #endif
-                            break;
+                            if(getHeight(x,y) >= 32000)
+                            {   
+                                necessaire = true;
+                                break;
+                            }
+                        }
+                        
+                        if(!necessaire)
+                        {
+                            // Ca n'a pas touché, on peut supprimmer le point entre les deux
+                            continuer = true;
+                            path.erase(path.begin()+i2-1);
                         }
+                        else
+                            pointValide++;
+                        break;
                     }
-                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
-                        if(y >= max(path[i-1].y,path[i+1].y))
+                    else
+                    {
+                        // x=a*y+b
+                        float a = (path[i1].x-path[i2].x)/(path[i1].y-path[i2].y);
+                        float b = path[i1].x - a*path[i1].y;
+                        float step = (mpc*0.5f)*cos(atan(a));
+                        
+                        necessaire = false;
+                        
+                        for(y=min(path[i1].y,path[i2].y);y<max(path[i1].y,path[i2].y);y+=step)
                         {
-                            logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
+                            x=a*y+b;
+                            
+                            #ifdef CODEBLOCK
+                                f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
+                            #endif // CODEBLOCK
+                            
+                            if(getHeight(x,y) >= 32000)
+                            {
+                                necessaire = true;
+                                break;
+                            }
                         }
-                    #endif
+                        
+                        if(!necessaire)
+                        {
+                            // Ca n'a pas touché, on peut supprimmer le point entre les deux
+                            continuer = true;
+                            path.erase(path.begin()+i2-1);
+                        }
+                        else
+                            pointValide++;
+                        break;
+                        
+                    }
                 }
+                if(continuer)
+                    break;
             }
-            if(path.back() != tempPath.back())
-                tempPath.push_back(path.back());
-            
-            if(tempPath.size() < path.size()) // On a réussit a enlever un point
-            {
-                path.clear();
-                for(i=0;i<tempPath.size();i++)
-                    path.push_back(tempPath[i]);
-                continuer = true;
-            }
-            else
-                continuer = false;
-            
-            tempPath.clear();
         }
         
+        #ifdef CODEBLOCK
+            f_tendage.close();
+        #endif
+        
+        
+        
         #if LOG_LEVEL >= 3
             logger.printf("[done] %d elements\r\n",path.size());
         #endif
@@ -345,5 +409,3 @@
 
 
 
-
-

--- a/Map/Map.h	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Map.h	Mon May 11 20:32:11 2015 +0000
@@ -41,8 +41,8 @@
         
         
         std::vector<SimplePoint> path;
+        std::vector<Obstacle*> obstacles;
     private:
-        std::vector<Obstacle*> obstacles;
 };
 
 

--- a/Map/Objectifs/Obj_clap.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Objectifs/Obj_clap.cpp	Mon May 11 20:32:11 2015 +0000
@@ -1,16 +1,47 @@
 #include "Obj_clap.h"
 
+#ifdef PLAN_A
+    extern Asserv<float> asserv;
+#else
+    extern aserv_planB asserv;
+#endif
 
-Obj_clap::Obj_clap(float x, float y, float theta, AX12 *ax12_brasG, AX12 *ax12_brasD)
+Obj_clap::Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD)
 :Objectif(x,y,theta)
 {
     this->ax12_brasG = ax12_brasG;
     this->ax12_brasD = ax12_brasD;
+    this->x2 = x2;
+    this->y2 = y2;
+    this->theta2 = theta2;
 }
 
 void Obj_clap::run()
 {
+    if(theta2 == PI/2)
+    {
+        ax12_brasD->setMaxTorque(MAX_TORQUE);
+        ax12_brasD->setGoal(BRASD_OUVERT);
+        wait(1);
+        asserv.setGoal(x2,y2,theta2);
+        while(!asserv.isArrived())wait(0.1);
+        ax12_brasD->setGoal(BRASD_FERME);
+        wait(1);
+        ax12_brasD->setMaxTorque(0);
+    }
+    else
+    {
+        ax12_brasG->setMaxTorque(MAX_TORQUE);
+        ax12_brasG->setGoal(BRASG_OUVERT);
+        wait(1);
+        asserv.setGoal(x2,y2,theta2);
+        while(!asserv.isArrived())wait(0.1);
+        ax12_brasG->setGoal(BRASG_FERME);
+        wait(1);
+        ax12_brasG->setMaxTorque(0);
+    }
     
+    done = true;
 }
 
 int Obj_clap::isActive()

--- a/Map/Objectifs/Obj_clap.h	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Objectifs/Obj_clap.h	Mon May 11 20:32:11 2015 +0000
@@ -9,12 +9,13 @@
 class Obj_clap: public Objectif
 {
 public:
-    Obj_clap(float x, float y, float theta, AX12 *ax12_brasG, AX12 *ax12_brasD);
+    Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD);
     virtual void run();
     virtual int isActive();
 private:
     AX12 *ax12_brasG;
     AX12 *ax12_brasD;
+    float x2,y2,theta2;
 };
 
 

--- a/Map/Objectifs/Obj_pince.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Objectifs/Obj_pince.cpp	Mon May 11 20:32:11 2015 +0000
@@ -1,5 +1,6 @@
 #include "Obj_pince.h"
 #include <vector>
+#include "Map.h"
 
 #ifdef PLAN_A
     extern Asserv<float> asserv;
@@ -9,6 +10,7 @@
 
 extern Odometry2 odometry;
 extern std::vector<Objectif*> objectifs;
+extern Map terrain;
 
 Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
 :Objectif(x,y,theta)
@@ -30,12 +32,21 @@
     
     for(unsigned int i=0;i < objectifs.size();i++)
     {
-        if(objectifs[i]->getId() == IDO_DEPOT && !objectifs[i]->isDone())
+        if(IDO_PC1 <= id && id <= IDO_PC4 && objectifs[i]->getId() == IDO_DEPOT_PC && !objectifs[i]->isDone())
+        {
+            objectifs[i]->activate();
+            break;
+        }
+        if(IDO_P1 <= id && id <= IDO_P16 && objectifs[i]->getId() == IDO_DEPOT_P && !objectifs[i]->isDone())
         {
             objectifs[i]->activate();
             break;
         }
     }
+    
+    for(unsigned int i=0;i < terrain.obstacles.size();i++)
+        if(id == terrain.obstacles[i]->getId())
+            terrain.obstacles[i]->desactivate();
 }
 
 int Obj_pince::isActive()

--- a/Map/Objectifs/Objectif.h	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Objectifs/Objectif.h	Mon May 11 20:32:11 2015 +0000
@@ -9,13 +9,6 @@
     #include "planB.h"
 #endif
 
-enum ID
-{
-    IDO_PC1,
-    IDO_PC2,
-    IDO_DEPOT
-};
-
 class Objectif
 {
 public:

--- a/Map/Obstacles/Obs_circle.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Obstacles/Obs_circle.cpp	Mon May 11 20:32:11 2015 +0000
@@ -1,6 +1,6 @@
 #include "Obs_circle.h"
 
-Obs_circle::Obs_circle(float robotRadius, float x, float y, float size):Obstacle(robotRadius)
+Obs_circle::Obs_circle(float robotRadius, int id, float x, float y, float size):Obstacle(robotRadius,id)
 {
     this->x = x;
     this->y = y;
@@ -9,6 +9,9 @@
 
 int Obs_circle::height(float x, float y)
 {
+    if(!active)
+        return 0;
+    
     float d = (x-this->x)*(x-this->x) + (y-this->y)*(y-this->y);
     if(d <= size2) // On est dans le cercle
     {

--- a/Map/Obstacles/Obs_circle.h	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Obstacles/Obs_circle.h	Mon May 11 20:32:11 2015 +0000
@@ -6,9 +6,10 @@
 class Obs_circle: public Obstacle
 {
     public:
-        Obs_circle(float robotRadius, float x, float y, float size);
+        Obs_circle(float robotRadius, int id, float x, float y, float size);
         virtual int height(float x, float y);
         
+        //void setPos(float x, float y);
     private:
         float x,y,size2; // size2 c'est la taille au carré
 };

--- a/Map/Obstacles/Obs_rect.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Obstacles/Obs_rect.cpp	Mon May 11 20:32:11 2015 +0000
@@ -1,6 +1,6 @@
 #include "Obs_rect.h"
 
-Obs_rect::Obs_rect(float robotRadius, float x1, float y1, float x2, float y2):Obstacle(robotRadius)
+Obs_rect::Obs_rect(float robotRadius, int id, float x1, float y1, float x2, float y2):Obstacle(robotRadius,id)
 {
     this->x1 = (x1<x2) ? x1:x2;
     this->x2 = (x1<x2) ? x2:x1;
@@ -10,6 +10,8 @@
 
 int Obs_rect::height(float x, float y)
 {
+    if(!active)
+        return 0;
     if(bigShape)
     {
         if(x1 - robotRadius < x && x < x2 + robotRadius && y1 - robotRadius < y && y < y2 + robotRadius)

--- a/Map/Obstacles/Obs_rect.h	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Obstacles/Obs_rect.h	Mon May 11 20:32:11 2015 +0000
@@ -6,7 +6,7 @@
 class Obs_rect: public Obstacle
 {
     public:
-        Obs_rect(float robotRadius, float x1, float y1, float x2, float y2);
+        Obs_rect(float robotRadius, int id, float x1, float y1, float x2, float y2);
         virtual int height(float x, float y);
         
     private:

--- a/Map/Obstacles/Obstacle.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Obstacles/Obstacle.cpp	Mon May 11 20:32:11 2015 +0000
@@ -1,11 +1,12 @@
 #include "Obstacle.h"
 
-Obstacle::Obstacle(float robotRadius)
+Obstacle::Obstacle(float robotRadius, int id)
 {
     this->robotRadius = robotRadius;
+    this->id = id;
+    this->active = true;
     bigShape = true;
     smoothBigShape = false;
-    id = -1;
 }
 
 Obstacle::~Obstacle()

--- a/Map/Obstacles/Obstacle.h	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Obstacles/Obstacle.h	Mon May 11 20:32:11 2015 +0000
@@ -4,7 +4,7 @@
 class Obstacle
 {
     public:
-        Obstacle(float robotRadius);
+        Obstacle(float robotRadius,int id);
         virtual ~Obstacle();
         
         virtual int height(float x, float y) = 0;
@@ -21,8 +21,11 @@
         int getId(){return id;}
         void setId(int id){this->id = id;}
         
+        void activate(){active=true;}
+        void desactivate(){active=false;}
     protected:
         bool bigShape,smoothBigShape;
+        bool active;
         float robotRadius;
         
         int id;

--- a/defines.h	Thu May 07 14:18:07 2015 +0000
+++ b/defines.h	Mon May 11 20:32:11 2015 +0000
@@ -2,7 +2,51 @@
 #define DEFINE_H
 
 #define PLAN_B
-#define OUT_USB
+//#define OUT_USB
+
+
+enum ID
+{
+    IDO_MG,
+    IDO_MH,
+    IDO_MD,
+    IDO_MB,
+    IDO_M1,
+    IDO_M2,
+    IDO_M3,
+    IDO_M4,
+    IDO_M5,
+    IDO_M6,
+    IDO_D1,
+    IDO_D2,
+    IDO_D3,
+    IDO_D4,
+    IDO_E,
+    IDO_S,
+    IDO_PC1,
+    IDO_PC2,
+    IDO_PC3,
+    IDO_PC4,
+    IDO_PC5,
+    IDO_P1,
+    IDO_P2,
+    IDO_P3,
+    IDO_P4,
+    IDO_P5,
+    IDO_P6,
+    IDO_P7,
+    IDO_P8,
+    IDO_P9,
+    IDO_P10,
+    IDO_P11,
+    IDO_P12,
+    IDO_P13,
+    IDO_P14,
+    IDO_P15,
+    IDO_P16,
+    IDO_DEPOT_PC,
+    IDO_DEPOT_P
+};
 
 #define ROBOTRADIUS 190
 

--- 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);