Trajectoire fonctionnelle pour la compétition de 2025

.vscode/settings.json

@@ -8,6 +8,10 @@
         "vl53l8_2024.h": "c",
         "trajet.h": "c",
         "trajectoire.h": "c",
-        "compare": "c"
+        "compare": "c",
+        "asser_position.h": "c",
+        "stdlib.h": "c",
+        "strategie.h": "c",
+        "strategie_deplacement.h": "c"
     }
 }

Asser_Moteurs.c

@@ -8,8 +8,8 @@
 float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
 float commande_I[3]; // Terme integral
 
-void AsserMoteur_Init(){
-    QEI_init();
+void AsserMoteur_Init(int id){
+    QEI_init(id);
     Moteur_Init();
     for(unsigned int i =0; i< 2; i ++){
         commande_I[i]=0;

Asser_Moteurs.h

@@ -5,4 +5,4 @@ void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s);
 float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
 float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
 void AsserMoteur_Gestion(int step_ms);
-void AsserMoteur_Init();
+void AsserMoteur_Init(int);

Asser_Position.c

@@ -5,6 +5,8 @@
 #define GAIN_P_POSITION 15
 #define GAIN_P_ORIENTATION 10
 
+#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
+
 struct position_t position_maintien;
 
 /// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
@@ -46,7 +48,7 @@ void Asser_Position(struct position_t position_consigne){
     rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
 
     if(delta_avance_mm < 10){
-        rotation_radian_s=0;
+        rotation_radian_s=delta_avance_mm/10 * rotation_radian_s;
     }
     
     // Commande en vitesse
@@ -60,4 +62,17 @@ void Asser_Position_set_Pos_Maintien(struct position_t position){
 
 void Asser_Position_maintien(){
     Asser_Position(position_maintien);
-}
+}
+
+float Asser_Position_get_erreur_angle(){
+    return delta_orientation_radian;
+}
+
+/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil
+/// @return 1 si panic, 0 si nominal
+int Asser_Position_panic_angle(){
+    if(delta_orientation_radian > MAX_ERREUR_ANGLE){
+        return 1;
+    }
+    return 0;
+}

Asser_Position.h

@@ -2,3 +2,5 @@
 void Asser_Position(struct position_t position_consigne);
 void Asser_Position_set_Pos_Maintien(struct position_t position);
 void Asser_Position_maintien();
+
+int Asser_Position_panic_angle();

CMakeLists.txt

@@ -14,12 +14,15 @@ add_executable(Mon_Projet
   Asser_Position.c
   Asser_Moteurs.c
   Commande_vitesse.c
+  Evitement.c
   Geometrie.c
   i2c_maitre.c
   Moteurs.c
   Localisation.c
   main.c
   QEI.c
+  Strategie_deplacement.c
+  Strategie.c
   Temps.c
   Trajectoire_bezier.c
   Trajectoire_circulaire.c

Evitement.c

@@ -0,0 +1,38 @@
+#include "pico/stdlib.h"
+#include "Asser_Moteurs.h"
+#include "Evitement.h"
+#include "Trajet.h"
+
+// 250 ms
+#define TEMPS_VALIDE_OBSTACLE_US 250000
+
+enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
+
+void Evitement_gestion(int step_ms){
+    static uint32_t temps_obstacle;
+    switch(evitement_statu){
+        case PAS_D_OBSTACLE:
+            if(Trajet_get_bloque() == 1 && AsserMoteur_RobotImmobile(step_ms)){
+                evitement_statu = OBSTACLE_NON_CONFIRME;
+                temps_obstacle = time_us_32();
+            }
+            break;
+        case OBSTACLE_NON_CONFIRME:
+            if(time_us_32() - temps_obstacle >  TEMPS_VALIDE_OBSTACLE_US){
+                evitement_statu = OBSTACLE_CONFIRME;
+            }
+            if(!Trajet_get_bloque()){
+                evitement_statu = PAS_D_OBSTACLE;
+            }
+            break;
+        case OBSTACLE_CONFIRME:
+            if(!Trajet_get_bloque()){
+                evitement_statu = PAS_D_OBSTACLE;
+            }
+            break;
+    }
+}
+
+enum evitement_statu_t Evitement_get_statu(){
+    return evitement_statu;
+}

Evitement.h

@@ -0,0 +1,8 @@
+enum evitement_statu_t{
+    PAS_D_OBSTACLE,
+    OBSTACLE_NON_CONFIRME,
+    OBSTACLE_CONFIRME,
+};
+
+enum evitement_statu_t Evitement_get_statu();
+void Evitement_gestion(int step_ms);

Localisation.c

@@ -6,9 +6,9 @@
 
 struct position_t position;
 
-void Localisation_init(){
+void Localisation_init(int id){
     Temps_init();
-    QEI_init();
+    QEI_init(id);
     position.x_mm = 0;
     position.y_mm = 0;
     position.angle_radian = 0;

Localisation.h

@@ -2,7 +2,7 @@
 
 struct position_t Localisation_get(void);
 void Localisation_gestion();
-void Localisation_init();
+void Localisation_init(int);
 
 void Localisation_set(float x_mm, float y_mm, float angle_radian);
 void Localisation_set_x(float x_mm);

QEI.c

@@ -17,8 +17,10 @@
 // Nombre d'impulsions par tour de roue : 8000
 // Impulsion / mm : 42,44
 
-#define IMPULSION_PAR_MM (12.45f)
+#define IMPULSION_PAR_MM_50_1 (12.45f)
+#define IMPULSION_PAR_MM_30_1 (7.47f)
 
+float impulsion_par_mm;
 
 
 struct QEI_t QEI_A, QEI_B;
@@ -28,7 +30,7 @@ bool QEI_est_init = false;
 PIO pio_QEI = pio0;
 
 
-void QEI_init(){
+void QEI_init(int identifiant){
     // Initialisation des 3 modules QEI
     // Chaque module QEI sera dans une machine à état du PIO 0
     if(!QEI_est_init){
@@ -48,6 +50,10 @@ void QEI_init(){
         QEI_B.value=0;
         QEI_est_init=true;
     }
+    impulsion_par_mm = IMPULSION_PAR_MM_50_1;
+    if(identifiant == 0 || identifiant >= 4){
+        impulsion_par_mm = IMPULSION_PAR_MM_30_1;
+    }
 
 }
 
@@ -91,5 +97,5 @@ int QEI_get(enum QEI_name_t qei){
 /// @param  qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
 /// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update()
 float QEI_get_mm(enum QEI_name_t qei){
-    return ((float) QEI_get(qei)) / (float)IMPULSION_PAR_MM;
+    return ((float) QEI_get(qei)) / impulsion_par_mm;
 }

QEI.h

@@ -11,6 +11,6 @@ enum QEI_name_t{
 extern struct QEI_t QEI_A, QEI_B, QEI_C;
 
 void QEI_update(void);
-void QEI_init(void);
+void QEI_init(int);
 int QEI_get(enum QEI_name_t qei);
 float QEI_get_mm(enum QEI_name_t qei);

Readme.md

@@ -3,7 +3,7 @@ PAMI 2024 - Poivron Robotique
 
 Code du PAMI 2024 de l'équipe Poivron Robotique.
 
-La cart e contien les éléments suivants :
+La carte contient les éléments suivants :
 
 *    Microcontrôleur Raspberry Pi Pico
 *    Connecteur pour l’arrêt d’urgence

Strategie.c

@@ -0,0 +1,57 @@
+#include "Strategie.h"
+#include "Geometrie.h"
+
+
+
+enum etat_action_t Strategie_super_star(uint32_t step_ms){
+	static enum{
+        SSS_TEST_TOURNE,
+		SSS_INIT,
+		SSS_AVANCE_1,
+		SSS_TOURNE,
+		SSS_AVANCE_2,
+		SSS_DANCE
+	} etat_sss = SSS_INIT;
+    struct trajectoire_t trajectoire;
+
+	switch(etat_sss){
+        case SSS_TEST_TOURNE:
+            Localisation_set(1135, 1895, 0);
+            etat_sss = SSS_TOURNE;
+            break;
+		case SSS_INIT:
+			Localisation_set(45, 1895, 0);
+			etat_sss = SSS_AVANCE_1;
+			break;
+
+		case SSS_AVANCE_1:
+            Trajet_config(TRAJECT_CONFIG_STD);
+            Trajectoire_droite(&trajectoire, 45, 1895, 1135, 1895, 0, 0);
+            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = SSS_TOURNE;
+            }
+            break;
+
+        case SSS_TOURNE:
+            //Trajet_config(100, 100);
+            Trajet_config(TRAJECT_CONFIG_STD);
+            Trajectoire_circulaire(&trajectoire, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
+            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = SSS_AVANCE_2;
+            }
+            break;
+
+        case SSS_AVANCE_2:
+            Trajet_config(TRAJECT_CONFIG_STD);
+            Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1580, 0, 0);
+            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = SSS_DANCE;
+            }
+            break;
+
+        case SSS_DANCE:
+            return ACTION_TERMINEE;
+	}
+    return ACTION_EN_COURS;
+
+}

Strategie.h

@@ -0,0 +1,53 @@
+#include "pico/stdlib.h"
+#include "Localisation.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+
+
+#ifndef STRATEGIE_H
+#define STRATEGIE_H
+
+#define COULEUR 15
+#define TIRETTE 14
+//#define CORR_ANGLE_DEPART_DEGREE  (-1.145)
+#define CORR_ANGLE_DEPART_DEGREE  (0)
+
+enum etat_action_t{
+    ACTION_EN_COURS,
+    ACTION_TERMINEE,
+    ACTION_ECHEC
+};
+
+enum longer_direction_t{
+    LONGER_VERS_A,
+    LONGER_VERS_C
+};
+
+enum couleur_t{
+    COULEUR_BLEU=0,
+    COULEUR_JAUNE,
+    COULEUR_INCONNUE,
+};
+
+enum evitement_t{
+    EVITEMENT_SANS_EVITEMENT,
+    EVITEMENT_PAUSE_DEVANT_OBSTACLE,
+    EVITEMENT_ARRET_DEVANT_OBSTACLE,
+    EVITEMENT_RETOUR_SI_OBSTABLE,
+    EVITEMENT_CONTOURNEMENT
+};
+
+struct objectif_t{
+    int priorite;
+    enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
+    enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE, 
+        ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
+};
+
+enum etat_action_t Strategie_super_star(uint32_t step_ms);
+
+enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
+
+// STRATEGIE_H
+#endif

Strategie_deplacement.c

@@ -0,0 +1,73 @@
+#include "Strategie.h"
+#include "Strategie_deplacement.h"
+#include "Trajet.h"
+#include "Evitement.h"
+#include "Geometrie.h"
+#include "VL53L8_2024.h"
+
+float distance_obstacle;
+
+enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum etat_trajet_t etat_trajet;
+    float angle_avancement;
+    static bool trajet_inverse = false;
+    
+    static enum {
+        PARCOURS_INIT,
+        PARCOURS_AVANCE,
+    } etat_parcourt=PARCOURS_INIT;
+
+    switch (etat_parcourt){
+        case PARCOURS_INIT:
+            Trajet_debut_trajectoire(trajectoire);
+            etat_parcourt = PARCOURS_AVANCE;
+            break;
+        
+        case PARCOURS_AVANCE:
+            if(evitement != EVITEMENT_SANS_EVITEMENT){
+                angle_avancement = Trajet_get_orientation_avance();
+                distance_obstacle = VL53L8_get_old_min_distance();
+                Trajet_set_obstacle_mm(distance_obstacle);
+            }
+
+            if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){
+                switch(evitement){
+                    case EVITEMENT_SANS_EVITEMENT:
+                        //printf("Evitement lors trajet EVITEMENT_SANS_EVITEMENT: ERREUR\n");
+                        break;
+                    case EVITEMENT_PAUSE_DEVANT_OBSTACLE:
+                        // Rien à faire ici
+                        break;
+                    
+                    case EVITEMENT_ARRET_DEVANT_OBSTACLE:
+                        etat_parcourt = PARCOURS_INIT;
+                        return ACTION_ECHEC;
+
+                    case EVITEMENT_RETOUR_SI_OBSTABLE:
+                        trajet_inverse = !trajet_inverse;
+                        Trajet_inverse();
+                        break;
+
+                    case EVITEMENT_CONTOURNEMENT: // TODO
+                        break;
+                        
+                }
+
+            }
+
+            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(etat_trajet == TRAJET_TERMINE){
+                if(trajet_inverse){
+                    etat_action = ACTION_ECHEC;
+                }else{
+                    etat_action = ACTION_TERMINEE;
+                }
+                etat_parcourt = PARCOURS_INIT;
+            }
+            break;
+    }
+
+    return etat_action;
+
+}

Trajectoire.c

@@ -9,13 +9,13 @@
 #define PRECISION_ABSCISSE 0.001f
 
 
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon,
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad, float rayon,
                         float orientation_debut_rad, float orientation_fin_rad){
     trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
     trajectoire->p1.x = centre_x;
     trajectoire->p1.y = centre_y;
-    trajectoire->angle_debut_degre = angle_debut_degre;
-    trajectoire->angle_fin_degre = angle_fin_degre;
+    trajectoire->angle_debut_rad = angle_debut_rad;
+    trajectoire->angle_fin_rad = angle_fin_rad;
     trajectoire->rayon = rayon;
     trajectoire->longueur = -1;
     trajectoire->orientation_debut_rad = orientation_debut_rad;
@@ -58,8 +58,8 @@ void Trajectoire_inverse(struct trajectoire_t * trajectoire){
     trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad;
 
     if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){
-        trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre;
-        trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
+        trajectoire->angle_debut_rad = old_trajectoire.angle_fin_rad;
+        trajectoire->angle_fin_rad = old_trajectoire.angle_debut_rad;
         return;
     }
     if(trajectoire->type == TRAJECTOIRE_DROITE){

Trajectoire.h

@@ -20,7 +20,7 @@ struct trajectoire_t {
     enum trajectoire_type_t type;
     struct point_xy_t p1, p2, p3, p4;
     float orientation_debut_rad, orientation_fin_rad;
-    float rayon, angle_debut_degre, angle_fin_degre;
+    float rayon, angle_debut_rad, angle_fin_rad;
     float longueur;
 };
 
@@ -29,7 +29,7 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
 float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse);
 float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm);
 double distance_points(struct point_xy_t point, struct point_xy_t point_old);
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre,
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad,
                             float rayon, float orientation_debut_rad, float orientation_fin_rad);
 void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad);
 void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,

Trajectoire_circulaire.c

@@ -4,23 +4,23 @@
 
 void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){
     float distance_angulaire;
-    if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){
-        distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre;
+    if(trajectoire->angle_debut_rad > trajectoire->angle_fin_rad){
+        distance_angulaire = trajectoire->angle_debut_rad - trajectoire->angle_fin_rad;
     }else{
-        distance_angulaire = trajectoire->angle_fin_degre - trajectoire->angle_debut_degre;
+        distance_angulaire = trajectoire->angle_fin_rad - trajectoire->angle_debut_rad;
     }
-    trajectoire->longueur = 2. * M_PI * trajectoire->rayon * distance_angulaire / 360.;
+    trajectoire->longueur = trajectoire->rayon * distance_angulaire;
 }
 
 /// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
 /// @param abscisse : compris entre 0 et 1
 struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){
     struct point_xy_t point;
-    float angle_degre;
+    float angle_rad;
 
-    angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse;
-    point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon;
-    point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon;
+    angle_rad = (float) trajectoire->angle_debut_rad * (1-abscisse) + (float) trajectoire->angle_fin_rad * abscisse;
+    point.x = trajectoire->p1.x + cos(angle_rad) * trajectoire->rayon;
+    point.y = trajectoire->p1.y + sin(angle_rad) * trajectoire->rayon;
 
     return point;
 }

Trajet.c

@@ -25,9 +25,9 @@ const float distance_pas_obstacle = 2000;
 float vitesse_max_contrainte_obstacle;
 
 /// @brief Initialise le module Trajet. A appeler en phase d'initilisation
-void Trajet_init(){
+void Trajet_init(int id){
     Temps_init();
-    AsserMoteur_Init();
+    AsserMoteur_Init(id);
     abscisse = 0;
     vitesse_mm_s = 0;
     position_mm = 0;

Trajet.h

@@ -11,6 +11,7 @@ enum etat_trajet_t{
 
 // Vitesse et acceleration pour translation pure (en mm/s et mm/s²)
 #define TRAJECT_CONFIG_RAPIDE 300, 1200
+#define TRAJECT_CONFIG_RAPIDE_ROUGE 500, 1200
 // Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
 #define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
 // Vitesse et acceleration - standard (en mm et mm/s²)
@@ -20,7 +21,7 @@ enum etat_trajet_t{
 
 extern const float distance_pas_obstacle;
 
-void Trajet_init();
+void Trajet_init(int);
 void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss);
 void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
 enum etat_trajet_t Trajet_avance(float temps_s);

VL53L8_2024.c

@@ -154,7 +154,7 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
 	int min_distance = 2000;
 	int col, row;
 	for(col=0; col<8; col++){
-		for(row=4; row<=6; row++){
+		for(row=0; row<=3; row++){
 			if(min_distance >  Results.distance_mm[col + 8*row]){
 				min_distance = Results.distance_mm[col + 8*row];
 			}

VL53L8_2024.h

@@ -4,3 +4,4 @@ void VL53L8_init(VL53L8CX_Configuration * Dev);
 void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance);
 
+float VL53L8_get_old_min_distance(void);

main.c

@@ -13,6 +13,7 @@
 #include "i2c_maitre.h"
 #include "Localisation.h"
 #include "Moteurs.h"
+#include "Strategie.h"
 #include "Temps.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -39,7 +40,6 @@ uint identifiant_lire(void);
 
 int get_tirette(void);
 int get_couleur(void);
-void configure_trajet(int identifiant, int couleur);
 
 void gestion_VL53L8CX(void);
 
@@ -54,17 +54,17 @@ VL53L8CX_Configuration Dev;
 
 void main(void)
 {
-	int ledpower = 500;
 	VL53L8CX_ResultsData Results;
+	bool fin_match = false;
 
 
 
     stdio_init_all();
 	Temps_init();
-	//tension_batterie_init();
+
 	identifiant_init();
-	Localisation_init();
-	Trajet_init();
+	Localisation_init(identifiant_lire());
+	Trajet_init(identifiant_lire());
 	i2c_maitre_init();
 
 	
@@ -80,31 +80,32 @@ void main(void)
 	gpio_put(LED1PIN, 1);
 
 	
-	//multicore_launch_core1(gestion_affichage);
-	multicore_launch_core1(gestion_VL53L8CX);
+	multicore_launch_core1(gestion_affichage);
+	// TODO: A remettre - quand on aura récupéré un capteur
+	//multicore_launch_core1(gestion_VL53L8CX);
 	sleep_ms(5000);
 	printf("Demarrage...\n");
 	
 	
-
-	configure_trajet(identifiant_lire(), get_couleur());
-
-	
 	float vitesse_init =300;
 	vitesse = vitesse_init;
 
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
 	while(get_tirette());
+	gpio_put(LED1PIN, 0);
 
-	sleep_ms(90000);
+	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
+	if(identifiant_lire() == 3){
+		sleep_ms(90000);
+	}
 
 	temps_depart_ms = Temps_get_temps_ms();
 
 	while(1){
 		
 		// Fin du match 
-		if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
+		if((Temps_get_temps_ms() -temps_depart_ms) >15000 || (fin_match == 1)){
 			Moteur_Stop();
 			while(1);
 		}
@@ -114,10 +115,11 @@ void main(void)
 				QEI_update();
 				Localisation_gestion();
 				
-				if(etat_trajet != TRAJET_TERMINE){
-					etat_trajet = Trajet_avance((float)step_ms/1000.);
-				}else{
+				if(Strategie_super_star(step_ms) == ACTION_TERMINEE){
 					Asser_Position_maintien();
+					if(Asser_Position_panic_angle()){
+						fin_match=1;
+					}
 				}
 				AsserMoteur_Gestion(step_ms);
 			}
@@ -160,18 +162,21 @@ void gestion_affichage(void){
 void affichage(void){
 /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
 	printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
-	printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
+
+	long temps_ms = (time_us_64()/1000);
+	printf(">temps_ms:%lu\n", temps_ms);
+	printf(">pos_angle:%.1f\n", Localisation_get().angle_radian);
 	printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
 
 	printf(">abscisse:%f\n",abscisse);
 	
 	struct position_t position_actuelle;
 	position_actuelle = Localisation_get();
-	printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
-	printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
-	printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
+	printf(">delta_orientation_radian:%lu:%.2f\n>angle_delta:%lu:%.2f\n", temps_ms, delta_orientation_radian, temps_ms, atan2f(delta_y_mm, delta_x_mm));
+	printf(">pos_x:%lu:%.2f\n>pos_y:%lu:%.2f\n", temps_ms, position_actuelle.x_mm, temps_ms, position_actuelle.y_mm);
+	printf(">con_x:%lu:%.2f\n>con_y:%lu:%.2f\n", temps_ms, point.point_xy.x, temps_ms, point.point_xy.y);
 	
-	printf(">couleur:%d\n>id:%d\n", get_couleur(), identifiant_lire());
+	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette());
 }
 
 void tension_batterie_init(void){
@@ -211,8 +216,9 @@ void identifiant_init(){
 	gpio_set_dir(COULEUR_PIN, GPIO_IN);
 }
 
-int get_tirette(void){
+int get_tirette(){
 	return !gpio_get(TIRETTE_PIN);
+	
 }
 
 int get_couleur(void){
@@ -224,85 +230,3 @@ int get_couleur(void){
 uint identifiant_lire(){
 	return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
 }
-
-void configure_trajet(int identifiant, int couleur){
-	
-	struct trajectoire_t trajectoire;
-
-	switch(couleur){
-		case COULEUR_JAUNE:
-			switch (identifiant)
-			{
-				case 0:
-					break;
-				case 1:
-					Localisation_set(3000-1249, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
-					3000-750, 1400, 3000-750, 2100, 0, 0);
-					break;
-				case 2:
-					Localisation_set(3000-1117, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1116, 2000-63, 3000-975, 2000-63, 
-					3000-540, 1400, 3000+60, 1400, -M_PI, -M_PI);
-					break;
-				case 3:
-					Localisation_set(1121-1130, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
-					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
-					break;
-				case 4:
-					break;
-				case 5:
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-
-		case COULEUR_BLEU:
-			switch (identifiant)
-			{
-				case 0:
-					break;
-				case 1:
-					Localisation_set(1249, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
-					750, 1400, 750, 2100, M_PI, M_PI);
-					break;
-				case 2:
-					Localisation_set(1117, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1116, 2000-63, 975, 2000-63, 
-					540, 1400, -60, 1400, M_PI, M_PI);
-					break;
-				case 3:
-					Localisation_set(1121, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
-					606, 2000-590, 225, 2000-225, M_PI, M_PI);
-					break;
-				case 4:
-					break;
-				case 5:
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-	}
-	
-	
-	
-
-	Trajet_config(TRAJECT_CONFIG_RAPIDE);
-	Trajet_debut_trajectoire(trajectoire);
-
-}