Revision 46:8eae88c45a78, committed 2016-04-20

Comitter:

IceTeam

Date:

Wed Apr 20 13:13:37 2016 +0000

Parent:

45:b53ae54062c6

Commit message:

Mise en static;

Changed in this revision

--- a/Functions/func.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/Functions/func.cpp	Wed Apr 20 13:13:37 2016 +0000
@@ -107,8 +107,8 @@
 
 void checkAround(void)
 {
-    if(capt1 > SEUIL+0.1) bleu = 1;
-    else bleu = 0;
+    if(capt1 > SEUIL+0.1) S::bleu = 1;
+    else S::bleu = 0;
     if(capt2 > SEUIL) blanc = 1;
     else blanc = 0;
     if(capt3 > SEUIL+0.1) rouge = 1;
@@ -135,8 +135,8 @@
 
 void init_interrupt(void)
 {
-    mybutton.fall(&pressed);
-    mybutton.rise(&unpressed);
+    S::mybutton.fall(&pressed);
+    S::mybutton.rise(&unpressed);
 
     EndL.fall(&ELpressed);
     EndL.rise(&ELunpressed);

--- a/Functions/func.h	Wed Apr 13 12:47:47 2016 +0000
+++ b/Functions/func.h	Wed Apr 20 13:13:37 2016 +0000
@@ -5,34 +5,13 @@
 #include "../RoboClaw/RoboClaw.h"
 #include "../Odometry/Odometry.h"
 #include "../StepperMotor/Stepper.h"
-#include "Map/Map.h"
+#include "Map/map.h"
 #include "AX12.h"
 
 #define SEUIL 0.25
 #define dt 10000
 
-extern Serial logger;
-extern RoboClaw roboclaw;
-extern DigitalOut bleu;
-extern DigitalOut blanc;
-extern DigitalOut rouge;
-extern Stepper RMot;
-extern Stepper ZMot;
-extern Stepper LMot;
-extern AnalogIn capt1;
-extern AnalogIn capt2;
-extern AnalogIn capt3;
-extern InterruptIn mybutton;
-extern InterruptIn EndR;
-extern InterruptIn EndZ;
-extern InterruptIn EndL;
-extern AX12 left_hand;
-extern AX12 right_hand;
-extern Ticker ticker;
-extern Odometry odo;
-
-extern int i, state;
-extern bool EL, EZ, ER;
+extern S STAT_CLASS;
 
 void ELpressed(void);
 void ELunpressed(void);

--- a/Map/Map.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,412 +0,0 @@
-#include "Map.h"
-#include "../Odometry/Odometry.h"
-#include "Obstacles/Obs_circle.h"
-
-#ifdef CODEBLOCK
-    using namespace std;
-    #include <iostream>
-    #include <fstream>
-#else
-    #include "mbed.h"
-    extern Serial logger;
-#endif
-#include <math.h>
-
-Map::Map()
-{
-    
-}
-
-Map::~Map()
-{
-    for(unsigned int i=0;i<obstacles.size();i++)
-        delete obstacles[i];
-}
-
-void Map::build()
-{
-    /*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_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,700,1000,100));// P16
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,600,100));// P16
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,1000,100));// P16
-}
-
-int Map::getHeight(float x, float y)
-{
-    int height = 0;
-    for(unsigned int i=0;i<obstacles.size();i++)
-        height += obstacles[i]->height(x,y);
-    return height;
-}
-
-float dist(Point *p1,Point *p2)
-{
-    int dx = p1->getx()-p2->getx();
-    if(dx<0) dx=-dx;
-    int dy = p1->gety()-p2->gety();
-    if(dy<0) dy=-dy;
-    return sqrt((float)dx*dx+dy*dy);
-}
-
-char Map::AStar(float x, float y, float goal_x, float goal_y, float mpc)
-{
-    /*! http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003 !*/
-    //float dx,dy; // Permet de diminuer l'erreur par rapport au centre des cases
-    //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
-    
-    path.clear();
-    
-    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 (%.3f,%.3f)\r\n",goal_x,goal_y);
-        #endif
-        return 4;
-    }
-    
-    if(getHeight(x,y) >= 32000)
-    {
-        #if LOG_LEVEL >= 2
-            logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
-        #endif
-        return 5;
-    }
-    
-    
-    unsigned int i=0;
-    unsigned int instanceDePoint=0;
-    
-    std::vector<Point*> openList;
-    openList.push_back(new Point(x/mpc,y/mpc));
-    openList[0]->setG(0);
-    openList[0]->setH(dist(openList[0],&goal));
-    openList[0]->setParent();
-    
-    std::vector<Point*> closeList;
-    
-    Point* current;
-    do
-    {
-        // On cherche le plus petit F dans la liste ouverte
-        current = openList[0];
-        
-        unsigned int pos = 0;
-        for(i=0;i<openList.size();i++)
-            if(openList[i]->getF() < current->getF())
-            {
-                current = openList[i];
-                pos = i;
-            }
-        
-        // On le place dans la liste fermé
-        closeList.push_back(current);
-        openList.erase(openList.begin()+pos);
-        
-        #if LOG_LEVEL >= 4 && LOG_ASTAR
-            logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
-            logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
-        #endif
-        
-        // On ajoute tous ses voisins viable das la liste ouverte
-        for(int dx=-1;dx<=1;dx++)
-        {
-            for(int dy=-1;dy<=1;dy++)
-            {
-                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++;
-                if(p == 0 || instanceDePoint >= MAXPOINT) // Overload !!!
-                {
-                    for(unsigned int i=0;i<openList.size();i++)
-                        delete openList[i];
-                    for(unsigned int i=0;i<closeList.size();i++)
-                        delete closeList[i];
-                    
-                    path.clear();
-                        
-                    #if LOG_LEVEL >= 1
-                        logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
-                    #endif
-                    
-                    return 3;
-                }
-                
-                if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
-                {
-                    delete p;
-                    continue; 
-                }
-                
-                int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
-                if(height>=32000)  // On ignore le point, il n'est pas accessible
-                {
-                    delete p;
-                    continue; // On ignore le point si il est déjà dans la liste fermé
-                }
-                
-                if(p->in(openList,pos))
-                {
-                    if(dx == 0 || dy == 0) // Mouvement non diagonnal
-                    {
-                        if(current->getG() + NDIAG_COST < openList[pos]->getG())
-                        {
-                            openList[pos]->setG(current->getG() + NDIAG_COST);
-                            openList[pos]->setParent(current);
-                        }
-                    }
-                    else
-                    {
-                        if(current->getG() + DIAG_COST < openList[pos]->getG())
-                        {
-                            openList[pos]->setG(current->getG() + DIAG_COST);
-                            openList[pos]->setParent(current);
-                        }
-                    }
-                    
-                    delete p;
-                }
-                else
-                {
-                    openList.push_back(p);
-                    if(dx == 0 || dy == 0) // Mouvement non diagonnal
-                        p->setG(current->getG() + NDIAG_COST);
-                    else
-                        p->setG(current->getG() + DIAG_COST);
-                    p->setH(height + dist(p,&goal));
-                    p->setParent(current);
-                }
-            }
-        }
-    }
-    while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
-    
-    
-    #if LOG_LEVEL >= 3
-        logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
-    #endif
-    
-    if(!openList.empty())
-    {
-        #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
-            logger.printf("[info - pathfinder] Recontruction du chemin ... ");
-        #endif
-        path.clear();
-        Point* c = closeList.back();
-        while(c != 0)
-        {
-            path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
-            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];
-        
-        #if LOG_LEVEL >= 3
-            logger.printf("[done] %d elements\r\n",path.size());
-            logger.printf("[info - pathfinder] Tendage du chemin ... ");
-        #endif
-        
-        
-        // Algo de 'tendage' du chemin
-        bool continuer = true;
-        unsigned int pointValide = 0;
-        
-        #ifdef CODEBLOCK
-            ofstream f_tendage("tendage.txtt");
-        #endif // CODEBLOCK
-        
-        
-        while(continuer)
-        {
-            continuer = false;
-            
-            for(unsigned int i1=pointValide;i1<path.size();i1++)
-            {
-                bool necessaire = false;
-                
-                for(unsigned int i2=i1+2;i2<path.size();i2++)
-                {
-                    //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)
-                    {
-                        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)
-                        {
-                            y=a*x+b;
-                            
-                            #ifdef CODEBLOCK
-                                f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
-                            #endif // CODEBLOCK
-                            
-                            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;
-                    }
-                    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)
-                        {
-                            x=a*y+b;
-                            
-                            #ifdef CODEBLOCK
-                                f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
-                            #endif // CODEBLOCK
-                            
-                            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(continuer)
-                    break;
-            }
-        }
-        
-        #ifdef CODEBLOCK
-            f_tendage.close();
-        #endif
-        
-        
-        
-        #if LOG_LEVEL >= 3
-            logger.printf("[done] %d elements\r\n",path.size());
-        #endif
-        
-        return 1;
-    }
-    else
-    {
-        for(i=0;i<openList.size();i++)
-            delete openList[i];
-
-        for(i=0;i<closeList.size();i++)
-            delete closeList[i];
-        
-        path.clear();
-        
-        return 2;
-    }
-}
-
-
-
-

--- a/Map/Map.h	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,49 +0,0 @@
-#ifndef MAP_H
-#define MAP_H
-
-#include "defines.h"
-
-#include "Obstacles/Obstacle.h"
-#include "Point.h"
-#include <vector>
-
-#define DIAG_COST 0.7071067/2
-#define NDIAG_COST 0.5/2
-
-#define LOG_LEVEL 2 //4 debug(very slow) - 3 errors/warnings/infos - 2 errors/warnings - 1 errors - 0 none
-#define LOG_ASTAR 1
-#define LOG_TENDEUR 1
-
-
-
-class SimplePoint
-{
-    public:
-        SimplePoint(float x, float y) : x(x),y(y) {}
-        SimplePoint(const SimplePoint &p) : x(p.x),y(p.y) {}
-        bool operator!=(SimplePoint& p) {return x!=p.x||y!=p.y;}
-        virtual ~SimplePoint() {}
-        float x,y;
-};
-
-class Map
-{
-    public:
-        Map();
-        ~Map();
-        void build();
-        
-        int getHeight(float x, float y);
-        
-        // mpc : metre par case, par defaut chaque case fait 10cm
-        // Position en mm !!
-        char AStar(float x, float y, float goal_x, float goal_y, float mpc=100);
-        
-        
-        std::vector<SimplePoint> path;
-        std::vector<Obstacle*> obstacles;
-    private:
-};
-
-
-#endif

--- a/Map/Obstacles/Obs_circle.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,30 +0,0 @@
-#include "Obs_circle.h"
-
-Obs_circle::Obs_circle(float robotRadius, int id, float x, float y, float size):Obstacle(robotRadius,id)
-{
-    this->x = x;
-    this->y = y;
-    this->size2 = size*size;
-}
-
-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
-    {
-        return 32000; // Interdit
-    }
-    else if(bigShape && d <= size2+robotRadius*robotRadius) // On est dans le grand cercle
-    {
-        if(!smoothBigShape)
-            return 32000; // Interdit
-        else
-        {
-            return 32000; // Interdit
-        }
-    }
-    return 0;
-}

--- a/Map/Obstacles/Obs_circle.h	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#ifndef OBS_CIRCLE_H_
-#define OBS_CIRCLE_H_
-
-#include "Obstacle.h"
-
-class Obs_circle: public Obstacle
-{
-    public:
-        /** Definit un obstacle de la forme d'un cercle 
-          * @param */
-        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é
-};
-
-#endif

--- a/Map/Obstacles/Obs_rect.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,26 +0,0 @@
-#include "Obs_rect.h"
-
-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;
-    this->y1 = (y1<y2) ? y1:y2;
-    this->y2 = (y1<y2) ? y2:y1;
-}
-
-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)
-            return 32000; // Dans le grand rctangle
-    }
-    else
-    {
-        if(x1 < x && x < x2 && y1 < y && y < y2)
-            return 32000; // Dans le petit rectangle
-    }
-    return 0;
-}

--- a/Map/Obstacles/Obs_rect.h	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,16 +0,0 @@
-#ifndef OBS_RECT_H_
-#define OBS_RECT_H_
-
-#include "Obstacle.h"
-
-class Obs_rect: public Obstacle
-{
-    public:
-        Obs_rect(float robotRadius, int id, float x1, float y1, float x2, float y2);
-        virtual int height(float x, float y);
-        
-    private:
-        float x1,y1,x2,y2;
-};
-
-#endif
\ No newline at end of file

--- a/Map/Obstacles/Obstacle.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,16 +0,0 @@
-#include "Obstacle.h"
-
-Obstacle::Obstacle(float robotRadius, int id)
-{
-    this->robotRadius = robotRadius;
-    this->id = id;
-    this->active = true;
-    bigShape = true;
-    smoothBigShape = false;
-}
-
-Obstacle::~Obstacle()
-{
-    
-}
-

--- a/Map/Obstacles/Obstacle.h	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef OBSTACLE_H_
-#define OBSTACLE_H_
-
-class Obstacle
-{
-    public:
-        Obstacle(float robotRadius,int id);
-        virtual ~Obstacle();
-        
-        virtual int height(float x, float y) = 0;
-        
-        void setBigShape(bool bs) {bigShape = bs;}
-        bool isBigShape() {return bigShape;}
-        
-        void setSmoothBigShape(bool sbs) {smoothBigShape = sbs;}
-        bool isSmoothBigShape() {return smoothBigShape;}
-        
-        void setRobotRadius(float robotRadius) {this->robotRadius = robotRadius;}
-        float getRobotRadius() {return robotRadius;}
-        
-        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;
-};
-
-#endif

--- a/Map/Point.h	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,78 +0,0 @@
-#ifndef POINT_H
-#define POINT_H
-
-#include <vector>
-
-class Point;
-
-class Point
-{
-    public:
-    Point()
-    {
-        x=y=G=H=0;
-    }
-    
-    Point(int x, int y, float G=0, float H=0) : x(x),y(y),G(G),H(H)
-    {
-        
-    }
-    
-    virtual ~Point()
-    {
-        
-    }
-    
-    Point operator=(const Point &acase)
-    {
-        x=acase.x;
-        y=acase.y;
-        G=acase.G;
-        H=acase.H;
-        p=acase.p;
-        return *this;
-    }
-    
-    void setx(int xx) { x=xx; }
-    void sety(int yy) { y=yy; }
-    void setG(float GG) { G=GG; }
-    void setH(float HH) { H=HH; }
-    void setParent(Point *pp) { p=pp; }
-    void setParent() { p=0; }
-    
-    int getx() { return x; }
-    int gety() { return y; }
-    float getF() { return G+H; }
-    float getG() { return G; }
-    float getH() { return H; }
-    Point* getParent() { return p; }
-    
-    
-    bool in(std::vector<Point*> &list, unsigned int &pos)
-    {
-        for(unsigned int i=0;i<list.size();i++)
-            if(list[i]->getx() == this->getx() && list[i]->gety() == this->gety())
-            {
-                pos = i;
-                return true;
-            }
-        return false;
-    }
-    
-    bool in(std::vector<Point*> &list)
-    {
-        for(unsigned int i=0;i<list.size();i++)
-            if(list[i]->getx() == this->getx() && list[i]->gety() == this->gety())
-                return true;
-        return false;
-    }
-    
-    private:
-    int x;
-    int y;
-    float G;
-    float H;
-    Point *p;
-};
-
-#endif // POINT_H

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/controle.h	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,17 @@
+#ifndef CONTROLE_H
+#define CONTROLE_H
+
+/* Points du rectangle :
+    0       1
+
+    2       3
+
+    Penser à supprimer MINDISTROBOT et le mettre dans la taille de l'obstacle (on augmentera dxtaille et dytaille de façon adaptée)
+    Autre methode d'amelioration : dans la boucle de test de croisement des obstacles, s'il n'y a pas de croisement, mettre ended=true, ca évitera de faire les tests pour tous les points disponibles !
+*/
+
+#define NULL 0
+#define BATARD 0xCCCCCCCC
+#define MINDISTROBOT 1
+
+#endif

--- a/Map/defines.h	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,116 +0,0 @@
-#ifndef DEFINE_H
-#define DEFINE_H
-
-#define PLAN_B
-//#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 150
-
-#define MAXPOINT 8000
-
-// ----- Loggeur ----- //
-
-#ifdef OUT_USB
-    #define OUT_TX USBTX
-    #define OUT_RX USBRX
-#else
-    #define OUT_TX PA_11
-    #define OUT_RX PA_12
-#endif
-
-// ----- Moteurs ----- //
-
-#define PWM_MOT1 PB_13
-#define PWM_MOT2 PB_14
-#define PWM_MOT3 PB_15
-
-#define DIR_MOT1 PC_9
-#define DIR_MOT2 PB_8
-#define DIR_MOT3 PB_9
-
-// ----- Odometrie ----- //
-
-#define ODO_G_B PA_10
-#define ODO_G_A PB_3
-
-#define ODO_D_B PB_5
-#define ODO_D_A PB_4
-
-// ----- Boutons ----- //
-
-#define LED_DESSUS PH_1
-#define BP_DESSUS PC_8
-#define TIRETTE_DESSUS PC_6
-#define COULEUR_DESSUS PC_5
-
-#define COULEUR_JAUNE 0
-#define COULEUR_VERTE 1
-
-// ----- AX12 ----- //
-
-#define AX12_TX PA_9
-#define AX12_RX NC
-
-#define MAX_TORQUE 300
-
-#define BRASG_OUVERT 60
-#define BRASG_FERME 155
-#define BRASD_OUVERT 240
-#define BRASD_FERME 145
-
-#define PINCE_OUVERTE 100
-#define PINCE_FERMEE 3
-
-// ----- Sharp ----- //
-
-#define SHARP_D A4
-#define SHARP_DG A3
-#define SHARP_DD A5
-#define SHARP_AG A2
-#define SHARP_AD A1
-
-#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/figure.h	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,41 @@
+#ifndef FIGURE_H
+#define FIGURE_H
+
+#include "point.h"
+
+class figure {
+public:
+    figure(float xc, float yc) {
+        xcentre = xc;
+        ycentre = yc;
+    }
+
+    float getX () { return xcentre; }
+    float getY () { return ycentre; }
+
+    bool CroisementSegment (point A1, point A2, point B1, point B2) {
+        if (ProdVect (A1, A2, B1, B2) != 0) { // On verifie que les droites ne sont pas parallèles
+            if (ProdVect (A1, A2, A1, B2)*ProdVect (A1, A2, A1, B1) <= 0
+                && ProdVect (B1, B2, B1, A1)*ProdVect (B1, B2, B1, A2) <= 0) {
+                return true;
+            }
+            else {
+                return false;
+            }
+        }
+        else {
+            return false;
+        }
+    }
+
+protected:
+    float xcentre, ycentre;
+
+    float ProdVect (float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+        return ((x2 - x1)*(y4 - y3) - (y2 - y1)*(x4 - x3));
+    }
+    float ProdVect (point A1, point A2, point B1, point B2) {
+        return ProdVect (A1.getX(), A1.getY (), A2.getX (), A2.getY (), B1.getX (), B1.getY (), B2.getX (), B2.getY ());
+    }
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/map.cpp	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,159 @@
+#include "map.h"
+
+map::map (Odometry* nodo) : Codo(nodo) {
+}
+
+void map::addObs (obsCarr nobs) {
+    obs.push_back (nobs);
+}
+
+void map::FindWay (float depX, float depY, float arrX, float arrY) {
+    point depart(depX, depY);
+    point arrivee(arrX, arrY);
+    FindWay(depart, arrivee);
+}
+
+void map::FindWay (point dep, point arr) {
+    //logger.printf("On a cherche un chemin\n\r");
+    nVector<pointParcours> open;
+    nVector<pointParcours> close;
+    points4 tmp;
+    bool val[4] = {true,true,true,true};
+    int os = obs.size ();
+    int i, j;
+    bool ended=false;   // On teste tous les points ajoutes dans l'open list pour savoir s'il y a intersection avec un obstacle. Ended passe à true quand aucun ne coupe un obstacle.
+    endedParc = false;
+
+    path.clear();
+    
+    pointParcours depP (dep, NULL, arr);
+    int indTMP1=0;  // Le point actuel
+    int PointEnding = 0;    
+    open.push_back (depP);
+
+    while (!ended && !open.empty ()) {
+        for (i = 0; i < open.size (); ++i) {
+            if (open[i].getP2 () < open[indTMP1].getP2 ())
+                indTMP1 = i;
+        }
+
+        close.push_first (open[indTMP1]);
+        open.erase (indTMP1);
+        indTMP1 = 0;
+
+        ended = true;
+        for (i = 0; i < os; ++i) {
+            if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
+                ended = false;
+                tmp = obs[i].getPoints ();
+
+                // On vérifie si le point croise un obstacle
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
+                        val[0] = false;
+                // On vérifie si le point existe déjà dans la liste ouverte
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p0)
+                        val[0] = false;
+                }
+                // On vérifie si le point existe déjà dans la liste fermée
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p0)
+                        val[0] = false;
+                }
+                if (val[0]) {
+                    open.push_back (pointParcours (tmp.p0, &close[indTMP1], arr));
+                }
+
+                // On repete l'operation pour le second point
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
+                        val[1] = false;
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p1)
+                        val[1] = false;
+                }
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p1)
+                        val[1] = false;
+                }
+                if (val[1]) {
+                    open.push_back (pointParcours (tmp.p1, &close[indTMP1], arr));
+                }
+
+                // On répète l'opération pour le troisième point
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
+                        val[2] = false;
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p2)
+                        val[2] = false;
+                }
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p2)
+                        val[2] = false;
+                }
+                if (val[2]) {
+                    open.push_back (pointParcours (tmp.p2, &close[indTMP1], arr));
+                }
+
+                // On répète l'opération pour le quatrieme point
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
+                        val[3] = false;
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p3)
+                        val[3] = false;
+                }
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p3)
+                        val[3] = false;
+                }
+                if (val[3]) {
+                    open.push_back (pointParcours (tmp.p3, &close[indTMP1], arr));
+                }
+
+                val[0] = true;
+                val[1] = true;
+                val[2] = true;
+                val[3] = true;
+            }
+        }
+    }
+
+    /* L'algorithme n'est pas bon. Je devrais prendre ici le plus court chemin vers l'arrivée pour ceux qui ne sont pas bloqués, et pas un aléatoire ... */
+    if (ended) {
+        pointParcours* pente;
+        pente = &close[0];
+        while (pente != NULL) {
+            path.push_first (*pente);
+            pente = pente->getPere ();
+        }
+        path.push_back (pointParcours(arr, NULL, arr));
+        path.erase(0);
+        endedParc = true;
+        /*
+        if (path.size() > 1)
+            path.erase(0);*/
+    }
+}
+
+void map::Execute(float XObjectif, float YObjectif) {
+    logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
+    FindWay (Codo->getX(), Codo->getY(), XObjectif, YObjectif);
+    
+    if (endedParc) {
+        //logger.printf("\n\r");
+        for (int i = 0; i < path.size (); i++) {
+            logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
+            //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
+            Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
+            logger.printf("Goto Fini\n\r");
+        }
+        //logger.printf("Chemin fini !\n\r");
+    }
+    else {
+        logger.printf("Chemin pas trouve ...\n\r");
+    }
+    endedParc = false;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/map.h	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,27 @@
+#ifndef MAP_H
+#define MAP_H
+
+#include "obsCarr.h"
+#include "pointParcours.h"
+#include "nVector.h"
+#include "controle.h"
+#include "Odometry.h"
+
+class map {
+public:
+    map (Odometry* nodo);
+    void addObs (obsCarr nobs);
+    void FindWay (point dep, point arr);
+    void FindWay (float depX, float depY, float arrX, float arrY);
+    void Execute (float XObjectif, float YObjectif);
+    nVector<pointParcours>& getParc () { return path; }
+    bool& getEnded () { return endedParc; }
+
+protected:
+    nVector<obsCarr> obs;
+    nVector<pointParcours> path;
+    bool endedParc; // Definit s'il existe un chemin parcourable dans le dernier FindWay
+    Odometry* Codo;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/nVector.h	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,168 @@
+#ifndef VECTOR_H
+#define VECTOR_H
+
+template<class T>
+class elem {
+public:
+    elem () { ; }
+    elem (T nval) : val (nval) {  next = NULL; prev = NULL; }
+    elem (T nval, elem* nprev) : val (nval) { next = NULL; prev = nprev; }
+
+    elem<T> * gNext () { return next; }
+    elem<T> * gprev () { return prev; }
+    T& getVal () { return val; }
+
+    void sNext (elem<T> * nnext) { next = nnext; }
+    void sPrev (elem<T> * nprev) { prev = nprev; }
+
+private:
+    T val;
+    elem<T> * next;
+    elem<T> * prev;
+};
+
+template<class T>
+class nVector {
+public:
+    nVector () {
+        first = NULL;
+        curr = NULL;
+        init = false;
+        VectorSize = 0;
+    }
+
+    void push_back (T val) {
+        if (init) {
+            setCurrLast ();
+            elem<T> * n = new elem<T> (val, curr);
+            curr->sNext (n);
+            n->sPrev (curr);
+            VectorSize++;
+        }
+        else {
+            init = true;
+            first = new elem<T> (val);
+            VectorSize = 1;
+        }
+    }
+
+    void push_first (T val) {
+        if (init) {
+            curr = first;
+            elem<T> * n = new elem<T> (val);
+            curr->sPrev (n);
+            n->sNext (curr);
+            first = n;
+            VectorSize++;
+        }
+        else {
+            init = true;
+            first = new elem<T> (val);
+            VectorSize = 1;
+        }
+    }
+
+    bool erase (int index) {
+        bool worked = true;
+
+        if (init) {
+            if (index > 0 && index < VectorSize) {
+                if (setCurrIndex (index)) {
+                    elem<T>* p = curr->gprev ();
+                    if (p != NULL)
+                        p->sNext (curr->gNext ());
+
+                    elem<T>* n = curr->gNext ();
+                    if (n != NULL)
+                        n->sPrev (p);
+
+                    delete curr;
+
+                    VectorSize--;
+                    if (VectorSize <= 0) {
+                        init = false;
+                    }
+                }
+            }
+            else if (index == 0 && index < VectorSize) {
+                elem<T> * n = first->gNext ();
+
+                if (n != NULL) {
+                    n->sPrev (NULL);
+                    delete first;
+                    first = n;
+                    VectorSize--;
+                }
+                else {
+                    init = false;
+                    delete first;
+                    VectorSize = 0;
+                }
+            }
+            else {
+                worked = false;
+            }
+        }
+        else {
+            worked = false;
+        }
+        return worked;
+    }
+
+    void concatenate (nVector<T>& t) {
+        for (int i = 0; i < t.size (); ++i)
+            push_back (t[i]);
+    }
+
+    int size () { return VectorSize; }
+    bool empty () { return !init; }
+
+    T& operator[](int ind) {
+        setCurrIndex (ind);
+        return curr->getVal ();
+    }
+
+    void show () {
+        for (int i = 0; i < VectorSize; ++i) {
+            setCurrIndex (i);
+            std::cout << "(" << curr << ") Element " << i + 1 << " pere : " << curr->gprev() << ", fils " << curr->gNext () << std::endl;
+        }
+    }
+    
+    void clear () {
+        while (VectorSize != 0) {
+            erase (0);
+        }
+    }
+private:
+    void setCurrLast () {
+        if (init) {
+            curr = first;
+            while (curr->gNext () != NULL) {
+                curr = curr->gNext ();
+            }
+        }
+    }
+
+    bool setCurrIndex (int index) {
+        curr = first;
+        bool worked = true;
+
+        for (int i = 0; i < index; ++i) {
+            if (curr->gNext () != NULL)
+                curr = curr->gNext ();
+            else {
+                worked = false;
+                break;
+            }
+        }
+        return worked;
+    }
+
+    elem<T>* first;
+    elem<T>* curr;
+
+    bool init;
+    int VectorSize;
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/obsCarr.cpp	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,31 @@
+#include "obsCarr.h"
+
+bool obsCarr::getCroisement (point A, point B) {
+    point p0 (xcentre - dxtaille, ycentre + dytaille);
+    point p1 (xcentre + dxtaille, ycentre + dytaille);
+    point p2 (xcentre - dxtaille, ycentre - dytaille);
+    point p3 (xcentre + dxtaille, ycentre - dytaille);
+
+    if (CroisementSegment (p0, p3, A, B) || CroisementSegment (p1, p2, A, B) || belongs(A) || belongs(B))
+        return true;
+    else
+        return false;
+}
+
+bool obsCarr::getCroisement (float X, float Y, point B) {
+    point A (X, Y);
+    return getCroisement (A, B);
+}
+
+points4 obsCarr::getPoints () {
+    return {
+        point (xcentre - (dxtaille + MINDISTROBOT), ycentre + (dytaille + MINDISTROBOT)),
+        point (xcentre + (dxtaille + MINDISTROBOT), ycentre + (dytaille + MINDISTROBOT)),
+        point (xcentre - (dxtaille + MINDISTROBOT), ycentre - (dytaille + MINDISTROBOT)),
+        point (xcentre + (dxtaille + MINDISTROBOT), ycentre - (dytaille + MINDISTROBOT)),
+    };
+}
+
+bool obsCarr::belongs (point& A) {
+    return (A.getX () <= xcentre + dxtaille && A.getX () >= xcentre - dxtaille && A.getY () <= ycentre + dytaille && A.getY () >= ycentre - dytaille);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/obsCarr.h	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,31 @@
+#ifndef OBSCARR_H
+#define OBSCARR_H
+
+#include "controle.h"
+#include "figure.h"
+#include "point.h"
+
+class obsCarr : public figure {
+public:
+    obsCarr (float xc, float yc, float dxt, float dyt) : figure (xc,yc) {
+        dxtaille = dxt;
+        dytaille = dyt;
+    }
+
+    /* Retourne true si le segment AB croise le rectangle */
+    bool getCroisement (point A, point B);
+    /* Retourne true si le segment AB croise le rectangle */
+    bool getCroisement (float X, float Y, point B);
+
+    /* Retourne 4 Points pas tres loin du rectangle par lesquels peut passer le robot ! */
+    points4 getPoints ();
+    bool belongs (point& A);
+
+    float getDXT () { return dxtaille; }
+    float getDYT () { return dytaille; }
+
+protected:
+    float dxtaille, dytaille;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/point.h	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,44 @@
+#ifndef POINT_H
+#define POINT_H
+
+typedef struct P4 points4;
+
+class point {
+public:
+    point (float nx, float ny) {
+        x = nx;
+        y = ny;
+    }
+    point () { ; }
+
+    float getX () { return x; }
+    float getY () { return y; }
+    void setX(float nx) { x = nx; }
+    void setY(float ny) { y = ny; }
+    
+    float operator*(point& a) {
+        return calculDistance2 (x, y, a.getX(), a.getY ());
+    }
+    bool operator==(point& a) {
+        return (x == a.getX () && y == a.getY ());
+    }
+    bool operator!=(point& a) {
+        return !(*this == a);
+    }
+
+protected:
+    float calculDistance2(float x1, float y1, float x2, float y2) {
+        return ((x1-x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
+    }
+
+    float x, y;
+};
+
+struct P4 {
+    point p0;
+    point p1;
+    point p2;
+    point p3;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/pointParcours.h	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,58 @@
+#ifndef POINTPARCOURS_H
+#define POINTPARCOURS_H
+#include "math.h"
+
+#include "controle.h"
+#include "point.h"
+
+typedef struct PP4 PointsParc4;
+
+class pointParcours : public point {
+public:
+    pointParcours (float nx, float ny, pointParcours * npere, point arr) : point (nx, ny) {
+        if (npere != NULL)
+            G2 = sqrt(npere->getG2() + calculDistance2 (nx, ny, npere->getX(), npere->getY ()));
+        else
+            G2 = 0;
+        pere = npere;
+        H2 = sqrt (calculDistance2 (nx, ny, arr.getX (), arr.getY ()));
+    }
+
+    pointParcours (point p, pointParcours * npere, point arr) : point (p) {
+        if (pere != NULL)
+            G2 = npere->getG2 () + calculDistance2 (p.getX(), p.getY(), npere->getX (), npere->getY ());
+        else
+            G2 = 0;
+        pere = npere;
+
+        H2 = sqrt(calculDistance2 (p.getX (), p.getY (), arr.getX (), arr.getY ()));
+    }
+
+    pointParcours * getPere () { return pere; }
+
+    long double getG2 () { return G2; }
+    long double getH2 () { return H2; }
+    long double getP2 () { return G2 + H2; }
+
+    bool operator==(pointParcours& a) {
+        // Autre version : return (x == a.getX () && y == a.getY () && a.getP2 () == G2 + H2);
+        return (x == a.getX () && y == a.getY ());
+    }
+
+    bool operator==(point& a) {
+        return (x == a.getX () && y == a.getY ());
+    }
+
+protected:
+    pointParcours * pere;
+    long double G2, H2;
+};
+
+struct PP4 {
+    pointParcours p0;
+    pointParcours p1;
+    pointParcours p2;
+    pointParcours p3;
+};
+
+#endif
\ No newline at end of file

--- a/Odometry/Odometry.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/Odometry/Odometry.cpp	Wed Apr 20 13:13:37 2016 +0000
@@ -1,5 +1,6 @@
 #include "Odometry.h"
 
+// GotoThet : settheta enleve.
 // M1 = Moteur droit, M2 = Moteur gauche
 
 Odometry::Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc) : roboclaw(rc)
@@ -108,7 +109,7 @@
 
 void Odometry::GotoThet(double theta_)
 {
-    led = 0;
+    S::led = 0;
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -142,17 +143,17 @@
 
     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
 
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("%6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)) logger.printf("[Theta] %6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
     wait(0.4);
-    setTheta(theta_);
-    led = 1;
+    //setTheta(theta_);
+    S::led = 1;
     //arrived = true;
-    //logger.printf("arrivey %d\n\r",pos_prog);
+    logger.printf("[End GotoThet] X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
 }
 
 void Odometry::GotoDist(double distance)
 {
-    led = 0;
+    S::led = 0;
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
@@ -166,11 +167,11 @@
 
     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
     
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); logger.printf("[Dist] PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
     wait(0.4);
-    led = 1;
+    S::led = 1;
     //logger.printf("arrivey %d\n\r",pos_prog);
-    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
+    logger.printf("[End GotoDist] X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
 }
 
 void Odometry::TestEntraxe(int i) {
@@ -204,7 +205,7 @@
 
     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
     
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)) logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
     wait(0.4);
-    led = 1;
+    S::led = 1;
 }
\ No newline at end of file

--- a/main.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/main.cpp	Wed Apr 20 13:13:37 2016 +0000
@@ -1,45 +1,66 @@
 #include "func.h"
+#include "Map/map.h"
 
 #define ATTENTE 0
 #define GO 1
 #define STOP 2
 
 /* Déclaration des différents éléments de l'IHM */
-InterruptIn mybutton(USER_BUTTON);
-DigitalIn button(USER_BUTTON);
-DigitalOut led(LED1);
-DigitalOut bleu(PC_5);
-DigitalOut blanc(PC_6);
-DigitalOut rouge(PC_8);
-
-/* AX12 */
-AX12 left_hand(PA_15, PB_7, 4, 250000);
-AX12 right_hand(PA_15, PB_7, 1, 250000);
-
-/* Sharp */
-AnalogIn capt1(PA_4);
-AnalogIn capt2(PB_0);
-AnalogIn capt3(PC_1);
-AnalogIn capt4(PC_0);
-
-/* Moteurs pas à pas */
-Stepper RMot(NC, PB_10, PA_8);
-Stepper ZMot(NC, PB_5, PB_4);
-Stepper LMot(NC, PB_3, PA_10);
-/* Fins de course */
-InterruptIn EndR(PB_15);
-InterruptIn EndZ(PB_14);
-InterruptIn EndL(PB_13);
-
-Ticker ticker;
-
-Serial logger(USBTX, USBRX);
-//Serial logger(PA_9, PA_10);
-RoboClaw roboclaw(460800, PA_11, PA_12);
-Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
-
-int i = 0, state = 0;
-bool EL = false, EZ = false, ER = false;
+class S { 
+public:
+    S () :  mybutton(USER_BUTTON), button(USER_BUTTON), led(LED1), bleu(PC_5), blanc(PC_6), rouge(PC_8)
+            left_hand(PA_15, PB_7, 4, 250000), right_hand(PA_15, PB_7, 1, 250000),
+            capt1(PA_4), capt2(PB_0), capt3(PC_1), capt4(PC_0),
+            RMot(NC, PB_10, PA_8), ZMot(NC, PB_5, PB_4), LMot(NC, PB_3, PA_10),
+            EndR(PB_15), EndZ(PB_14), EndL(PB_13),
+            logger(PA_9, PA_10), roboclaw(460800, PA_11, PA_12), odo(61.7, 61.8, ENTRAXE, 4096, roboclaw) {
+        
+        i = 0;
+        state = 0;
+        EL = false;
+        EZ = false
+        ER = false;            
+    }
+            
+    static InterruptIn mybutton;
+    static DigitalIn button;
+    static DigitalOut led;
+    static DigitalOut bleu;
+    static DigitalOut blanc;
+    static DigitalOut rouge;
+    
+    /* AX12 */
+    static AX12 left_hand;
+    static AX12 right_hand;
+    
+    /* Sharp */
+    static AnalogIn capt1;
+    static AnalogIn capt2;
+    static AnalogIn capt3;
+    static AnalogIn capt4;
+    
+    /* Moteurs pas à pas */
+    static Stepper RMot;
+    static Stepper ZMot;
+    static Stepper LMot;
+    /* Fins de course */
+    static InterruptIn EndR;
+    static InterruptIn EndZ;
+    static InterruptIn EndL;
+    
+    static Ticker ticker;
+    
+    //Serial logger(USBTX, USBRX);
+    static Serial logger;
+    static RoboClaw roboclaw;
+    static Odometry odo;
+    
+    static int i;
+    static state;
+    static bool EL;
+    static EZ;
+    static ER;
+}    
 
 void init(void);
 
@@ -47,6 +68,20 @@
 int main(void)
 {
     init();
+    
+    odo.setPos(110, 1000, 0);
+    map m(&odo);
+    m.addObs(obsCarr (1250, 1000, 220, 220));
+    m.addObs(obsCarr (1500, 750, 220, 220));
+    m.addObs(obsCarr (1500, 1250, 220, 220));
+    
+    m.Execute(1000,1000);
+    m.Execute(1500,1000);
+    m.Execute(1500,1500);
+    m.Execute(110,1000);
+    
+    odo.GotoThet(0);
+    
     while(1) JPO();
 }
 
@@ -61,7 +96,7 @@
     wait_ms(1000);
     bleu = 0, blanc = 0, rouge = 0;
     
-    while(button);
+    while(S::button);
     wait(1);
     
     init_ax12();