Strategie ninja 2, 1 seul sursaut au rebroussement

1 sursaut au rebroussement entre 1A et 1B et 1 sursaut au départ après le recalage

.vscode/settings.json

@@ -12,6 +12,7 @@
         "asser_position.h": "c",
         "stdlib.h": "c",
         "strategie.h": "c",
-        "strategie_deplacement.h": "c"
+        "strategie_deplacement.h": "c",
+        "servomoteur.h": "c"
     }
 }

.vscode/tasks.json

@@ -2,7 +2,7 @@
     "tasks": [
         {
             "type": "shell",
-            "command": "cd build; cmake ../; make",
+            "command": "mkdir -p build; cd build; cmake ../; make",
             "label": "CMake in build/",
             "problemMatcher": [],
             "group": {
@@ -13,7 +13,7 @@
         {
             "type": "shell",
             "command": "cd build; cmake ../; make Flash",
-            "label": "CMake & Make & Flash",
+            "label": "mkdir -p build; CMake & Make & Flash",
             "problemMatcher": [],
             "group": {
                 "kind": "build",

Asser_Moteurs.c

@@ -60,6 +60,11 @@ uint32_t AsserMoteur_RobotImmobile(int step_ms){
     return 0;
 }
 
+void AsserMoteurs_stop(void){
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
+}
+
 /// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
 /// @param step_ms 
 void AsserMoteur_Gestion(int step_ms){

Asser_Moteurs.h

@@ -6,3 +6,4 @@ 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(int);
+void AsserMoteurs_stop(void);

Asser_Position.c

@@ -47,9 +47,9 @@ void Asser_Position(struct position_t position_consigne){
     avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
     rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
 
-    if(delta_avance_mm < 10){
+    /*if(delta_avance_mm < 10){
         rotation_radian_s=delta_avance_mm/10 * rotation_radian_s;
-    }
+    }*/
     
     // Commande en vitesse
     commande_vitesse(avance_mm_s, rotation_radian_s);

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.13)
 
 include(pico_sdk_import.cmake)
 
-project(Mon_Projet C CXX ASM)
+project(PAMI2026a C CXX ASM)
 set(CMAKE_C_STNDARD 11)
 set(CMAKE_CXX_STANDARD 17)
 
@@ -10,7 +10,7 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
 
 pico_sdk_init()
 
-add_executable(Mon_Projet
+add_executable(PAMI2026a
   Asser_Position.c
   Asser_Moteurs.c
   Commande_vitesse.c
@@ -23,6 +23,7 @@ add_executable(Mon_Projet
   QEI.c
   Strategie_deplacement.c
   Strategie.c
+  Servomoteur.c
   Temps.c
   Trajectoire_bezier.c
   Trajectoire_circulaire.c
@@ -38,11 +39,11 @@ add_executable(Mon_Projet
   Platform/platform.c
 )
 
-pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
+pico_generate_pio_header(PAMI2026a ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
 
-target_include_directories(Mon_Projet PRIVATE VL53L8CX_ULD_API/inc/)
+target_include_directories(PAMI2026a PRIVATE VL53L8CX_ULD_API/inc/)
 
-target_link_libraries(Mon_Projet
+target_link_libraries(PAMI2026a
   hardware_adc 
   hardware_i2c 
   hardware_pwm 
@@ -51,12 +52,12 @@ target_link_libraries(Mon_Projet
   pico_multicore
 )
 
-pico_enable_stdio_usb(Mon_Projet 1)
+pico_enable_stdio_usb(PAMI2026a 1)
-pico_enable_stdio_uart(Mon_Projet 1)
+pico_enable_stdio_uart(PAMI2026a 1)
 
-pico_add_extra_outputs(Mon_Projet)
+pico_add_extra_outputs(PAMI2026a)
 
 add_custom_target(Flash
-    DEPENDS Mon_Projet
+    DEPENDS PAMI2026a
-    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/Mon_Projet.uf2
+    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI2026a.uf2
 )

QEI.c

@@ -17,7 +17,10 @@
 // Nombre d'impulsions par tour de roue : 8000
 // Impulsion / mm : 42,44
 
-#define IMPULSION_PAR_MM_50_1 (12.45f)
+// #define IMPULSION_PAR_MM_50_1 (14.15f)
+// Mesuré sur 1 mètre
+
+#define IMPULSION_PAR_MM_50_1 (14.15f)
 #define IMPULSION_PAR_MM_30_1 (7.47f)
 
 float impulsion_par_mm;
@@ -51,7 +54,7 @@ void QEI_init(int identifiant){
         QEI_est_init=true;
     }
     impulsion_par_mm = IMPULSION_PAR_MM_50_1;
-    if(identifiant == 0 || identifiant >= 4){
+    if(identifiant >= 4){
         impulsion_par_mm = IMPULSION_PAR_MM_30_1;
     }
 

Servomoteur.c

@@ -6,10 +6,7 @@
 
 void Servomoteur_Init(void){
     uint slice_num;
-    gpio_set_function(SERVO5, GPIO_FUNC_PWM);
+    gpio_set_function(SERVO0, GPIO_FUNC_PWM);
-    gpio_set_function(SERVO6, GPIO_FUNC_PWM);
-    gpio_set_function(SERVO7, GPIO_FUNC_PWM);
-    gpio_set_function(SERVO_TURBINE, GPIO_FUNC_PWM);
     
     pwm_config pwm_servo = pwm_get_default_config();
     
@@ -21,22 +18,15 @@ void Servomoteur_Init(void){
     // À la valeur finale, rebouclage à 0 (et non un compte à rebours)
     pwm_config_set_phase_correct(&pwm_servo, false);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO5);
+    slice_num = pwm_gpio_to_slice_num(SERVO0);
     pwm_init(slice_num, &pwm_servo, true);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO_TURBINE);
+    slice_num = pwm_gpio_to_slice_num(SERVO1);
     pwm_init(slice_num, &pwm_servo, true);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO6);
+    Servomoteur_set(SERVO0, SERVO_VALEUR_0_5MS);
-    pwm_init(slice_num, &pwm_servo, true);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO7);
+    Servomoteur_set(SERVO1, SERVO_VALEUR_0_5MS);
-    pwm_init(slice_num, &pwm_servo, true);
-
-    Servomoteur_set(SERVO5, SERVO_VALEUR_1_5MS);
-    Servomoteur_set(SERVO6, SERVO_VALEUR_1_5MS);
-    Servomoteur_set(SERVO7, SERVO_VALEUR_1_5MS);
-    Servomoteur_set(SERVO_TURBINE, SERVO_VALEUR_1MS);
 }
 
 void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
@@ -45,30 +35,3 @@ void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
     //        B ou 1 si la GPIO est impaire
     pwm_set_chan_level(slice_num, servomoteur & 0x01, valeur);
 }
-
-void Moteur_Init(){
-    uint slice_num;
-    gpio_set_function(SERVO8_MOT_PROPULSEUR, GPIO_FUNC_PWM);
-
-    pwm_config pwm_servo = pwm_get_default_config();
-
-    // On veut un rebouclage au bout de 20 ms (50 Hz).
-    // Division de l'horloge système (133 MHz) par 25 : 5230 kHz
-    pwm_config_set_clkdiv_int(&pwm_servo, 25);
-    // Valeur max du PXM pour avoir 5 kHz :  5230 kHz / 5 kHz : 1064
-    pwm_config_set_wrap(&pwm_servo, 1064);
-    // À la valeur finale, rebouclage à 0 (et non un compte à rebours)
-    pwm_config_set_phase_correct(&pwm_servo, false);
-
-    slice_num = pwm_gpio_to_slice_num(SERVO8_MOT_PROPULSEUR);
-    pwm_init(slice_num, &pwm_servo, true);
-
-    Moteur_Set(0);
-
-}
-
-/// @brief Envoie la consigne PWM au moteur.
-/// @param consigne : consigne entre 0 et 1.
-void Moteur_Set(double consigne){
-    Servomoteur_set(SERVO8_MOT_PROPULSEUR, (uint16_t) (1064 * consigne));
-}

Servomoteur.h

@@ -9,17 +9,11 @@
 
 
 // Servomoteurs
-#define SERVO5 4
+#define SERVO0 0
-#define SERVO6 5
+#define SERVO1 1
-#define SERVO7 6
+#define DOIGT_AVANCE 0, SERVO_VALEUR_1_5MS
-#define SERVO8_MOT_PROPULSEUR 15
+#define DOIGT_RETRAIT 0, SERVO_VALEUR_0_5MS
-#define SERVO_TURBINE 7
-
-#define DOIGT_AVANCE 1, SERVO_VALEUR_1_5MS
-#define DOIGT_RETRAIT 1, SERVO_VALEUR_1_5MS
 
 
 void Servomoteur_Init(void);
 void Servomoteur_set(uint8_t servomoteur, uint16_t valeur);
-void Moteur_Init(void);
-void Moteur_Set(double consigne);

Strategie.c

@@ -1,171 +1,331 @@
 #include "Strategie.h"
+#include "Asser_Position.h"
 #include "Geometrie.h"
 #include "Servomoteur.h"
 
 
 
-enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur){
+enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t couleur){
 	static enum{
-		SSS_INIT,
+		NINJA_INIT,
-		SSS_AVANCE,
+		NINJA_GRENIER1,
-		SSS_DANCE
+        NINJA_GRENIER2A,
-	} etat_sss = SSS_INIT;
+        NINJA_GRENIER2A_CALAGE,
+        NINJA_GRENIER2B,
+        NINJA_GRENIER2C,
+        NINJA_GRENIER2C_CALAGE,
+        NINJA_GRENIER2D,
+		NINJA_FIN
+	} etat_sss = NINJA_INIT;
     static struct trajectoire_t trajectoire_composee;
     static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
 
-
-
 	switch(etat_sss){
-		case SSS_INIT:
+		case NINJA_INIT:
             if(couleur == COULEUR_JAUNE){
-                Localisation_set(45, 1895, 0);
+                Localisation_set(645, 1870, 0);
-                Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
+                Trajectoire_bezier(&trajectoire1, 
-                Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
+                    645, 1870,
-                Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
+                    800, 1870,
+                    890, 1730,
+                    990, 1730, 0., 0.);
+                Trajectoire_droite(&trajectoire2, 
+                    990, 1730,
+                    1150, 1730, 0, 0);
+                
             }else{
-                Localisation_set(3000 - 45, 1895, M_PI);
+                Localisation_set(3000 - 45, 1870, M_PI);
-                Trajectoire_droite(&trajectoire1, 3000-45, 1895, 3000- 1135, 1895, M_PI, M_PI);
+                Trajectoire_bezier(&trajectoire1, 
-                Trajectoire_circulaire(&trajectoire2, 3000-1135, 1645, M_PI/2, M_PI, 250, M_PI, -M_PI/2);
+                    3000 - 45, 1870, 3000 - 800, 1870,
-                Trajectoire_droite(&trajectoire3, 3000-1385, 1645, 3000-1385, 1580, 0, 0);
+                    3000 - 890, 1730, 3000 - 990, 1730, 0., 0.);
+                Trajectoire_droite(&trajectoire2, 3000 - 990, 1730, 3000 - 1150, 1730, 0, 0);
             }
 
             Trajectoire_composee_init(&trajectoire_composee);
             Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
             Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
+			etat_sss = NINJA_GRENIER1;
-			etat_sss = SSS_AVANCE;
 			break;
 
-		case SSS_AVANCE:
+		case NINJA_GRENIER1:
-            Trajet_config(TRAJECT_CONFIG_RAPIDE);
+            Trajet_config(TRAJECT_CONFIG_STD);
             if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-                etat_sss = SSS_DANCE;
+                etat_sss = NINJA_GRENIER2A;
+                if(couleur == COULEUR_JAUNE){
+                    Trajectoire_bezier(&trajectoire1, 
+                        1150, 1730,
+                        1030, 1730,
+                        980, 1850,
+                        980, 1970, 0., 0.);
+                }
+                
             }
             break;
 
-        case SSS_DANCE:
+        case NINJA_GRENIER2A:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2A_CALAGE;
+            }
+            break;
+
+        case NINJA_GRENIER2A_CALAGE:
+            if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2B;
+                Localisation_set_y(1975);
+                Localisation_set_x(980);
+                Localisation_set_angle(-M_PI/2);
+                if(couleur == COULEUR_JAUNE){
+                    Trajectoire_bezier(&trajectoire1, 
+                        980, 1975,
+                        980, 1850,
+                        1160, 1925,
+                        1420, 1925, 0., 0.);
+                }
+
+            }
+            break;
+
+        
+        case NINJA_GRENIER2B:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2C;
+                if(couleur == COULEUR_JAUNE){
+                    Trajectoire_bezier(&trajectoire1, 
+                        1420, 1925,
+                        1377, 1925,
+                        1354, 1945,
+                        1354, 1980, 0., 0.);
+                }
+            }
+            break;
+
+        case NINJA_GRENIER2C:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2C_CALAGE;
+            }
+            break;
+
+        case NINJA_GRENIER2C_CALAGE:
+            if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2D;
+                Localisation_set_y(1975);
+                Localisation_set_x(1354);
+                Localisation_set_angle(-M_PI/2);
+                Trajectoire_droite(&trajectoire1, 
+                        1354, 1975,
+                        1354, 1630., 0., 0.);
+            }
+            break;
+
+        case NINJA_GRENIER2D:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = ACTION_TERMINEE;
+            }
+            break;
+
+        case NINJA_FIN:
+            //Asser_Position_maintien();
             return ACTION_TERMINEE;
 	}
     return ACTION_EN_COURS;
 
 }
 
-
+enum etat_action_t Strategie_Ninja_2(uint32_t step_ms, enum couleur_t couleur){
-
-enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur){
 	static enum{
-		SSS_INIT,
+		NINJA_INIT,
-		SSS_AVANCE,
+		NINJA_GRENIER1A,
-		SSS_DANCE
+        NINJA_GRENIER1B,
-	} etat_sss = SSS_INIT;
+        NINJA_GRENIER1B_RECALE,
+        NINJA_GRENIER1C,
+        NINJA_GRENIER1D,
+        NINJA_GRENIER2A,
+        NINJA_GRENIER2A_RECALE,
+        NINJA_GRENIER2B,
+        NINJA_GRENIER2C,
+		NINJA_FIN
+	} etat_sss = NINJA_INIT;
     static struct trajectoire_t trajectoire_composee;
     static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
 
-
-
 	switch(etat_sss){
-		case SSS_INIT:
+		case NINJA_INIT:
-			Localisation_set(45, 1895, 0);
+            if(couleur == COULEUR_JAUNE){
-            Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
+                Localisation_set(645, 1870, 0);
-            Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
+                Trajet_config(TRAJECT_CONFIG_STD);
-            Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
+                Trajectoire_bezier(&trajectoire1, 
-            Trajectoire_composee_init(&trajectoire_composee);
+                    645, 1870,
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
+                    935, 1870,
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
+                    1025, 1920,
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
+                    1315-50, 1920, 0., 0.);
-			etat_sss = SSS_AVANCE;
+                
+            }
+			etat_sss = NINJA_GRENIER1A;
 			break;
 
-		case SSS_AVANCE:
+		case NINJA_GRENIER1A:
-            Trajet_config(TRAJECT_CONFIG_RAPIDE);
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-            if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER1B;
-                etat_sss = SSS_DANCE;
+                if(couleur == COULEUR_JAUNE){
+                    Trajet_config(100,100);
+                    Trajectoire_bezier(&trajectoire1, 
+                        1315-50, 1920,
+                        1270-50, 1920,
+                        1230-50, 1900,
+                        1230-50, 1970, 0., 0.);
+                }
+                
             }
             break;
 
-        case SSS_DANCE:
+        case NINJA_GRENIER1B:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER1B_RECALE;               
+            }
+            break;
+
+        case NINJA_GRENIER1B_RECALE:
+            if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER1C;
+                Trajet_config(100,100);
+                Localisation_set(1230-50, 1975, -M_PI/2);
+                if(couleur == COULEUR_JAUNE){
+                    Trajectoire_bezier(&trajectoire1, 
+                        1230-50, 1970,
+                        1230-50, 1900,
+                        1255, 1725,
+                        1195, 1725, 0., 0.);
+                }
+            }
+            break;
+
+        case NINJA_GRENIER1C:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER1D;
+                if(couleur == COULEUR_JAUNE){
+                    Trajet_config(TRAJECT_CONFIG_STD);
+                    Trajectoire_droite(&trajectoire1, 
+                        1195, 1725,
+                        1050, 1725, 0., 0.);
+                }
+            }
+            break;
+
+        case NINJA_GRENIER1D:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2A;
+                Trajectoire_bezier(&trajectoire1, 
+                        1050, 1725,
+                        1170, 1725, 
+                        1200, 1820,
+                        1200, 1950,
+                        0., 0.);
+            }
+            break;
+
+        case NINJA_GRENIER2A:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2A_RECALE;
+            }
+            break;
+        
+        case NINJA_GRENIER2A_RECALE:
+            if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2B;
+                Trajet_config(100, 100);
+                Localisation_set(1200, 1975, -M_PI/2);
+                if(couleur == COULEUR_JAUNE){
+                    Trajectoire_bezier(&trajectoire1, 
+                        1200, 1975,
+                        1200, 1890,
+                        1135, 1775,
+                        1260, 1775, 0., 0.);
+                }
+            }
+            break;
+
+        case NINJA_GRENIER2B:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_GRENIER2C;
+                Trajet_config(TRAJECT_CONFIG_STD);
+                if(couleur == COULEUR_JAUNE){
+                    Trajectoire_droite(&trajectoire1, 
+                            1260, 1775,
+                            1400, 1775, 
+                            0., 0.);
+                }
+            }
+            break;
+
+        case NINJA_GRENIER2C:
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = NINJA_FIN;
+            }
+            break;
+
+        case NINJA_FIN:
+            //Asser_Position_maintien();
             return ACTION_TERMINEE;
 	}
     return ACTION_EN_COURS;
 
 }
 
-enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur){
+enum etat_action_t Strategie_test(uint32_t step_ms){
-	static enum{
+    static enum{
-		SSS_INIT,
+		TEST_INIT,
-		SSS_AVANCE,
+        TEST_EN_COURS,
-		SSS_DANCE
+        TEST_TERMINE
-	} etat_sss = SSS_INIT;
+	} etat_test = TEST_INIT;
-    static struct trajectoire_t trajectoire_composee;
-    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
 
-
+    switch(etat_test){
-
+        case TEST_INIT:
-	switch(etat_sss){
+            if(Strategie_recale(0, step_ms) == ACTION_TERMINEE){
-		case SSS_INIT:
+                etat_test = TEST_TERMINE;
-			Localisation_set(45, 1895, 0);
-            Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
-            Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
-            Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
-            Trajectoire_composee_init(&trajectoire_composee);
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
-			etat_sss = SSS_AVANCE;
-			break;
-
-		case SSS_AVANCE:
-            Trajet_config(TRAJECT_CONFIG_RAPIDE);
-            if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-                etat_sss = SSS_DANCE;
             }
             break;
 
-        case SSS_DANCE:
+        case TEST_TERMINE:
             return ACTION_TERMINEE;
-	}
+    }
-    return ACTION_EN_COURS;
     
+
+    return ACTION_EN_COURS;
 }
 
-enum etat_action_t Strategie_groupie_3(uint32_t step_ms, enum couleur_t couleur){
+enum etat_action_t Strategie_1_metre(uint32_t step_ms){
-	static enum{
+    static enum{
-		SSS_INIT,
+		TEST_INIT,
-		SSS_AVANCE,
+        TEST_EN_COURS,
-		SSS_DANCE
+        TEST_TERMINE
-	} etat_sss = SSS_INIT;
+	} etat_test = TEST_INIT;
-    static struct trajectoire_t trajectoire_composee;
+    static struct trajectoire_t trajectoire1;
-    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
+    Trajet_config(TRAJECT_CONFIG_STD);
     
 
-
+    switch(etat_test){
-	switch(etat_sss){
+        case TEST_INIT:
-		case SSS_INIT:
+            Localisation_set(0, 0, 0);
-			Localisation_set(45, 1895, 0);
+            Trajectoire_droite(&trajectoire1, 
-            Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
+                        0, 0,
-            Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
+                        1000, 0, 0., 0.);
-            Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
+            etat_test = TEST_EN_COURS;
-            Trajectoire_composee_init(&trajectoire_composee);
+            break;
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
+        case TEST_EN_COURS:
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
+                etat_test = TEST_TERMINE;
-			etat_sss = SSS_AVANCE;
-			break;
-
-		case SSS_AVANCE:
-            Trajet_config(TRAJECT_CONFIG_RAPIDE);
-            if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-                etat_sss = SSS_DANCE;
             }
             break;
 
-        case SSS_DANCE:
+        case TEST_TERMINE:
             return ACTION_TERMINEE;
-	}
+    }
-    return ACTION_EN_COURS;
     
+
+    return ACTION_EN_COURS;
 }
 
+
 void PAMI_dance(int identifiant){
     switch (identifiant)
     {

Strategie.h

@@ -1,3 +1,4 @@
+#include "Asser_Moteurs.h"
 #include "pico/stdlib.h"
 #include "Localisation.h"
 #include "Temps.h"
@@ -12,6 +13,8 @@
 #define TIRETTE 14
 //#define CORR_ANGLE_DEPART_DEGREE  (-1.145)
 #define CORR_ANGLE_DEPART_DEGREE  (0)
+#define RECALE_SENS_AVANT 1
+#define RECALE_SENS_ARRIERE 0
 
 enum etat_action_t{
     ACTION_EN_COURS,
@@ -45,13 +48,15 @@ struct objectif_t{
         ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
 };
 
-enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t);
+enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t);
-enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur);
+enum etat_action_t Strategie_Ninja_2(uint32_t step_ms, enum couleur_t couleur);
-enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur);
+enum etat_action_t Strategie_test(uint32_t step_ms);
-enum etat_action_t Strategie_groupie_3(uint32_t step_ms, enum couleur_t couleur);
+enum etat_action_t Strategie_1_metre(uint32_t step_ms);
+
 void PAMI_dance(int);
 
 enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
+enum etat_action_t Strategie_recale(int sens_avant, uint32_t step_ms);
 
 // STRATEGIE_H
 #endif

Strategie_deplacement.c

@@ -71,3 +71,38 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
     return etat_action;
 
 }
+
+enum etat_action_t Strategie_recale(int sens_avant, uint32_t step_ms){
+    const float vitesse_recale = 200;
+    static int timer_recale_ms;
+    static enum {
+        RECALE_INIT,
+        RECALE_AVANCE,
+    } etat_recale=RECALE_INIT;
+
+    switch(etat_recale){
+        case RECALE_INIT:
+            if(sens_avant){
+                AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_recale);
+                AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_recale);
+            }else{
+                AsserMoteur_setConsigne_mm_s(MOTEUR_A, -vitesse_recale);
+                AsserMoteur_setConsigne_mm_s(MOTEUR_B, -vitesse_recale);
+            }
+            timer_recale_ms = 800;
+            etat_recale=RECALE_AVANCE;
+            break;
+        
+        case RECALE_AVANCE:
+            timer_recale_ms -= step_ms;
+            if(timer_recale_ms <= 0){
+                AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
+                AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
+                etat_recale=RECALE_INIT;
+                return ACTION_TERMINEE;
+            }
+            break;
+    }
+    return ACTION_EN_COURS;
+
+}

Trajet.c

@@ -103,7 +103,7 @@ int Trajet_terminee(float abscisse){
         return 1;
     }*/
     
-    if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
+    if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER && trajet_trajectoire.type != TRAJECTOIRE_COMPOSEE){
         if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.1){
             return 1;
         }

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=0; row<=3; row++){
+		for(row=5; row<8; row++){
 			if(min_distance >  Results.distance_mm[col + 8*row]){
 				min_distance = Results.distance_mm[col + 8*row];
 			}

main.c

@@ -14,6 +14,7 @@
 #include "Localisation.h"
 #include "Moteurs.h"
 #include "Strategie.h"
+#include "Servomoteur.h"
 #include "Temps.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -37,10 +38,10 @@ uint16_t tension_batterie_lire(void);
 
 void identifiant_init(void);
 uint get_identifiant(void);
-
 int get_tirette(void);
 int get_couleur(void);
 
+void gestion_PAMI(uint32_t step_ms, int * asser_pos);
 void gestion_VL53L8CX(void);
 
 const uint32_t step_ms=1;
@@ -49,7 +50,6 @@ float distance1_mm=0, distance2_mm=0;
 // DEBUG
 extern float abscisse; 
 extern struct point_xyo_t point;
-float vitesse;
 VL53L8CX_Configuration Dev;
 
 void main(void)
@@ -58,7 +58,6 @@ void main(void)
 	bool fin_match = false;
 
 
-
     stdio_init_all();
 	Temps_init();
 
@@ -67,10 +66,13 @@ void main(void)
 	Trajet_init(get_identifiant());
 	i2c_maitre_init();
 
+	Servomoteur_Init();
+
 	
 
 	uint32_t temps_ms = Temps_get_temps_ms();
 	uint32_t temps_depart_ms;
+	int asser_pos=1;
 	struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
 	float vitesse_mm_s=100;
 	
@@ -81,63 +83,40 @@ void main(void)
 
 	gpio_init(TIRETTE_PIN);
 	gpio_set_dir(LED1PIN, GPIO_IN);
+	gpio_pull_up(TIRETTE_PIN);
+	
 
 	
 	
-	multicore_launch_core1(gestion_affichage);
 	// TODO: A remettre - quand on aura récupéré un capteur
-	//multicore_launch_core1(gestion_VL53L8CX);
+	multicore_launch_core1(gestion_VL53L8CX);
+	
 
-	sleep_ms(85000);
 	printf("Demarrage...\n");
 	
 
-	float vitesse_init =300;
-	vitesse = vitesse_init;
-
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
-	while(get_tirette());
-	gpio_put(LED1PIN, 0);
-
-	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
-	if(get_identifiant() == 3){
-		sleep_ms(90000);
-	}
-
-	temps_depart_ms = Temps_get_temps_ms();
-
 	while(1){
 		
 		// Fin du match 
-		if((Temps_get_temps_ms() -temps_depart_ms) >15000 || (fin_match == 1)){
-			Moteur_Stop();
-			while(1){
-
-			}
-		}
 		if(temps_ms != Temps_get_temps_ms()){
 			temps_ms = Temps_get_temps_ms();
 			if(temps_ms % step_ms == 0){
 				QEI_update();
 				Localisation_gestion();
 				
-				if(Strategie_super_star(step_ms, COULEUR_JAUNE) == ACTION_TERMINEE){
+				gestion_PAMI(step_ms, &asser_pos);
-					Asser_Position_maintien();
+				if(asser_pos){
-					if(Asser_Position_panic_angle()){
+					AsserMoteur_Gestion(step_ms);
-						fin_match=1;
-					}
 				}
-				AsserMoteur_Gestion(step_ms);
 			}
 		}
 		
-		
-		
 	}
 }
 
-void gestion_PAMI(uint32_t step_ms){
+void gestion_PAMI(uint32_t step_ms, int * asser_pos){
 	static enum{
 		PAMI_ATTENTE_TIRETTE,
 		PAMI_ATTENTE_TEMPO,
@@ -146,6 +125,24 @@ void gestion_PAMI(uint32_t step_ms){
 		PAMI_DANCE,
 	}etat_PAMI;
 	static uint32_t temps_tirette, temps_mouvement;
+	enum etat_action_t etat_action=ACTION_EN_COURS;
+
+	// Identifiant 0 : PAMI Ninja
+	// Tempo Demarrage : 0 s
+	// Tempo action : 100 s
+	uint32_t tempo_demarrage_ms, tempo_action_ms;
+	if(get_identifiant() == 0){
+		tempo_demarrage_ms = 2000;
+		tempo_action_ms = 980000;
+	}else{
+
+	// Autres
+	// Tempo Demarrage : 85 s
+	// Tempo action : 15 s
+		tempo_demarrage_ms = 85000;
+		tempo_action_ms = 15000;
+
+	}
 
 	switch (etat_PAMI)
 	{
@@ -160,7 +157,7 @@ void gestion_PAMI(uint32_t step_ms){
 		if(get_tirette() == 1 && (Temps_get_temps_ms() - temps_tirette > 1000)){
 			etat_PAMI = PAMI_FIN_TEMPO_MANUELLE;
 		}
-		if (Temps_get_temps_ms() - temps_tirette > 85000){
+		if (Temps_get_temps_ms() - temps_tirette > tempo_demarrage_ms){
 			etat_PAMI = PAMI_TRAJECTOIRE;
 			temps_mouvement = Temps_get_temps_ms();
 		}
@@ -174,21 +171,33 @@ void gestion_PAMI(uint32_t step_ms){
 		break;
 
 	case PAMI_TRAJECTOIRE:
-		switch (get_identifiant())
+		if(Temps_get_temps_ms() - temps_mouvement > tempo_action_ms){
-		{
+			etat_PAMI = PAMI_DANCE;
-			case 0: Strategie_super_star(step_ms, get_couleur()); break;
+		}
-			case 1: Strategie_groupie_1(step_ms, get_couleur()); break;
+		if(etat_action == ACTION_EN_COURS){
-			case 2: Strategie_groupie_2(step_ms, get_couleur()); break;
+			switch (get_identifiant())
-			case 3: Strategie_groupie_3(step_ms, get_couleur()); break;
+			{
-			default: break;
+				case 0: etat_action = Strategie_Ninja_2(step_ms, get_couleur()); break;
+				case 1: etat_action = Strategie_1_metre(step_ms); break;
+				case 7: etat_action = Strategie_test(step_ms); break;
+				default: etat_action = ACTION_TERMINEE; break;
+			}
+			if(etat_action == ACTION_TERMINEE){
+				Moteur_Stop();
+				*asser_pos = 0;
+			}
 		}
 		break;
 
 	case PAMI_DANCE:
 		Moteur_Stop();
-		while(1){
+		*asser_pos = 0;
-			PAMI_dance(get_identifiant());
+		if(Temps_get_temps_ms() - temps_tirette > 15000){
+			while(1){
+				PAMI_dance(get_identifiant());
+			}
 		}
+		break;
 	
 	default:
 		break;
@@ -210,7 +219,7 @@ void gestion_VL53L8CX(void){
 		if(isReady){
 			VL53L8_lecture( &Dev, &Results);
 			VL53L8_min_distance(Results, &distance_obstacle);
-			Trajet_set_obstacle_mm(distance_obstacle);
+			//Trajet_set_obstacle_mm(distance_obstacle);
 		}
 		affichage();
 	}
@@ -234,7 +243,7 @@ void affichage(void){
 	printf(">pos_angle:%.1f\n", Localisation_get().angle_radian);
 	printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
 
-	printf(">abscisse:%f\n",abscisse);
+	printf(">abscisse:%lu:%f\n",temps_ms, abscisse);
 	
 	struct position_t position_actuelle;
 	position_actuelle = Localisation_get();
@@ -242,7 +251,7 @@ void affichage(void){
 	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>Tirette:%d\n", get_couleur(), get_identifiant(), get_tirette());
+	printf(">couleur:%lu:%d\n>id:%lu:%d\n>Tirette:%lu:%d\n", temps_ms, get_couleur(),temps_ms,  get_identifiant(),temps_ms, get_tirette());
 }
 
 void tension_batterie_init(void){