Revision 40:ca4dd3faffa8, committed 2016-04-13

Comitter:

IceTeam

Date:

Wed Apr 13 11:27:01 2016 +0000

Parent:

37:da3a2c781672

Commit message:

First publish;

Changed in this revision

--- a/Map/Map.cpp	Sat Feb 06 10:11:28 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,412 +0,0 @@
-#include "Map.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,30));// P16
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,600,30));// P16
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,900,800,50));// 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	Sat Feb 06 10:11:28 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	Sat Feb 06 10:11:28 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	Sat Feb 06 10:11:28 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	Sat Feb 06 10:11:28 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	Sat Feb 06 10:11:28 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	Sat Feb 06 10:11:28 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	Sat Feb 06 10:11:28 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	Sat Feb 06 10:11:28 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

--- a/Map/defines.h	Sat Feb 06 10:11:28 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,118 +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 190
-
-#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
-
-#define PI 3.14159f
-
-// ----- 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

--- a/Odometry/Odometry.cpp	Sat Feb 06 10:11:28 2016 +0000
+++ b/Odometry/Odometry.cpp	Wed Apr 13 11:27:01 2016 +0000
@@ -90,9 +90,11 @@
 void Odometry::GotoXY(double x_goal, double y_goal)
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
+    logger.printf("odo::GotoXY %f\n\r", theta_);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
     logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
+    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoDist(dist_);
 }
 
@@ -100,7 +102,7 @@
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
+    //logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
     GotoDist(dist_);
     GotoThet(theta_goal);
@@ -139,7 +141,9 @@
     //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
 
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
-
+    
+    logger.printf("On a enclenché le mouvement");
+    
     //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);

--- a/Odometry/Odometry.h	Sat Feb 06 10:11:28 2016 +0000
+++ b/Odometry/Odometry.h	Wed Apr 13 11:27:01 2016 +0000
@@ -8,13 +8,13 @@
 #define C 1.0
 
 /* Vitesse d'acceleration d'angle reduite de 8000->4000 */
-#define accel_angle 8000
-#define vitesse_angle 16000
-#define deccel_angle 8000
+#define accel_angle 2100
+#define vitesse_angle 3000
+#define deccel_angle 2100
 
-#define accel_dista 8000
-#define vitesse_dista 16000
-#define deccel_dista 8000
+#define accel_dista 9000
+#define vitesse_dista 20000
+#define deccel_dista 9000
 
 /*
 *   Author : Benjamin Bertelone, reworked by Simon Emarre

--- a/RoboClaw.lib	Sat Feb 06 10:11:28 2016 +0000
+++ b/RoboClaw.lib	Wed Apr 13 11:27:01 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ARES/code/RoboClaw/#ad00b3431ff5
+https://developer.mbed.org/teams/ARES/code/RoboClaw/#ece8a8cdf4b0

--- a/main.cpp	Sat Feb 06 10:11:28 2016 +0000
+++ b/main.cpp	Wed Apr 13 11:27:01 2016 +0000
@@ -1,5 +1,5 @@
 #include "Odometry/Odometry.h"
-#include "Map/Map.h"
+#include "map/map.h"
 
 #define dt 10000
 #define ATTENTE 0
@@ -27,33 +27,35 @@
 int main(void)
 {
     init();
-    /*double angle_v = 2*PI/5;
+    double angle_v = 2*PI/5;
     double distance_v = 200.0;
-    std::vector<SimplePoint> path;
-    Map map;
-    
+    float the;
     
-    //Construction des obstacles
-    map.build();
+    odo.setPos(0, 1000, 0);
+    point dep (odo.getX(), odo.getY());
+    point arr (1400, 1000);
     
-    float x=odo.getX();
-    float y=odo.getY();
-    float the = 0;
+    map m;
+    m.addObs(obsCarr (550, 1030, 250, 230));
+    m.addObs(obsCarr (740, 720, 240, 240));
+    m.addObs(obsCarr (1170, 1260, 220, 220));
     
-    map.AStar(x, y, 1400, 1000, 75);
-    path = map.path;
+    /*m.addObst (carre (1000, 1000, 210, 210));
+    m.addObst (carre (1300, 700, 180, 180));*/
+    m.FindWay (dep, arr);
+    logger.printf("On a cherche un chemin\n\r");
     
-    for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
-        odo.GotoXYT(path[i].x, path[i].y, the);
+    if (m.getEnded()) {
+        nVector<pointParcours> p = m.getParc ();
+        logger.printf ("Il y a %d points de parcours\n\r", p.size());
+        for (int i = 0; i < p.size (); i++) {
+            logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
+            //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
+            odo.GotoXY(p[i].getX(), p[i].getY());
+        }
     }
-    
-    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
-    path = map.path;
-    
-    for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
-        odo.GotoXYT(path[i].x, path[i].y, the);
+    else {
+        logger.printf("Chemin pas trouve ...");
     }
     
     //odo.GotoThet(PI);
@@ -62,37 +64,69 @@
     
     //odo.GotoThet(-PI/2);
     wait(2000);
+    while (1);
     //odo.GotoXYT(2250,500,0);*/
+    /*roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    int state = 0;
     while(1)
     {
-        while(logger.readable()) 
-        {
+       // while(logger.readable()) 
+        //{
             char c = logger.getc();
             if(c=='z')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, speed_angle);
+                if (state != 1) {
+                    state = 1;
+                    logger.printf("Z\r\n");
+                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+                }
             }
             else if(c == 's')
             {
-                roboclaw.ForwardM1(ADR, 0);
-                roboclaw.ForwardM2(ADR, 0);
+                if (state != 2) {
+                    state = 2;
+                    roboclaw.ForwardM1(ADR, 0);
+                    roboclaw.ForwardM2(ADR, 0);
+                }
             }
             else if(c == 'd')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, speed_angle);
+                if (state != 3) {
+                    state = 3;
+                    logger.printf("D\r\n");
+                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+                    roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+                }
             }
             else if(c == 'q')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, -speed_angle);
+                if (state != 4) {
+                    state = 4;
+                    logger.printf("Q\r\n");
+                    roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
+                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+                }
             }
             else if(c == 'x')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, -speed_angle);
+                if (state != 5) {
+                    state = 5;
+                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
+                }
             }
-        }
-        roboclaw.ForwardM1(ADR, 0);
-        roboclaw.ForwardM2(ADR, 0);
-    }
+            else if (c == '\0') { ; }
+            else {
+                if (state != 0) {
+                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
+                    state = 0;
+                }
+            }
+       // }
+       // roboclaw.ForwardM1(ADR, 0);
+       // roboclaw.ForwardM2(ADR, 0);
+    }*/
 }
 
 void init(void)
@@ -108,9 +142,10 @@
     while(button);
     wait(1);
     mybutton.fall(&pressed);
-    mybutton.rise(&unpr
-    essed);
+    mybutton.rise(&unpressed);
     ticker.attach_us(&update_main,dt); // 100 Hz
+    
+    logger.printf("End init\n\r");
 }
 
 void update_main(void)

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/map/controle.h	Wed Apr 13 11:27:01 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/map/figure.h	Wed Apr 13 11:27:01 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/map/map.cpp	Wed Apr 13 11:27:01 2016 +0000
@@ -0,0 +1,177 @@
+#include "map.h"
+
+map::map () {
+}
+
+void map::addObs (obsCarr nobs) {
+    obs.push_back (nobs);
+}
+
+void map::FindWay (point dep, point arr) {
+    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;
+
+    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;
+
+                /*
+                if (val[0]) {       // Si le point est accessible et n'existe pas deja
+                    open.push_back (pointParcours (tmp.p0, &close[indTMP1], &arr)); // On le rajoute dans la liste ouverte
+                    ended = true;   // On suppose que le parcours s'arrete ici
+                    for (int a = 0; a < os && ended; ++a)
+                        if (obs[a].getCroisement (tmp.p0, arr)) {   // Si on croise un obstacle, le parcours ne s'arrete pas !
+                            ended = false;
+                            std::cout << "Croisement de " << tmp.p0.getX() << " - " << tmp.p0.getY() << " l'obstacle " << a << std::endl;
+                        }
+                    if (ended)
+                        PointEnding = open.size ();
+                }
+                if (val[1] && !ended) {
+                    ended = true;
+                    open.push_back (pointParcours (tmp.p1, &close[indTMP1], &arr));
+                    for (int a = 0; a < os && ended; ++a)
+                        if (obs[a].getCroisement (tmp.p1, arr)) {   
+                            ended = false;
+                            std::cout << "Croisement de " << tmp.p1.getX () << " - " << tmp.p1.getY () << " l'obstacle " << a << std::endl;
+                        }
+                    if (ended)
+                        PointEnding = open.size ();
+                }
+                if (val[2] && !ended) {
+                    ended = true;
+                    open.push_back (pointParcours (tmp.p2, &close[indTMP1], &arr));
+                    for (int a = 0; a < os && ended; ++a)
+                        if (obs[a].getCroisement (tmp.p2, arr)) {
+                            ended = false;
+                            std::cout << "Croisement de " << tmp.p2.getX () << " - " << tmp.p2.getY () << " l'obstacle " << a << std::endl;
+                        }
+                    if (ended)
+                        PointEnding = open.size ();
+                }
+                if (val[3] && !ended) {
+                    ended = true;
+                    open.push_back (pointParcours (tmp.p3, &close[indTMP1], &arr));
+                    for (int a = 0; a < os && ended; ++a)
+                        if (obs[a].getCroisement (tmp.p3, arr)) {
+                            ended = false;
+                            std::cout << "Croisement de " << tmp.p3.getX () << " - " << tmp.p3.getY () << " l'obstacle " << a << std::endl;
+                        }
+                    if (ended)
+                        PointEnding = open.size ();
+                }
+
+                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, &path[path.size()-1], &arr));
+        endedParc = true;
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/map/map.h	Wed Apr 13 11:27:01 2016 +0000
@@ -0,0 +1,23 @@
+#ifndef MAP_H
+#define MAP_H
+
+#include "obsCarr.h"
+#include "pointParcours.h"
+#include "nVector.h"
+#include "controle.h"
+
+class map {
+public:
+    map ();
+    void addObs (obsCarr nobs);
+    void FindWay (point dep, point arr);
+    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
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/map/nVector.h	Wed Apr 13 11:27:01 2016 +0000
@@ -0,0 +1,162 @@
+#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;
+        }
+    }
+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 13 11:27:01 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 13 11:27:01 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 13 11:27:01 2016 +0000
@@ -0,0 +1,42 @@
+#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; }
+
+    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 13 11:27:01 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