Revision 20:30942f018252, committed 2016-01-07

Comitter:

IceTeam

Date:

Thu Jan 07 18:22:38 2016 +0000

Parent:

18:0f1fefe78266

Commit message:

Une version toute pourrie, pleine de bug. Toute les verification d'accessibilit? avec getHeight ont ete mise en commentaire, il faudra les decommenter;

Changed in this revision

--- a/Asserv_Plan_B/Scratch.h	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,20 +0,0 @@
-/*float erreur_distance_g = distance_g-(m_odometry.getDistLeft()-memo_g); //- distance parcourue par la roue gauche depuis l'état 2
-float erreur_distance_d = distance_d-(m_odometry.getDistRight()-memo_d);
-cmd_g = erreur_distance_g*Kp_distance;
-cmd_d = erreur_distance_d*Kp_distance;
-m_motorL.setSpeed(cmd_g);
-m_motorR.setSpeed(cmd_d);*/
-
-/*void aserv_planB::control_speed()
-{
-    vitesse_d = m_odometry.getVitRight();
-    vitesse_g = m_odometry.getVitLeft();
-
-    erreur_g = consigne_g - vitesse_g;
-    cmd_g = erreur_g*Kp;
-    erreur_d = consigne_d - vitesse_d;
-    cmd_d = erreur_d*Kp;
-
-    m_motorL.setSpeed(cmd_g);
-    m_motorR.setSpeed(cmd_d);
-}*/
\ No newline at end of file

--- a/Asserv_Plan_B/planB.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,199 +0,0 @@
-#include "planB.h"
-#include "defines.h"
-
-extern Serial logger;
-
-aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
-{
-    limite = 0.55;
-    lim_max = 0.30;
-    lim_min = 0.1995;
-    cmd = 0;
-    cmd_g = 0;
-    cmd_d = 0;
-    
-    somme_erreur_theta = 0;
-    delta_erreur_theta = 0;
-    erreur_precedente_theta = 0;
-    
-    somme_erreur_distance = 0;
-    delta_erreur_distance = 0;
-    erreur_precedente_distance = 0;
-    distanceGoal = 0;
-    distance = 0;
-    
-    Kp_angle = 3.5; //Fixed à 3.0 pour 180 deg
-    Ki_angle = 0.0;
-    Kd_angle = 0.2;
-    
-    Kp_distance = 0.0041;
-    Ki_distance = 0.0001;//0.000001
-    Kd_distance = 0.01;//0.05
-    
-    N = 0;
-    arrived = false;
-    squip = false;
-    state = 0; // Etat ou l'on ne fait rien
-}
-
-void aserv_planB::setGoal(float x, float y, float phi)
-{
-    arrived = false;
-    m_goalX = x;
-    m_goalY = y;
-    m_goalPhi = phi;
-    distanceGoal = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
-    state = 1; // Etat de rotation 1
-    Kd_distance = 0.01;
-    N = 0;
-}
-
-void aserv_planB::stop(void)
-{
-    m_motorL.setSpeed(0);
-    m_motorR.setSpeed(0);
-    state = 0;
-}
-
-void aserv_planB::setGoal(float x, float y)
-{
-    squip = true;
-    setGoal(x, y, 0);
-}
-
-void aserv_planB::update(float dt)
-{
-    thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
-    float erreur_theta = thetaGoal-m_odometry.getTheta();
-    
-    float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
-    
-    delta_erreur_theta = erreur_theta - erreur_precedente_theta;
-    erreur_precedente_theta = erreur_theta;
-    somme_erreur_theta += erreur_theta;
-    
-    delta_erreur_distance = erreur_distance - erreur_precedente_distance;
-    erreur_precedente_distance = erreur_distance;
-    somme_erreur_distance += erreur_distance;
-    
-    if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
-    if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
-
-    // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
-    if(state == 1)
-    {
-        //logger.printf("%.2f\r\n", erreur_theta*180/PI);
-        cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
-        
-        if(cmd > limite) cmd = limite;
-        else if(cmd < -limite) cmd = -limite;
-        
-        m_motorL.setSpeed(-cmd);
-        m_motorR.setSpeed(cmd);
-        
-        if(abs(erreur_theta) < 0.05f) N++;
-        else N = 0;
-        if(N > 5)
-        {
-            m_motorL.setSpeed(0);
-            m_motorR.setSpeed(0);
-            state = 2;
-            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
-            somme_erreur_theta = 0;
-        }
-    }
-
-    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
-    if(state == 2) 
-    {
-        //Source d'erreurs et de ralentissements
-        /*if(delta_erreur_distance > 0) // On a dépassé le point
-        {
-            state = 1;
-            return;
-        }*/
-        
-        if(abs(erreur_distance) > 55.0f) somme_erreur_distance = 0;
-        
-        cmd_g = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance - (erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle);
-        cmd_d = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance + erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
-        
-        
-        if(abs(erreur_distance) > 55.0f)
-        {
-            if(cmd_g > limite) cmd_g = limite;
-            else if(cmd_g < -limite) cmd_g = -limite;
-            
-            if(cmd_d > limite) cmd_d = limite;
-            else if(cmd_d < -limite) cmd_d = -limite;
-        }
-        else
-        {
-            Kd_distance = 0.01;
-            if(cmd_g > lim_max) cmd_g = lim_max;
-            else if(cmd_g < -lim_max) cmd_g = -lim_max;
-            
-            if(cmd_d > lim_max) cmd_d = lim_max;
-            else if(cmd_d < -lim_max) cmd_d = -lim_max;
-        }
-        
-        if(cmd_g > 0.01f && cmd_g < lim_min) cmd_g = lim_min;
-        if(cmd_g < -0.01f && cmd_g > -lim_min) cmd_g = -lim_min;
-        
-        if(cmd_d > 0.01f && cmd_d < lim_min) cmd_d = lim_min;
-        if(cmd_d < -0.01f && cmd_d > -lim_min) cmd_d = -lim_min;
-            
-        m_motorL.setSpeed(cmd_g);
-        m_motorR.setSpeed(cmd_d);
-        
-        if(abs(erreur_distance) < 5.0f) N++;
-        else N = 0;
-        if(N > 10)
-        {
-            delta_erreur_theta = 0;
-            erreur_precedente_theta = 0;
-            somme_erreur_theta = 0;
-            erreur_theta = 0;
-            m_motorL.setSpeed(0);
-            m_motorR.setSpeed(0);
-            if(squip == true) 
-            {
-                arrived = true;
-                squip = false;
-                state = 0;
-            }
-            else state = 3;
-            //logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
-        }
-    }
-
-    // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
-    if(state == 3)
-    {
-        erreur_theta = m_goalPhi-m_odometry.getTheta();
-        
-        if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
-        if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
-        
-        cmd = erreur_theta*Kp_angle;
-        
-        if(cmd > limite) cmd = limite;
-        else if(cmd < -limite) cmd = -limite;
-        
-        m_motorL.setSpeed(-cmd);
-        m_motorR.setSpeed(cmd);
-        
-        if(abs(erreur_theta)< 0.05) N++;
-        else N = 0;
-        if(N > 10)
-        {
-            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
-            somme_erreur_theta = 0;
-            arrived = true;
-            squip = false;
-            state = 0;
-            m_motorL.setSpeed(0);
-            m_motorR.setSpeed(0);
-        }
-    }
-}

--- a/Asserv_Plan_B/planB.h	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,48 +0,0 @@
-#ifndef PLANB_H
-#define PLANB_H
-
-#include "mbed.h"
-#include "Odometry2.h"
-#include "Motor.h"
-
-class aserv_planB
-{
-public:
-    aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR);
-    void update(float dt);
-    void control_speed();
-    void setGoal(float x, float y, float theta);
-    void setGoal(float x, float y);
-    void stop(void);
-    bool isArrived(void) {return arrived;}
-    float carre(float x) {return x*x;}
-    float Kp_angle, Kd_angle, Ki_angle;
-    float Kp_distance, Ki_distance, Kd_distance;
-
-private:
-    Odometry2 &m_odometry;
-    Motor &m_motorL;
-    Motor &m_motorR;
-    
-    float cmd;
-    float cmd_g, cmd_d;
-    float limite;
-    float lim_min, lim_max;
-    
-    float somme_erreur_theta, somme_erreur_distance;
-    float delta_erreur_theta, delta_erreur_distance;
-    float erreur_precedente_theta, erreur_precedente_distance;
-    float distanceGoal, distance;
-    float thetaGoal;
-    
-    float m_goalX, m_goalY, m_goalPhi;
-    
-    bool arrived, squip;
-    int N;
-    
-    char state;
-    //char etat_angle;
-};
-
-
-#endif

--- a/Map/Map.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ b/Map/Map.cpp	Thu Jan 07 18:22:38 2016 +0000
@@ -10,6 +10,7 @@
 #else
     #include "mbed.h"
     extern Serial logger;
+    extern Serial pc;
 #endif
 #include <math.h>
 
@@ -69,7 +70,7 @@
     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,1000,1000,30));// P16
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1000+1000/20,1000+1000/20,30/20));// P16
 }
 
 int Map::getHeight(float x, float y)
@@ -98,7 +99,7 @@
     path.clear();
     
     Point goal(goal_x/mpc,goal_y/mpc);
-    if(getHeight(goal_x,goal_y) >= 32000)
+    /*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);
@@ -112,7 +113,7 @@
             logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
         #endif
         return 5;
-    }
+    }*/
     
     
     unsigned int i=0;
@@ -127,9 +128,9 @@
     std::vector<Point*> closeList;
     
     Point* current;
+    pc.printf("@Map::AStar l.130 : Debut du do\n\r");
     do
     {
-        
         // On cherche le plus petit F dans la liste ouverte
         current = openList[0];
         
@@ -151,6 +152,7 @@
         #endif
         
         // On ajoute tous ses voisins viable das la liste ouverte
+        pc.printf("@Map::AStar l.155 : Debut du for d'ajout des voisins dans la liste ouverte\n\r");
         for(int dx=-1;dx<=1;dx++)
         {
             for(int dy=-1;dy<=1;dy++)
@@ -170,10 +172,8 @@
                         delete closeList[i];
                     
                     path.clear();
-                        
-                    #if LOG_LEVEL >= 1
-                        logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
-                    #endif
+                    
+                    logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
                     
                     return 3;
                 }
@@ -185,11 +185,12 @@
                 }
                 
                 int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
-                if(height>=32000)  // On ignore le point, il n'est pas accessible
+                /*if(height>=32000)  // On ignore le point, il n'est pas accessible
                 {
+                    pc.printf("@Map::AStar l.190 : Point inaccessible\n\r");
                     delete p;
                     continue; // On ignore le point si il est déjà dans la liste fermé
-                }
+                }*/
                 
                 if(p->in(openList,pos))
                 {
@@ -224,6 +225,10 @@
                 }
             }
         }
+        pc.printf("@Map::AStar l.229 : Fin du while\n\r");
+        if(!openList.empty())
+            pc.printf("@Map::AStar l.231 : openList.size = %f\n\r", openList.size());
+        pc.printf("@Map::AStar l.233 : dist(closeList.back(),goal)=%f\n\r", dist(closeList.back(),&goal));
     }
     while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
     
@@ -231,7 +236,7 @@
     #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
-    
+    pc.printf("@Map::AStar l.155 : Debut du tendage\n\r");
     if(!openList.empty())
     {
         #ifdef CODEBLOCK
@@ -319,7 +324,7 @@
                         {
                             y=a*x+b;
                             
-                            #ifdef CODEBLOCK
+                            /*#ifdef CODEBLOCK
                                 f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
                             #endif // CODEBLOCK
                             
@@ -327,7 +332,7 @@
                             {   
                                 necessaire = true;
                                 break;
-                            }
+                            }*/
                         }
                         
                         if(!necessaire)
@@ -353,7 +358,7 @@
                         {
                             x=a*y+b;
                             
-                            #ifdef CODEBLOCK
+                            /*#ifdef CODEBLOCK
                                 f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
                             #endif // CODEBLOCK
                             
@@ -361,7 +366,7 @@
                             {
                                 necessaire = true;
                                 break;
-                            }
+                            }*/
                         }
                         
                         if(!necessaire)

--- a/Map/Objectifs/Obj_clap.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-#include "Obj_clap.h"
-
-#ifdef PLAN_A
-    extern Asserv<float> asserv;
-#else
-    extern aserv_planB asserv;
-#endif
-
-Obj_clap::Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD)
-:Objectif(x,y,theta)
-{
-    this->ax12_brasG = ax12_brasG;
-    this->ax12_brasD = ax12_brasD;
-    this->x2 = x2;
-    this->y2 = y2;
-    this->theta2 = theta2;
-}
-
-void Obj_clap::run()
-{
-    if(theta2 == PI/2)
-    {
-        ax12_brasD->setMaxTorque(MAX_TORQUE);
-        ax12_brasD->setGoal(BRASD_OUVERT);
-        wait(1);
-        asserv.setGoal(x2,y2,theta2);
-        while(!asserv.isArrived())wait(0.1);
-        ax12_brasD->setGoal(BRASD_FERME);
-        wait(1);
-        ax12_brasD->setMaxTorque(0);
-    }
-    else
-    {
-        ax12_brasG->setMaxTorque(MAX_TORQUE);
-        ax12_brasG->setGoal(BRASG_OUVERT);
-        wait(1);
-        asserv.setGoal(x2,y2,theta2);
-        while(!asserv.isArrived())wait(0.1);
-        ax12_brasG->setGoal(BRASG_FERME);
-        wait(1);
-        ax12_brasG->setMaxTorque(0);
-    }
-    
-    done = true;
-}
-
-int Obj_clap::isActive()
-{
-    if(!active)
-        return false;
-    
-    if(ax12_brasG->getGoal() == BRASG_OUVERT || ax12_brasD->getGoal() == BRASD_OUVERT)
-        return false;
-    
-    return true;
-}

--- a/Map/Objectifs/Obj_clap.h	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-#ifndef OBJ_CLAP_H
-#define OBJ_CLAP_H
-
-#include "defines.h"
-
-#include "AX12.h"
-#include "Objectif.h"
-
-class Obj_clap: public Objectif
-{
-public:
-    Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD);
-    virtual void run();
-    virtual int isActive();
-private:
-    AX12 *ax12_brasG;
-    AX12 *ax12_brasD;
-    float x2,y2,theta2;
-};
-
-
-
-#endif

--- a/Map/Objectifs/Obj_depot.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,37 +0,0 @@
-#include "Obj_depot.h"
-
-extern Motor motorL;
-extern Motor motorR;
-
-Obj_depot::Obj_depot(float x, float y, float theta, AX12 *ax12_pince)
-:Objectif(x,y,theta)
-{
-    this->ax12_pince = ax12_pince;
-    desactivate();
-}
-
-void Obj_depot::run()
-{
-    ax12_pince->setGoal(PINCE_OUVERTE);
-    wait(1.5);
-    ax12_pince->setMaxTorque(0);
-    
-    motorL.setSpeed(-0.3);
-    motorR.setSpeed(-0.3);
-    wait(0.5);
-    motorL.setSpeed(0);
-    motorR.setSpeed(0);
-    
-    done = true;
-}
-
-int Obj_depot::isActive()
-{
-    if(!active)
-        return false;
-    
-    if(ax12_pince->getGoal() == PINCE_OUVERTE)
-        return false;
-    
-    return true;
-}

--- a/Map/Objectifs/Obj_depot.h	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-#ifndef OBJ_DEPOT_H
-#define OBJ_DEPOT_H
-
-
-#include "defines.h"
-
-#include "AX12.h"
-#include "Objectif.h"
-
-class Obj_depot: public Objectif
-{
-public:
-    Obj_depot(float x, float y, float theta, AX12 *ax12_pince);
-    virtual void run();
-    virtual int isActive();
-private:
-    AX12 *ax12_pince;
-};
-
-
-
-#endif

--- a/Map/Objectifs/Obj_pince.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,61 +0,0 @@
-#include "Obj_pince.h"
-#include <vector>
-#include "Map.h"
-
-#ifdef PLAN_A
-    extern Asserv<float> asserv;
-#else
-    extern aserv_planB asserv;
-#endif
-
-extern Odometry2 odometry;
-extern std::vector<Objectif*> objectifs;
-extern Map terrain;
-
-Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
-:Objectif(x,y,theta)
-{
-    this->ax12_pince = ax12_pince;
-    this->xp = xp;
-    this->yp = yp;
-}
-
-void Obj_pince::run()
-{
-    asserv.setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
-    while(!asserv.isArrived())wait(0.1);
-    
-    ax12_pince->setMaxTorque(MAX_TORQUE);
-    ax12_pince->setGoal(PINCE_FERMEE);
-    wait(1.5);
-    done = true;
-    
-    for(unsigned int i=0;i < objectifs.size();i++)
-    {
-        if(IDO_PC1 <= id && id <= IDO_PC4 && objectifs[i]->getId() == IDO_DEPOT_PC && !objectifs[i]->isDone())
-        {
-            objectifs[i]->activate();
-            break;
-        }
-        if(IDO_P1 <= id && id <= IDO_P16 && objectifs[i]->getId() == IDO_DEPOT_P && !objectifs[i]->isDone())
-        {
-            objectifs[i]->activate();
-            break;
-        }
-    }
-    
-    for(unsigned int i=0;i < terrain.obstacles.size();i++)
-        if(id == terrain.obstacles[i]->getId())
-            terrain.obstacles[i]->desactivate();
-}
-
-int Obj_pince::isActive()
-{
-    if(!active)
-        return false;
-    
-    if(ax12_pince->getGoal() == PINCE_FERMEE)
-        return false;
-    
-    return true;
-}

--- a/Map/Objectifs/Obj_pince.h	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-#ifndef OBJ_PINCE_H
-#define OBJ_PINCE_H
-
-
-#include "defines.h"
-
-#include "AX12.h"
-#include "Objectif.h"
-
-class Obj_pince: public Objectif
-{
-public:
-    Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince);
-    virtual void run();
-    virtual int isActive();
-private:
-    AX12 *ax12_pince;
-    float xp, yp;
-};
-
-
-
-#endif

--- a/Map/Objectifs/Objectif.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,11 +0,0 @@
-#include "Objectif.h"
-
-Objectif::Objectif(float x, float y, float theta)
-{
-    this->x = x;
-    this->y = y;
-    this->theta = theta;
-    done = false;
-    active = true;
-    id = -1;
-}
\ No newline at end of file

--- a/Map/Objectifs/Objectif.h	Thu Jan 07 15:54:49 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,35 +0,0 @@
-#ifndef OBJECTIF_H
-#define OBJECTIF_H
-
-#include "defines.h"
-
-#ifdef PLAN_A
-    #include "Asserv.h"
-#else
-    #include "planB.h"
-#endif
-
-class Objectif
-{
-public:
-    Objectif(float x, float y, float theta);
-    
-    virtual void run() = 0;
-    bool isDone(){return done;}
-    float getX(){return x;}
-    float getY(){return y;}
-    float getTheta(){return theta;}
-    
-    int getId(){return id;}
-    void setId(int id){this->id = id;}
-    
-    virtual int isActive(){return active;}
-    void activate(){active=true;}
-    void desactivate(){active=false;}
-protected:
-    float x,y,theta;
-    bool done,active;
-    int id;
-};
-
-#endif

--- a/main.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ b/main.cpp	Thu Jan 07 18:22:38 2016 +0000
@@ -4,7 +4,7 @@
     Map/define.h
     Map/Obstacles/Obstacle.h
 */ 
-#include "Map/Map.h"
+#include "Map.h"
 
 #define dt 10000
 #define ATTENTE 0
@@ -15,7 +15,7 @@
 DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
 Ticker ticker;
-//Serial pc(USBTX, USBRX);
+Serial logger(PA_9, PA_10);
 Serial pc(PA_9, PA_10);
 RoboClaw roboclaw(460800, PA_11, PA_12);
 Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
@@ -38,11 +38,14 @@
     init();
     //Construction des obstacles
     map.build();
-    map.Astar(0, 1000, 2000, 1000, 1);
+    pc.printf("map built\n\r");
+    map.AStar(1000, 1000+1000/20, 1000+2000/20, 1000+1000/20, 1);
+    pc.printf("Astar done\n\r");
     path = map.path;
-    
+    pc.printf("Hello from main : taille=%f\n\r", path.size());
     for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, 0);
+        pc.printf("Hello from GotoXY : x=%f, y=%f\n\r", path[i].x, path[i].y);
+        odo.GotoXYT(20*(path[i].x-1000), 20*(path[i].y-1000), 0);
     }
         
     //odo.GotoXYT(500, 50, 0);
@@ -58,7 +61,7 @@
 void init(void)
 {
     pc.baud(9600);
-    pc.printf("Hello from main !\n\r");
+    pc.printf("Hello from init !\n\r");
     wait_ms(500);
     
     odo.setPos(0, 0, 0);
@@ -70,6 +73,7 @@
     mybutton.fall(&pressed);
     mybutton.rise(&unpressed);
     ticker.attach_us(&update_main,dt); // 100 Hz
+    pc.printf("Leave from init !\n\r");
 }
 
 void update_main(void)