Revision 76:a862cb10559c, committed 2016-05-05

Comitter:

sype

Date:

Thu May 05 06:34:13 2016 +0000

Parent:

75:195dd2bb13a3

Commit message:

debuggage

Changed in this revision

--- a/Functions/defines.h	Thu May 05 04:36:59 2016 +0000
+++ b/Functions/defines.h	Thu May 05 06:34:13 2016 +0000
@@ -8,10 +8,10 @@
 #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 SEUIL 0.3
 #define VERT 1
 #define VIOLET 2
 #define ATTENTE 0
@@ -23,7 +23,6 @@
 #define BAS 0
 #define DELAY 0.007
 
-extern bool front_Sharp_activated;
 extern Serial logger;
 extern RoboClaw roboclaw;
 extern DigitalOut bleu;

--- a/Functions/func.cpp	Thu May 05 04:36:59 2016 +0000
+++ b/Functions/func.cpp	Thu May 05 06:34:13 2016 +0000
@@ -145,17 +145,19 @@
 
 void homologation(void)
 {
-    goHomeZ();
-    ZMot.mm(50, HAUT);
-    goHomeL();
-    goHomeR();
-    LMot.mm(50, HAUT);
-    RMot.mm(50, HAUT);
+    //goHomeZ();
+    //ZMot.mm(50, HAUT);
+    //goHomeL();
+    //goHomeR();
+    //LMot.mm(50, HAUT);
+    //RMot.mm(50, HAUT);
     while(START==0) logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
     roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, vitesse_dista);
     while(1) {
         while(capt2.read() < SEUIL) checkAround();
-        while(1) roboclaw.SpeedAccelM1M2(accel_dista, 0, 0);
+        roboclaw.ForwardM1(0);
+        roboclaw.ForwardM2(0);
+        while(1);
     }
 }
 

--- a/Map/Objectif/objectif.cpp	Thu May 05 04:36:59 2016 +0000
+++ b/Map/Objectif/objectif.cpp	Thu May 05 06:34:13 2016 +0000
@@ -1,46 +1,33 @@
 #include "objectif.h"
 
-/* Dernier Changement : Romain 0h20 */
+/* Dernier Changement : Romain 20h20 */
 
-objectif::objectif (int ntype, float nx_obj, float ny_obj, float nthet_obj, Odometry * nCodo, AX12 * np, ControlleurPince * npince, float arr) {
+objectif::objectif (int ntype, float nx_obj, float ny_obj, float nthet_obj, AX12 * np, ControlleurPince * npince) {
 	Parasol = np;
 	pince = npince;
 	type = ntype;
 	x_objectif = nx_obj;
 	y_objectif = ny_obj;
 	thet_objectif = nthet_obj;
-	Codo = nCodo;
-	marche_arriere = arr;
 }
 
 bool objectif::Action () {
 	switch(type) {
-		case OBJ_BLOC_PRISE:
+		case OBJ_BLOC:
 			return obj_bloc_action();
-		case OBJ_BLOC_LACHE:
-			return obj_para_lache_action();
+			break;
 		case OBJ_PARA:
 			return obj_para_action();
+			break;
 		default:
-			return true;
-	}
-
-	if (marche_arriere != 0) {
-		Codo->GotoDist(-marche_arriere);
+			break;
 	}
 }
 
 bool objectif::obj_bloc_action () {
-	front_Sharp_activated = false;
 	return true;
 }
 
 bool objectif::obj_para_action() {
 	return true;
-}
-
-bool objectif::obj_para_lache_action() {
-	Codo->GotoDist(-80);
-	front_Sharp_activated = true;
-	return true;
 }
\ No newline at end of file

--- a/Map/Objectif/objectif.h	Thu May 05 04:36:59 2016 +0000
+++ b/Map/Objectif/objectif.h	Thu May 05 06:34:13 2016 +0000
@@ -1,18 +1,16 @@
 #ifndef OBJECTIF_H
 #define OBJECTIF_H
 
-/* Dernier Changement : Romain 0h20 
+/* Dernier Changement : Romain 20h20 
 Inclu dans : map.h */
 
 #include "objectif_type.h"
 #include "../../AX12/AX12.h"
-#include "../../Odometry/Odometry.h"
 #include "../../ControlleurPince/ControlleurPince.h"
-#include "../../Functions/defines.h"
 
 class objectif {
 public:
-	objectif (int ntype, float nx_obj, float ny_obj, float nthet_obj, Odometry * nCodo, AX12 * np, ControlleurPince * npince, float arr = 0);
+	objectif (int ntype, float nx_obj, float ny_obj, float nthet_obj, AX12 * np, ControlleurPince * npince);
 	bool Action ();
 	float getX() { return x_objectif; }
 	float getY() { return y_objectif; }
@@ -21,14 +19,11 @@
 private:
 	bool obj_bloc_action();
 	bool obj_para_action();
-	bool obj_para_lache_action();
 
 	int type;
 	float x_objectif, y_objectif, thet_objectif;
-	float marche_arriere;
 	AX12 * Parasol;
 	ControlleurPince * pince;
-	Odometry * Codo;
 };
 
 #endif
\ No newline at end of file

--- a/Map/Objectif/objectif_type.h	Thu May 05 04:36:59 2016 +0000
+++ b/Map/Objectif/objectif_type.h	Thu May 05 06:34:13 2016 +0000
@@ -1,9 +1,7 @@
 #ifndef OBJECTIF_TYPE_H
 #define OBJECTIF_TYPE_H
 
-#define OBJ_BLOC_PRISE 1
+#define OBJ_BLOC 1
 #define OBJ_PARA 2
-#define OBJ_BLOC_LACHE 3
-#define RIEN 0
 
 #endif
\ No newline at end of file

--- a/Map/map.cpp	Thu May 05 04:36:59 2016 +0000
+++ b/Map/map.cpp	Thu May 05 06:34:13 2016 +0000
@@ -2,34 +2,7 @@
 
 /* Dernier Changement : Romain 20h30 */
 
-void map::Build_Objectives() {
-    if (objectifs.empty()) {
-        logger.printf("map::Build_Objectives Creation des Objectifs ...\n\r");
-        if (couleur == VERT) {
-            addObj (objectif (OBJ_BLOC_PRISE, 120, 1000, 0, Codo, Parasol, pince));
-            addObj (objectif (OBJ_BLOC_LACHE, 120, 1000, 0, Codo, Parasol, pince, 50));
-        }
-        else {
-            addObj (objectif (OBJ_BLOC_PRISE, 3000-350, 850, 0, Codo, Parasol, pince));
-            addObj (objectif (RIEN, 3000-550, 850, 0, Codo, Parasol, pince));
-            addObj (objectif (OBJ_BLOC_LACHE, 3000-1050, 1100, 0, Codo, Parasol, pince));
-            addObj (objectif (RIEN, 3000-200, 850, 0, Codo, Parasol, pince));
-        }
-    }
-}
-
-map::map (Odometry* nodo, AX12 * np, ControlleurPince * npince, int ncouleur, int nformation) : Codo(nodo), Parasol(np), pince(npince) {
-    couleur = ncouleur;
-    formation = nformation;
-
-    if (couleur == VERT) {
-        Codo->setPos(X_START_VERT, Y_START, 0);
-    }
-    else {
-        Codo->setPos(Y_START, Y_START, -PI);
-    }
-    Build();
-    Build_Objectives();
+map::map (Odometry* nodo, AX12 * np, ControlleurPince * npince) : Codo(nodo) {
 }
 
 void map::addObs (obsCarr nobs) {
@@ -60,6 +33,7 @@
     
     pointParcours depP (dep, NULL, arr);
     int indTMP1=0;  // Le point actuel
+    int PointEnding = 0;    
     open.push_back (depP);
 
     while (!ended && !open.empty ()) {
@@ -199,10 +173,8 @@
 }
 
 void map::Execute(int obj) {
+    // logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
     objectif o = objectifs[obj];
-    logger.printf("map::Execute(int obj) Realisation de l'objectif %d\n\r", obj);
-    logger.printf("Depart [%f, %f] Objectif [%f, %f]\n\r", Codo->getX(), Codo->getY(), o.getX(), o.getY());
-    // logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
     FindWay (Codo->getX(), Codo->getY(), o.getX(), o.getY());
     
     if (endedParc) {
@@ -211,7 +183,6 @@
             // 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());
-            Codo->Stop();
             // logger.printf("Goto Fini\n\r");
         }
         Codo->GotoThet(o.getThet());
@@ -224,78 +195,57 @@
     endedParc = false;
 }
 
-void map::Execute() {
-    for (int i = 0; i < objectifs.size();++i) {
-        logger.printf("map::Execute() Realisation de l'objectif %d\n\r", i);
-        Execute(i);
-    }
-}
-
-void map::Build () {
-    logger.printf("map::Build Creation des obstacles ...\n\r");
+void map::Build (int couleur, int formation) {
     if (couleur == VERT) {
         max_x_table = 1400;
-        max_y_table = 1900;
-        min_x_table = 100;
-        min_y_table = 100;
+        max_y_table = 1800;
+        min_x_table = 0;
+        min_y_table = 0;
     }
     else {
         max_x_table = 2900;
-        max_y_table = 1900;
+        max_y_table = 1800;
         min_x_table = 1600;
-        min_y_table = 100;
+        min_y_table = 0;
     }
 
     if (couleur == VERT) {
-        // Attention les commentaires sont inversées par rapport aux valeur en x et y des obstacles.
-        // Il faut lire les commentaires de la façon dont la carte est présentée dans le règlement
-        // Un / signifie un point de départ. Milieu/Haut/Haut signifie que l'on part du milieu, que l'on va en haut puis encore en haut  
-
         addObs(obsCarr (0, 2000, 250, 150));  // Coté haut droite
         addObs(obsCarr (200, 2000, 200, 50));
-
-        addObs(obsCarr (800, 100, 100, 15));    // Petit obstacle en haut à gauche
+        addObs(obsCarr (800, 100, 100, 15));
     }
     else {
         addObs(obsCarr (3000, 2000, 250, 150));     // Coté bas droite
         addObs(obsCarr (2800, 2000, 200, 50));
-
-        addObs(obsCarr (2200, 100, 100, 15));   // Petit Obstacle en haut à gauche
+        addObs(obsCarr (2200, 100, 100, 15));
     }
 
-    addObs(obsCarr (1500, 750, 1100, 15));      // Obstacle du milieu à la verticale
-    addObs(obsCarr (1500, 1050, 20, 300));      // Vitre du milieu (horizontale)
+    addObs(obsCarr (1500, 750, 1100, 15));
+    addObs(obsCarr (1500, 1050, 20, 300));
     
     if (formation == 1) { 
         Build_formation_1 (couleur);
     }
-    if (formation == 2) {
-        Build_formation_2 (couleur);        
-    }
-    if (formation == 3) {
-        Build_formation_3 (couleur);   
-    }
     else {
-        addObs(obsCarr (1250, 1000, 220, 220)); // Obstacles du test standard hors-coupe. A ignorer
+        addObs(obsCarr (1250, 1000, 220, 220));
         addObs(obsCarr (1500, 750, 220, 220));
         addObs(obsCarr (1500, 1250, 220, 220));   
     }
 }
 
 void map::Build_formation_1 (int couleur) {
-    logger.printf("map::Build_formation1 Ajout des coquillages de la formation 1 ...\n\r");
     if (couleur == VERT) {
-        addObs(obsCarr (200, 2000-450, 40, 40));    // Coquillage du haut - droite
-        addObs(obsCarr (200, 2000-750, 40, 40));    // Coquillage sur le même axe horizontal
+        addObs(obsCarr (200, 2000-450, 40, 40));   // Coquillages du haut droit
+        addObs(obsCarr (200, 2000-750, 40, 40));
         
-        addObs(obsCarr (900, 2000-550, 40, 40));    // Coqullage du milieu/haut/haut
-        addObs(obsCarr (1200, 2000-350, 40, 40));   // Coquillage du milieu/haut
+        addObs(obsCarr (900, 2000-550, 40, 40));
+        addObs(obsCarr (1200, 2000-350, 40, 40)); // Coquillages du milieu/haut
         
-        addObs(obsCarr (1500, 2000-550, 40, 40));   // Coquillage du milieu gauche
-        addObs(obsCarr (1500, 2000-350, 40, 40));   // Coquillage du milieu droit
+        addObs(obsCarr (1500, 2000-550, 40, 40));
+        addObs(obsCarr (1500, 2000-350, 40, 40)); 
         
         //addObs(obsCarr (3000-900, 2000-550, 40, 40));
-        addObs(obsCarr (3000-1200, 2000-350, 40, 40));   // Coquillage du milieu bas
+        addObs(obsCarr (3000-1200, 2000-350, 40, 40));   
         
         //addObs(obsCarr (3000-200, 2000-450, 40, 40));   // Coquillages du bas droite
         //addObs(obsCarr (3000-200, 2000-750, 40, 40));        
@@ -307,64 +257,51 @@
         //addObs(obsCarr (900, 2000-550, 40, 40));
         addObs(obsCarr (1200, 2000-350, 40, 40)); // Coquillages du milieu/haut
         
-        addObs(obsCarr (1500, 2000-550, 40, 40));   // Coquillage du milieu gauche
-        addObs(obsCarr (1500, 2000-350, 40, 40));   // Coquillage du milieu droite
+        addObs(obsCarr (1500, 2000-550, 40, 40));
+        addObs(obsCarr (1500, 2000-350, 40, 40)); 
         
-        addObs(obsCarr (3000-900, 2000-550, 40, 40));   // Coquillage du milieu/bas 
-        addObs(obsCarr (3000-1200, 2000-350, 40, 40));  // Coquillage du milieu/bas/bas
+        addObs(obsCarr (3000-900, 2000-550, 40, 40));
+        addObs(obsCarr (3000-1200, 2000-350, 40, 40));   
         
         addObs(obsCarr (3000-200, 2000-450, 40, 40));   // Coquillages du bas droite
-        addObs(obsCarr (3000-200, 2000-750, 40, 40));   // Coquillage sur le même axe horizontal       
+        addObs(obsCarr (3000-200, 2000-750, 40, 40));        
     }
 }
 
 void map::Build_formation_2 (int couleur) {
     if (couleur == VERT) {
-        addObs(obsCarr (200, 2000-450, 40, 40));    // Coquillage du haut gauche
-        addObs(obsCarr (200, 2000-750, 40, 40));    // Coquillage du haut droite
+        addObs(obsCarr (200, 2000-450, 40, 40));   // Coquillages du haut droit
+        addObs(obsCarr (200, 2000-750, 40, 40));
+
+        addObs(obsCarr (600, 2000-450, 40, 40));   // Coquillages du milieu haut
+        addObs(obsCarr (600, 2000-750, 40, 40));
 
-        addObs(obsCarr (600, 2000-450, 40, 40));    // Coquillage du milieu/haut/haut droite
-        addObs(obsCarr (600, 2000-750, 40, 40));    // Coquillage du milieu/haut/haut sur le même axe horizontal
-
-        addObs(obsCarr (1200, 2000-350, 40, 40));   // Coquillage du milieu/haut droite
+        addObs(obsCarr (900, 2000-550, 40, 40));
+        addObs(obsCarr (1200, 2000-350, 40, 40)); // Coquillages du milieu/haut
+        
+        addObs(obsCarr (1500, 2000-550, 40, 40));
+        addObs(obsCarr (1500, 2000-350, 40, 40)); 
+        
+        //addObs(obsCarr (3000-900, 2000-550, 40, 40));
+        addObs(obsCarr (3000-1200, 2000-350, 40, 40));   
+        
+        //addObs(obsCarr (3000-200, 2000-450, 40, 40));   // Coquillages du bas droite
+        //addObs(obsCarr (3000-200, 2000-750, 40, 40));        
     }
     else {
         //addObs(obsCarr (200, 2000-450, 40, 40));   // Coquillages du haut droit
         //addObs(obsCarr (200, 2000-750, 40, 40));
         
         //addObs(obsCarr (900, 2000-550, 40, 40));
-        addObs(obsCarr (3000-200, 2000-450, 40, 40));    // Coquillage du bas gauche
-        addObs(obsCarr (3000-200, 2000-750, 40, 40));    // Coquillage du bas droite
-
-        addObs(obsCarr (3000-600, 2000-450, 40, 40));    // Coquillage du milieu bas/bas droite
-        addObs(obsCarr (3000-600, 2000-750, 40, 40));    // Coquillage du milieu bas/bas sur le même axe horizontal
-
-        addObs(obsCarr (1800, 2000-350, 40, 40));   // Coquillage du milieu/bas droite
-    }
-}
-
-void map::Build_formation_3 (int couleur) {
-    if (couleur == VERT) {
-        addObs(obsCarr (200, 2000-450, 40, 40));    // Coquillage du haut gauche
-        addObs(obsCarr (200, 2000-750, 40, 40));    // Coquillage du haut droite
-
-        addObs(obsCarr (600, 2000-450, 40, 40));    // Coquillage du milieu/haut/haut droite
-        addObs(obsCarr (600, 2000-750, 40, 40));    // Coquillage du milieu/haut/haut sur le même axe horizontal
-
-        addObs(obsCarr (600, 2000-150, 40, 40));   // Coquillage du milieu/haut droite
-    }
-    else {
-        //addObs(obsCarr (200, 2000-450, 40, 40));   // Coquillages du haut droit
-        //addObs(obsCarr (200, 2000-750, 40, 40));
+        addObs(obsCarr (1200, 2000-350, 40, 40)); // Coquillages du milieu/haut
+        
+        addObs(obsCarr (1500, 2000-550, 40, 40));
+        addObs(obsCarr (1500, 2000-350, 40, 40)); 
         
-        //addObs(obsCarr (900, 2000-550, 40, 40));
-        // Inverser bas et haut
-        addObs(obsCarr (3000-200, 2000-450, 40, 40));    // Coquillage du bas gauche
-        addObs(obsCarr (3000-200, 2000-750, 40, 40));    // Coquillage du bas droite
-
-        addObs(obsCarr (3000-600, 2000-450, 40, 40));    // Coquillage du milieu bas/bas droite
-        addObs(obsCarr (3000-600, 2000-750, 40, 40));    // Coquillage du milieu bas/bas sur le même axe horizontal
-
-        addObs(obsCarr (3000-600, 2000-150, 40, 40));   // Coquillage du milieu/bas droite
+        addObs(obsCarr (3000-900, 2000-550, 40, 40));
+        addObs(obsCarr (3000-1200, 2000-350, 40, 40));   
+        
+        addObs(obsCarr (3000-200, 2000-450, 40, 40));   // Coquillages du bas droite
+        addObs(obsCarr (3000-200, 2000-750, 40, 40));        
     }
 }
\ No newline at end of file

--- a/Map/map.h	Thu May 05 04:36:59 2016 +0000
+++ b/Map/map.h	Thu May 05 06:34:13 2016 +0000
@@ -1,9 +1,9 @@
 #ifndef MAP_H
 #define MAP_H
 
-/* Dernier Changement : Romain 0h20 
+/* Dernier Changement : Romain 20h30 
 Inclu dans : main.cpp 
-La marche arriere est un reliquat a supprimer */
+Il faut encore gerer les formations de coquillage */
 
 #include "obsCarr.h"
 #include "pointParcours.h"
@@ -15,21 +15,19 @@
 #include "../AX12/AX12.h"
 #include "../../StepperMotor/Stepper.h"
 
-#define X_START_VERT 110
-#define X_START_VIOLET 2890
-#define Y_START 850
+#define MAP_RIGHTSIDE 1
+#define MAP_LEFTSIDE 2
 
 class map {
 public:
-    map (Odometry* nodo, AX12 * np, ControlleurPince * npince, int ncouleur, int nformation);
+    map (Odometry* nodo, AX12 * np, ControlleurPince * npince);
     void addObs (obsCarr nobs);
     void addObj (objectif nobj);
     void FindWay (point dep, point arr);
     void FindWay (float depX, float depY, float arrX, float arrY);
     void Execute (int obj);
-    void Execute ();
     void Execute (float XObjectif, float YObjectif);
-    void Build();
+    void Build(int couleur, int formation);
     nVector<pointParcours>& getParc () { return path; }
     bool& getEnded () { return endedParc; }
 
@@ -39,18 +37,14 @@
     nVector<objectif> objectifs;
 
     bool endedParc; // Definit s'il existe un chemin parcourable dans le dernier FindWay
-    int couleur, formation;
     Odometry* Codo;
-    AX12 * Parasol;
-    ControlleurPince * pince;
+    AX12 * A1, * A2, * A3;
+    Stepper * S1, * S2;
 
     float min_x_table, min_y_table, max_x_table, max_y_table;
 
     void Build_formation_1 (int couleur);
     void Build_formation_2 (int couleur);
-    void Build_formation_3 (int couleur);
-    
-    void Build_Objectives ();
 };
 
 #endif
\ No newline at end of file

--- a/Map/obsCarr.h	Thu May 05 04:36:59 2016 +0000
+++ b/Map/obsCarr.h	Thu May 05 06:34:13 2016 +0000
@@ -5,8 +5,6 @@
 #include "figure.h"
 #include "point.h"
 
-#define TAILLE_SECURITE 155
-
 class obsCarr : public figure {
 public:
     obsCarr (float xc, float yc, float dxt, float dyt) : figure (xc,yc) {

--- a/Odometry/Odometry.cpp	Thu May 05 04:36:59 2016 +0000
+++ b/Odometry/Odometry.cpp	Thu May 05 06:34:13 2016 +0000
@@ -107,7 +107,6 @@
 
 void Odometry::GotoThet(double theta_)
 {
-    logger.printf("Odometry::GotoThet...");
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -141,40 +140,35 @@
 
     //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 ("[Thet] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left); 
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left))
+        logger.printf ("[Thet] %d\t%d\n\r", 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);
     
     wait(0.4);
     setTheta(theta_);
     //arrived = true;
     //logger.printf("arrivey %d\n\r",pos_prog);
-    logger.printf("End\n\r");
 }
 
 void Odometry::GotoDist(double distance)
 {
-    logger.printf("Odometry::GotoDist...");
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
-    bool avant = distance >= 0 ? true: false;
 
     int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
 
     int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
     int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
 
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
     //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);
-    
     wait(0.4);
     //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\n\r");
 }
 
 void Odometry::TestEntraxe(int i) {
@@ -194,8 +188,8 @@
     
     roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
     
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
-       // logger.printf ("[Dist] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left))
+        logger.printf ("[Dist] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left);
         
     wait(0.4);
 }

--- a/Odometry/Odometry.h	Thu May 05 04:36:59 2016 +0000
+++ b/Odometry/Odometry.h	Thu May 05 06:34:13 2016 +0000
@@ -108,10 +108,6 @@
     double carre(double a) {
         return a*a;
     }
-    void Stop() {
-        roboclaw.ForwardM1(0);
-        roboclaw.ForwardM2(0);
-    }
     
     void getEnc();
     

--- a/main.cpp	Thu May 05 04:36:59 2016 +0000
+++ b/main.cpp	Thu May 05 06:34:13 2016 +0000
@@ -1,5 +1,5 @@
 #include "func.h"
-#include "Map/map.h"
+#include "map.h"
 
 #include "ControlleurPince.h"
 
@@ -45,7 +45,7 @@
 DigitalIn EnZ(PB_14);
 DigitalIn EnL(PB_13);
 
-ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL,left_hand,right_hand);
+ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL, left_hand, right_hand);
 
 Ticker ticker;
 //Serial logger(USBTX, USBRX);
@@ -55,7 +55,6 @@
 
 int SIMON_i = 0, SIMON_state = 0, SCouleur = VERT, SStart = 0, SSchema = 1;
 bool SIMON_EL = false, SIMON_EZ = false, SIMON_ER = false, SGauche = false, SDevant = false, SDroite = false;
-bool front_Sharp_activated = true;
 
 void init(void);
 void update_main(void);
@@ -63,34 +62,21 @@
 /* Debut du programme */
 int main(void)
 {
-    init();
-    map m(&odo, NULL, &controlleurPince, VIOLET, 1);
-    m.Execute(0);
-    m.Execute();
-    /*drapeau = 0;
-    init();
-
-    map m(&odo);
-    m.addObs(obsCarr (1250, 1000, 220, 220));
-    m.addObs(obsCarr (1500, 750, 220, 220));
-    m.addObs(obsCarr (1500, 1250, 220, 220));
-
-    init();    
-    
-    int cote = MAP_RIGHTSIDE;
-    /*map m(&odo);
-    m.Build(cote);
-    m.Execute(1000,1000);
-    m.Execute(1500,1000);
-    m.Execute(1500,1500);
-    m.Execute(110,1000);
-
-    odo.GotoThet(0);
     roboclaw.ForwardM1(0);
     roboclaw.ForwardM2(0);
-    logger.printf ("Chemin Fini !");
+    roboclaw.ResetEnc();
 
-    while(1);*/
+    init_interrupt();
+    odo.setPos(110, 1000, 0);
+    logger.printf("Depart homologation !\n\r");
+    while(START != 1);
+    logger.printf("GO !!\n\r");
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, 10000, accel_angle, vitesse_angle, deccel_angle, 10000, 1);
+    logger.printf("Faut avancer !!\n\r");
+    while(1);
+    //odo.GotoXY(1000, 1000);
+    //odo.GotoXY(500, 1500);
+    
 }
 
 void init(void)
@@ -98,24 +84,25 @@
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
 
-    wait(0.5);
-    controlleurPince.init();
-    wait(0.5);
-    controlleurPince.home();
-    
-    //depart();
     init_interrupt();
-    wait_ms(1);
-    while (CAMP == 0);
-    while (CAMP == 1);
-    
-    //while(START==0);
+
+    SCouleur = VERT;
+    LEDV = 1;
+    LEDR = 0;
+
+    odo.setPos(110, 1000, 0);
+    roboclaw.ResetEnc();
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    wait_ms(500);
+    depart();
+    wait_ms(100);
+    while(START==0);
     logger.printf("End init\n\r");
 }
 
 void update_main(void)
 {
     odo.update_odo();
-    if (front_Sharp_activated)
-        checkAround();
+    //checkAround();
 }
\ No newline at end of file

--- a/main.cpp.orig	Thu May 05 04:36:59 2016 +0000
+++ b/main.cpp.orig	Thu May 05 06:34:13 2016 +0000
@@ -1,8 +1,6 @@
 #include "func.h"
 #include "map.h"
 
-#include "ControlleurPince.h"
-
 /* Déclaration des différents éléments de l'IHM */
 
 DigitalIn CAMP(PA_15);
@@ -24,8 +22,8 @@
 DigitalOut blanc(PC_6);
 DigitalOut rouge(PC_8);
 
-AX12 left_hand(PA_9, PA_10, 3, 250000);
-AX12 right_hand(PA_9, PA_10, 2, 250000);
+//AX12 left_hand(PA_9, PA_10, 4, 250000);
+//AX12 right_hand(PA_9, PA_10, 1, 250000);
 
 /* Sharp */
 AnalogIn capt1(PA_4);
@@ -34,9 +32,9 @@
 AnalogIn capt4(PC_0);
 
 /* Moteurs pas à pas */
-Stepper RMot(NC, PA_8, PC_7, PB_15, 4);
-Stepper ZMot(NC, PB_4, PB_10, PB_14, 5);
-Stepper LMot(NC, PB_5, PB_3, PB_13, 4);
+Stepper RMot(NC, PA_8, PC_7, 4);
+Stepper ZMot(NC, PB_4, PB_10, 5);
+Stepper LMot(NC, PB_5, PB_3, 4);
 /* Fins de course */
 InterruptIn EndR(PB_15);
 InterruptIn EndZ(PB_14);
@@ -45,8 +43,6 @@
 DigitalIn EnZ(PB_14);
 DigitalIn EnL(PB_13);
 
-ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL, left_hand, right_hand);
-
 Ticker ticker;
 //Serial logger(USBTX, USBRX);
 Serial logger(PA_2, PA_3);
@@ -62,53 +58,8 @@
 /* Debut du programme */
 int main(void)
 {
-<<<<<<< local
-    init();
-    map m(&odo, NULL, &controlleurPince, VERT, 1);
-    m.Execute(0);
-    m.Execute();
-=======
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-    
-    //logger.printf("Depart homologation !\n\r");
-    //homologation();
-    
-    controlleurPince.init();
-    controlleurPince.home();
-    wait(1);
-    controlleurPince.open();
-    logger.printf("z 10 \r\n");
-    controlleurPince.setPos(10,-1);
-    wait(1);
-    logger.printf("z 20 \r\n");
-    controlleurPince.setPos(20,-1);
-    wait(1);
-    controlleurPince.close();
-    logger.printf("d 30 \r\n");
-    controlleurPince.setPos(-1,30);
-    wait(1);
-    logger.printf("c 50 \r\n");
-    controlleurPince.setPos(-1,-1,50);
-    wait(1);
-    logger.printf("d 0 \r\n");
-    controlleurPince.setPos(-1,0);
-    
-    while(1);
-    
-    
-    /*wait(1);
-    logger.printf("set pos 20 ...\n\r");
-    controlleurPince.setPos(20.f,0.f,0.f);
-    wait(1);
-    logger.printf("set pos 70 ...\n\r");
-    controlleurPince.setPos(70.f,0.f,0.f);
-    wait(1);
-    logger.printf("set pos 20 ...\n\r");
-    controlleurPince.setPos(20.f,0.f,0.f);
-    logger.printf("Done ...\n\r");*/
-    
->>>>>>> other
+    logger.printf("Depart homologation !\n\r");
+    homologation();
     /*drapeau = 0;
     init();
 
@@ -117,11 +68,6 @@
     m.addObs(obsCarr (1500, 750, 220, 220));
     m.addObs(obsCarr (1500, 1250, 220, 220));
 
-    init();    
-    
-    int cote = MAP_RIGHTSIDE;
-    /*map m(&odo);
-    m.Build(cote);
     m.Execute(1000,1000);
     m.Execute(1500,1000);
     m.Execute(1500,1500);
@@ -140,15 +86,23 @@
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
 
-    controlleurPince.home();
-    
+    init_interrupt();
+    goHome();
+
+    SCouleur = VERT;
+    LEDV = 1;
+    LEDR = 0;
+
+    odo.setPos(110, 1000, 0);
+    roboclaw.ResetEnc();
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    wait_ms(500);
+    while(1);
     //depart();
     init_interrupt();
-    wait_ms(1);
-    while (CAMP == 0);
-    while (CAMP == 1);
-    
-    //while(START==0);
+    wait_ms(100);
+    while(START==0);
     logger.printf("End init\n\r");
 }