Réception des premiers message pour piloter la LED par USB avec le format de messagerie : Fonctionnel (voir notes du Readme.md)

.vscode/c_cpp_properties.json

@@ -2,6 +2,7 @@
     "env": {
         "myDefaultIncludePath": [
             "${workspaceFolder}",
+            "${workspaceFolder}/VL53L8CX_ULD_API/inc",
             "${workspaceFolder}/build",
             "${env:PICO_SDK_PATH}/src/**/include",
             "${env:PICO_SDK_PATH}/src/common/pico_base/include",

.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"
     }
 }

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

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

Asser_Position.c

@@ -48,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

CMakeLists.txt

@@ -14,18 +14,22 @@ add_executable(Mon_Projet
   Asser_Position.c
   Asser_Moteurs.c
   Commande_vitesse.c
+  communication.c
   Evitement.c
   Geometrie.c
   i2c_maitre.c
+  messagerie.c
   Moteurs.c
   Localisation.c
   main.c
   QEI.c
   Strategie_deplacement.c
   Strategie.c
+  Servomoteur.c
   Temps.c
   Trajectoire_bezier.c
   Trajectoire_circulaire.c
+  Trajectoire_composees.c
   Trajectoire_droite.c
   Trajectoire.c
   Trajet.c
@@ -59,3 +63,7 @@ add_custom_target(Flash
     DEPENDS Mon_Projet
     COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/Mon_Projet.uf2
 )
+
+# Suppression de la conversion automatique LF -> CRLF
+add_definitions(-DPICO_STDIO_ENABLE_CRLF_SUPPORT=0)
+

Readme.md

@@ -1,18 +1,19 @@
-PAMI 2024 - Poivron Robotique
+Propulsion 2026 - Riombotique
 =======================================
 
-Code du PAMI 2024 de l'équipe Poivron Robotique.
+Basé sur le code du PAMI 2025 de l'équipe Poivron Robotique.
 
-La carte contient les éléments suivants :
+Principales évolutions: 
 
-*    Microcontrôleur Raspberry Pi Pico
+- Ajout de la communication USB
-*    Connecteur pour l’arrêt d’urgence
+
-*    2 prises moteurs (pilotés par un L293D)
+Méthode de test
-*    2 prises codeurs
+---------------
-*    1 prise Gyroscope (L3GD20H)
+Éteindre la LED avec la trame : `echo -e "\xFF\xFF\x06rD\x00\x01\x36\x00" > /dev/ttyACM0`
-*    1 prise I2C pour du TOF
+Allumer la LED avec la trame : `echo -e "\xFF\xFF\x06rD\x00\x01\x06\x00" > /dev/ttyACM0`
-*    1 prise "choix couleur"
+
-*    1 prise tirette
+Difficultés
-*    Surveillance tension batterie
+-----------
-*    1 LED
+Il suffit d'une petite modification du code (ajout d'un `printf` par exemple), pour que la communication se bloque côté PC 
-*    3 Dip Switch
+L'utilisation du second cœur pour envoyer la télémétrie peut perturber le débug avec printf => Augment grandement le délai de réponse 
+La communication crash rapidement : à cause de la danse du PAMI => Suppression de la dance du PAMI (c'était codé à l'arrache)

Servomoteur.c

@@ -0,0 +1,37 @@
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "Servomoteur.h"
+
+
+
+void Servomoteur_Init(void){
+    uint slice_num;
+    gpio_set_function(SERVO0, 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 254 : 523 kHz
+    pwm_config_set_clkdiv_int(&pwm_servo, 254);
+    // Valeur max du PXM pour avoir 50 Hz :  523 kHz / 50 Hz : 10460
+    pwm_config_set_wrap(&pwm_servo, 10460);
+    // À 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(SERVO0);
+    pwm_init(slice_num, &pwm_servo, true);
+
+    slice_num = pwm_gpio_to_slice_num(SERVO1);
+    pwm_init(slice_num, &pwm_servo, true);
+
+    Servomoteur_set(SERVO0, SERVO_VALEUR_0_5MS);
+
+    Servomoteur_set(SERVO1, SERVO_VALEUR_0_5MS);
+}
+
+void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
+    uint slice_num = pwm_gpio_to_slice_num(servomoteur);
+    // Chan : A ou 0 si la GPIO est paire
+    //        B ou 1 si la GPIO est impaire
+    pwm_set_chan_level(slice_num, servomoteur & 0x01, valeur);
+}

Servomoteur.h

@@ -0,0 +1,19 @@
+#include "pico/stdlib.h"
+
+// Valeur des servomoteurs
+#define SERVO_VALEUR_MAX 10459
+#define SERVO_VALEUR_2MS 1056
+#define SERVO_VALEUR_1_5MS 784
+#define SERVO_VALEUR_1MS 523
+#define SERVO_VALEUR_0_5MS 261
+
+
+// Servomoteurs
+#define SERVO0 0
+#define SERVO1 1
+#define DOIGT_AVANCE 0, SERVO_VALEUR_1_5MS
+#define DOIGT_RETRAIT 0, SERVO_VALEUR_0_5MS
+
+
+void Servomoteur_Init(void);
+void Servomoteur_set(uint8_t servomoteur, uint16_t valeur);

Strategie.c

@@ -1,51 +1,90 @@
 #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 etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur){
 	static enum{
-        SSS_TEST_TOURNE,
 		SSS_INIT,
-		SSS_AVANCE_1,
+		SSS_AVANCE,
-		SSS_TOURNE,
+		SSS_FIN
-		SSS_AVANCE_2,
+	} etat_sss = SSS_INIT;
-		SSS_DANCE
+    static struct trajectoire_t trajectoire_composee;
-	} etat_sss = SSS_TEST_TOURNE;
+    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
-    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);
+            if(couleur == COULEUR_JAUNE){
-			etat_sss = SSS_AVANCE_1;
+                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);
+            }else{
+                Localisation_set(3000 - 45, 1895, M_PI);
+                Trajectoire_droite(&trajectoire1, 3000-45, 1895, 3000- 1135, 1895, M_PI, M_PI);
+                Trajectoire_circulaire(&trajectoire2, 3000-1135, 1645, M_PI/2, M_PI, 250, M_PI, -M_PI/2);
+                Trajectoire_droite(&trajectoire3, 3000-1385, 1645, 3000-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_1:
+		case SSS_AVANCE:
-            Trajet_config(TRAJECT_CONFIG_STD);
+            Trajet_config(TRAJECT_CONFIG_RAPIDE);
-            Trajectoire_droite(&trajectoire, 45, 1895, 1135, 1895, 0, 0);
+            if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = SSS_FIN;
-                //etat_sss = SSS_TOURNE;
-                etat_sss = SSS_DANCE;
             }
             break;
 
-        case SSS_TOURNE:
+        case SSS_FIN:
-            Trajet_config(100, 100);
+            Asser_Position_maintien();
-            Trajectoire_circulaire(&trajectoire, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
+            return ACTION_TERMINEE;
-            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+	}
-                //etat_sss = SSS_AVANCE_2;
+    return ACTION_EN_COURS;
-                etat_sss = SSS_DANCE;
-            }
-            break;
 
-        case SSS_AVANCE_2:
+}
-            Trajet_config(TRAJECT_CONFIG_STD);
+
-            Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1600, 0, 0);
+
-            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+
+enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur){
+	static enum{
+		SSS_INIT,
+		SSS_AVANCE,
+		SSS_DANCE
+	} etat_sss = SSS_INIT;
+    static struct trajectoire_t trajectoire_composee;
+    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
+
+
+
+	switch(etat_sss){
+		case SSS_INIT:
+            if(couleur == COULEUR_JAUNE){
+                Localisation_set(45, 1780, 0);
+                Trajectoire_bezier(&trajectoire1, 45, 1780, 480, 1780, 710, 1342, 960, 1410, 0, 0);
+                Trajectoire_bezier(&trajectoire2, 960, 1410, 1014, 1424, 1053, 1432, 1092, 1460, 0, 0);
+            }else{
+                Localisation_set(3000 - 45, 1780, M_PI);
+                Trajectoire_bezier(&trajectoire1, 3000 - 45, 1780, 3000 - 480, 1780, 3000 - 710, 1342, 3000 - 960, 1410, 0, 0);
+                Trajectoire_bezier(&trajectoire2, 3000 - 960, 1410, 3000 - 1014, 1424, 3000 - 1053, 1432, 3000 - 1092, 1460, 0, 0);    
+            }
+            Trajectoire_composee_init(&trajectoire_composee);
+            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
+            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
+			etat_sss = SSS_AVANCE;
+			break;
+
+		case SSS_AVANCE:
+            Trajet_config(TRAJECT_CONFIG_RAPIDE);
+            if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_PAUSE_DEVANT_OBSTACLE) == ACTION_TERMINEE){
                 etat_sss = SSS_DANCE;
             }
             break;
@@ -56,3 +95,130 @@ enum etat_action_t Strategie_super_star(uint32_t step_ms){
     return ACTION_EN_COURS;
 
 }
+
+enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur){
+	static enum{
+		SSS_INIT,
+		SSS_AVANCE,
+		SSS_DANCE
+	} etat_sss = SSS_INIT;
+    static struct trajectoire_t trajectoire_composee;
+    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
+
+
+
+	switch(etat_sss){
+		case SSS_INIT:
+            if(couleur == COULEUR_JAUNE){
+                Localisation_set(45, 1600, 0);
+                Trajectoire_bezier(&trajectoire1, 45, 1600, 326, 1600, 935, 980, 1500, 1380, 0, 0);
+            }else{
+                Localisation_set(3000 - 45, 1600, M_PI);
+                Trajectoire_bezier(&trajectoire1, 3000 - 45, 1600, 3000 - 326, 1600, 3000 - 935, 980, 3000 - 1500, 1380, 0, 0);
+            }
+			etat_sss = SSS_AVANCE;
+			break;
+
+		case SSS_AVANCE:
+            Trajet_config(TRAJECT_CONFIG_RAPIDE);
+            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_PAUSE_DEVANT_OBSTACLE) == ACTION_TERMINEE){
+                etat_sss = SSS_DANCE;
+            }
+            break;
+
+        case SSS_DANCE:
+            return ACTION_TERMINEE;
+	}
+    return ACTION_EN_COURS;
+
+}
+
+enum etat_action_t Strategie_groupie_3(uint32_t step_ms, enum couleur_t couleur){
+	static enum{
+		SSS_INIT,
+		SSS_AVANCE,
+		SSS_DANCE
+	} etat_sss = SSS_INIT;
+    static struct trajectoire_t trajectoire_composee;
+    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
+
+
+
+	switch(etat_sss){
+		case SSS_INIT:
+			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:
+            return ACTION_TERMINEE;
+	}
+    return ACTION_EN_COURS;
+
+}
+
+void PAMI_dance(int identifiant){
+    switch (identifiant)
+    {
+    case 0:
+        sleep_ms(500);
+        Servomoteur_set(DOIGT_AVANCE);
+        sleep_ms(250);
+        Servomoteur_set(DOIGT_RETRAIT);
+        sleep_ms(250); // t = 1,0 s
+        sleep_ms(750); // t = 1,75 s
+        Servomoteur_set(DOIGT_AVANCE);
+        sleep_ms(250);
+        Servomoteur_set(DOIGT_RETRAIT);
+        sleep_ms(250); // t = 2,25 s
+        sleep_ms(950); // t = 3,2 s
+        for(int i =0; i<3;i++){
+            Servomoteur_set(DOIGT_AVANCE);
+            sleep_ms(200);
+            Servomoteur_set(DOIGT_RETRAIT);
+            sleep_ms(200);
+        }// t = 4,4 s
+        sleep_ms(1400); // t = 5,8
+        break;
+    case 1:
+    case 2:
+    case 3:
+        sleep_ms(1100); // t = 1,1 s
+        Servomoteur_set(DOIGT_AVANCE);
+        sleep_ms(250);
+        Servomoteur_set(DOIGT_RETRAIT);
+        sleep_ms(250); // t = 1,6 s
+        sleep_ms(800); // t = 2,4 s
+        Servomoteur_set(DOIGT_AVANCE);
+        sleep_ms(250);
+        Servomoteur_set(DOIGT_RETRAIT);
+        sleep_ms(250); // t = 2,9 s
+        sleep_ms(1600); // t = 4,5 s
+        for(int i =0; i<3;i++){
+            Servomoteur_set(DOIGT_AVANCE);
+            sleep_ms(150);
+            Servomoteur_set(DOIGT_RETRAIT);
+            sleep_ms(150);
+        }// t = 5,4 s
+        sleep_ms(400); // t = 5,8 s
+        break;
+    
+    default:
+        break;
+    }
+
+}

Strategie.h

@@ -1,3 +1,4 @@
+#include "Asser_Moteurs.h"
 #include "pico/stdlib.h"
 #include "Localisation.h"
 #include "Temps.h"
@@ -45,7 +46,11 @@ 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 etat_action_t Strategie_super_star(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_groupie_2(uint32_t step_ms, enum couleur_t couleur);
+enum etat_action_t Strategie_groupie_3(uint32_t step_ms, enum couleur_t couleur);
+void PAMI_dance(int);
 
 enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
 

Trajectoire.c

@@ -2,15 +2,15 @@
 #include "Trajectoire_bezier.h"
 #include "Trajectoire_circulaire.h"
 #include "Trajectoire_droite.h"
+#include "Trajectoire_composees.h"
 
 #include "math.h"
 
-#define NB_MAX_TRAJECTOIRES 5
 #define PRECISION_ABSCISSE 0.001f
 
 
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad, float rayon,
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad,
-                        float orientation_debut_rad, float orientation_fin_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;
@@ -34,8 +34,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1
     trajectoire->orientation_fin_rad = 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,
+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 orientation_debut_rad, float orientation_fin_rad){
+                        float p4_x, float p4_y, float orientation_debut_rad, float orientation_fin_rad){
     trajectoire->type = TRAJECTOIRE_BEZIER;
     trajectoire->p1.x = p1_x;
     trajectoire->p1.y = p1_y;
@@ -50,6 +50,27 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1
     trajectoire->orientation_fin_rad = orientation_fin_rad;
 }
 
+/// @brief Initialise une trajectoire composée
+/// @param trajectoire : trajectoire à initialiser
+void Trajectoire_composee_init(struct trajectoire_t * trajectoire){
+    trajectoire->type = TRAJECTOIRE_COMPOSEE;
+    trajectoire->longueur = -1;
+    trajectoire->nb_trajectoire = 0;
+    for (int index =0; index <NB_MAX_TRAJECTOIRE ; index++){
+        trajectoire->trajectoires[index] = NULL;
+    }
+}
+
+/// @brief Ajoute une trajectoire à une trajectoire composée déjà initialisée
+/// @param trajectoire_composee 
+/// @param trajectoire 
+void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire){
+    if(trajectoire_composee->nb_trajectoire < NB_MAX_TRAJECTOIRE){
+        trajectoire_composee->trajectoires[trajectoire_composee->nb_trajectoire] = trajectoire;
+        trajectoire_composee->nb_trajectoire++;
+    }
+}
+
 void Trajectoire_inverse(struct trajectoire_t * trajectoire){
     struct trajectoire_t old_trajectoire;
     old_trajectoire = *trajectoire;
@@ -97,6 +118,10 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
             case TRAJECTOIRE_BEZIER:
                 Trajectoire_bezier_get_longueur(trajectoire);
                 break;
+
+            case TRAJECTOIRE_COMPOSEE:
+                Trajectoire_composee_get_longueur(trajectoire);
+                break;
         }
     }
     return trajectoire->longueur;
@@ -123,6 +148,10 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
             point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
             break;
 
+        case TRAJECTOIRE_COMPOSEE:
+           point_xyo = Trajectoire_composee_get_point(trajectoire, abscisse);
+           break;
+
     }
     return point_xyo;    
 }

Trajectoire.h

@@ -1,10 +1,13 @@
 #ifndef TRAJECTOIRE_H
 #define TRAJECTOIRE_H
 
+#define NB_MAX_TRAJECTOIRE 10
+
 enum trajectoire_type_t{
     TRAJECTOIRE_DROITE,
     TRAJECTOIRE_CIRCULAIRE,
     TRAJECTOIRE_BEZIER,
+    TRAJECTOIRE_COMPOSEE
 };
 
 struct point_xy_t{
@@ -22,6 +25,9 @@ struct trajectoire_t {
     float orientation_debut_rad, orientation_fin_rad;
     float rayon, angle_debut_rad, angle_fin_rad;
     float longueur;
+    // Pour les trajectoires composées
+    struct trajectoire_t * trajectoires[NB_MAX_TRAJECTOIRE];
+    int nb_trajectoire;
 };
 
 float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
@@ -35,6 +41,9 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1
 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,
                         float orientation_debut_rad, float orientation_fin_rad);
 void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad);
+void Trajectoire_composee_init(struct trajectoire_t * trajectoire);
+void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire);
+
 void Trajectoire_inverse(struct trajectoire_t * trajectoire);
 
 #endif

Trajectoire_composees.c

@@ -0,0 +1,24 @@
+#include <math.h>
+#include "Trajectoire.h"
+#include "Trajectoire_composees.h"
+
+void Trajectoire_composee_get_longueur(struct trajectoire_t * trajectoire_composee){
+    trajectoire_composee->longueur = 0;
+    for(unsigned int i = 0 ; i<trajectoire_composee->nb_trajectoire ; i++) {
+        trajectoire_composee->longueur += Trajectoire_get_longueur_mm(trajectoire_composee->trajectoires[i]);
+    }
+}
+
+
+struct point_xyo_t Trajectoire_composee_get_point(struct trajectoire_t * trajectoire_composee, double abscisse){
+    // Obtenir la trajectoire sur laquelle nous sommes...
+    int index_trajectoire;
+    double abscisse_etendue, abscisse_locale;
+
+    abscisse_etendue = abscisse * trajectoire_composee->nb_trajectoire;
+    index_trajectoire = (int) floor(abscisse_etendue);
+    abscisse_locale = abscisse_etendue - floor(abscisse_etendue);
+
+    return Trajectoire_get_point(trajectoire_composee->trajectoires[index_trajectoire], abscisse_locale);
+}
+

Trajectoire_composees.h

@@ -0,0 +1,3 @@
+
+struct point_xyo_t Trajectoire_composee_get_point(struct trajectoire_t * trajectoire_composee, double abscisse);
+void Trajectoire_composee_get_longueur(struct trajectoire_t * trajectoire_composee);

communication.c

@@ -0,0 +1,151 @@
+#include "pico/error.h"
+#include <stdio.h>
+#include "communication.h"
+#include "messagerie.h"
+#include "tusb.h"
+
+#define TAMPON_TAILLE 1020
+
+struct com_reception_buffer_t{
+    char tampon[TAMPON_TAILLE];     // Tampon tournant - 1er niveau de tampon
+    unsigned int index_tampon_ecriture, index_tampon_lecture;
+};
+
+struct com_reception_buffer_t com_reception_buffer;
+
+
+void communication_init(){
+    com_reception_buffer.index_tampon_ecriture=0;
+    com_reception_buffer.index_tampon_lecture=0;
+}
+
+/// @brief Incrémente l'index du tampon tournant
+/// @param index 
+/// @return 
+void increment_index(unsigned int *index){
+    *index = 1 + *index ;
+    if((*index) >= TAMPON_TAILLE){
+        *index = 0;
+    }
+}
+
+/// @brief augmente la position l'index du tampon tournant
+/// @param index 
+/// @return 
+void augmente_index(unsigned int *index, unsigned int offset){
+    *index = offset + *index ;
+    if((*index) >= TAMPON_TAILLE){
+        *index -= TAMPON_TAILLE;
+    }
+}
+
+/// @brief Vérifie si des caractères ont été reçu par la liaison série
+/// et analyse si un message valide a été reçu.
+void communication_reception_message(){
+    int input_char;
+    int chaîne_octets_reçus[TAMPON_TAILLE];
+    unsigned int index_chaine_recue;
+    unsigned int index_tampon;
+    struct message_t message;
+    // Si un caractère est reçu, ajout du caractère au tampon tournant
+
+    /// TODO: Tester le code suivant à la place de while get_char();
+    /*
+    char nb_recu=0;
+    if(tud_cdc_available()){
+        char tampon_usb[128];
+        nb_recu = tud_cdc_read(tampon_usb, 128);
+        // copie dans le tampon tournant
+        for(int i=0; i<nb_recu; i++){
+            input_char = tampon_usb[i];
+            com_reception_buffer.index_tampon_ecriture++;
+            if(com_reception_buffer.index_tampon_ecriture >= TAMPON_TAILLE){
+                com_reception_buffer.index_tampon_ecriture = 0;
+            }
+            com_reception_buffer.tampon[com_reception_buffer.index_tampon_ecriture] = input_char;
+        }
+    }*/
+    
+    input_char = stdio_getchar_timeout_us(0);
+    while(input_char != PICO_ERROR_TIMEOUT){
+        com_reception_buffer.index_tampon_ecriture++;
+        if(com_reception_buffer.index_tampon_ecriture >= TAMPON_TAILLE){
+            com_reception_buffer.index_tampon_ecriture = 0;
+        }
+        com_reception_buffer.tampon[com_reception_buffer.index_tampon_ecriture] = input_char;
+
+        // Caractère suivant ?
+        input_char = stdio_getchar_timeout_us(0);
+    }
+
+    // Copie du tampon tournant dans une chaine
+    // Parce que c'est plus simple à traiter
+    index_chaine_recue = 0;
+    index_tampon = com_reception_buffer.index_tampon_lecture;
+    if(index_tampon != com_reception_buffer.index_tampon_ecriture){
+        while(index_tampon != com_reception_buffer.index_tampon_ecriture){
+            chaîne_octets_reçus[index_chaine_recue] = com_reception_buffer.tampon[index_tampon];
+            index_chaine_recue++;
+            increment_index(&index_tampon);
+        }
+        chaîne_octets_reçus[index_chaine_recue] = com_reception_buffer.tampon[index_tampon];
+        index_chaine_recue++;
+        increment_index(&index_tampon);
+    }
+
+    // Traitement 
+    // Si on trouve le début du message
+    // Si on trouve la taille du message
+    // Si le caractère de fin est bien à la fin du message
+    int fin_message = 0;
+    for(int i=0; i<index_chaine_recue; i++){
+        int index_fin_message = 0;
+        // Com v2
+        // OxFF OxFF <taille> ... ... <fin (0x00)>
+        if(i + 2 < index_chaine_recue){
+            // Test début message
+            if(chaîne_octets_reçus[i] == 0xFF && chaîne_octets_reçus[i+1] == 0xFF){
+                //printf("Debut message: 0xFF 0xFF\n");
+                // Taille du message (sans l'entête)
+                uint8_t offset = chaîne_octets_reçus[i+2];
+                if(i + 2 + offset < index_chaine_recue){
+                    // printf("Message totalement reçu, taille %d\n", offset);
+                    // Test fin message
+                    if(chaîne_octets_reçus[i + 2 + offset] == 0x00){
+                        // printf("Fin message OK\n");
+                        // Lecture du message
+                        message.type = 'b'; // Message binaire (par opposition à un message Texte)
+                        message.taille_donnees = offset - 1;
+                        for(int index_donnees = 0; index_donnees < message.taille_donnees; index_donnees++){
+                            message.donnees[index_donnees] = chaîne_octets_reçus[index_donnees + i + 3];
+                        }
+                        fin_message = i + 2 + offset;
+                        messagerie_put_message(message);
+                    }else{
+                        // printf("Fin message NOK, attendu 0x00, recu %x\n", chaîne_octets_reçus[i + 2 + offset]);
+                    }
+                }
+            }
+        }
+        if(chaîne_octets_reçus[i] == '>'){
+            message.type = chaîne_octets_reçus[i];
+            message.taille_donnees=0;
+            while(message.taille_donnees + i  < index_chaine_recue){
+                message.donnees[message.taille_donnees] = chaîne_octets_reçus[i+message.taille_donnees];
+                if(message.donnees[message.taille_donnees] == '\n'){
+                    i = i + message.taille_donnees; 
+                    message.taille_donnees++;
+                    message.donnees[message.taille_donnees] = '\0';
+                    messagerie_put_message  (message);
+                    fin_message = i;
+                    break;
+                }
+                message.taille_donnees++;
+            }
+        }
+    }
+
+    // Mettre à jour l'index de lecture du tampon tournant
+    augmente_index(&(com_reception_buffer.index_tampon_lecture), fin_message);
+
+}

communication.h

@@ -0,0 +1,3 @@
+
+void communication_reception_message(void);
+void communication_init(void);

main.c

@@ -9,11 +9,14 @@
 #include "hardware/pwm.h"
 #include "Asser_Position.h"
 #include "Asser_Moteurs.h"
+#include "communication.h"
 #include "Commande_vitesse.h"
 #include "i2c_maitre.h"
 #include "Localisation.h"
+#include "messagerie.h"
 #include "Moteurs.h"
 #include "Strategie.h"
+#include "Servomoteur.h"
 #include "Temps.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -27,8 +30,8 @@
 #define TIRETTE_PIN 6
 #define COULEUR_PIN 4
 
-#define COULEUR_BLEU 1
+#define COULEUR_BLEU 0
-#define COULEUR_JAUNE 0
+#define COULEUR_JAUNE 1
 
 void affichage(void);
 void gestion_affichage(void);
@@ -36,11 +39,11 @@ void tension_batterie_init(void);
 uint16_t tension_batterie_lire(void);
 
 void identifiant_init(void);
-uint identifiant_lire(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,28 +52,33 @@ 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)
 {
 	VL53L8CX_ResultsData Results;
+	struct message_t message;
 	bool fin_match = false;
 
 
-
     stdio_init_all();
 	Temps_init();
 
 	identifiant_init();
-	Localisation_init(identifiant_lire());
+	Localisation_init(get_identifiant());
-	Trajet_init(identifiant_lire());
+	Trajet_init(get_identifiant());
-	i2c_maitre_init();
+	//i2c_maitre_init();
+
+	Servomoteur_Init();
+
+	communication_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;
 	
@@ -79,57 +87,158 @@ void main(void)
 	gpio_set_dir(LED1PIN, GPIO_OUT );
 	gpio_put(LED1PIN, 1);
 
+	gpio_init(PICO_DEFAULT_LED_PIN);
+	gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT );
+	gpio_put(PICO_DEFAULT_LED_PIN, 1);
+
+	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);
 	sleep_ms(5000);
+
 	printf("Demarrage...\n");
 	
 	
-	float vitesse_init =300;
+	// TODO: A remettre - quand on aura récupéré un capteur
-	vitesse = vitesse_init;
+	if(get_identifiant() != 0){
+		//multicore_launch_core1(gestion_VL53L8CX);
+		multicore_launch_core1(gestion_affichage);
+	}else{
+		multicore_launch_core1(gestion_affichage);
+	}
+
+
+	
 
 	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(identifiant_lire() == 3){
-		sleep_ms(90000);
-	}
-
-	temps_depart_ms = Temps_get_temps_ms();
-
 	while(1){
+
+		
+		communication_reception_message();
+		
+		if(messagerie_message_disponible()){
+			uint8_t id_carte, registre;
+			while(messagerie_message_disponible()){
+				message = messagerie_get_message();
+
+				if(message.type == 'b'){
+					switch(message.donnees[0]){
+						case 'r': // réception de données
+							// Reception de données
+							// données[0]: commande ('r' ici)
+							// données[1]: id de la carte
+							// données[2]: 1er registre
+							// données[3 à tailles_données -1]: valeurs
+							id_carte = message.donnees[1];
+							registre = message.donnees[2];
+							if(id_carte == 'D'){
+								if(message.donnees[4] > 0 && message.donnees[4] < 30){
+									printf("LED ON\n");
+									gpio_put(PICO_DEFAULT_LED_PIN, 1);
+								}else{
+									printf("LED OFF\n");
+									gpio_put(PICO_DEFAULT_LED_PIN, 0);
+								}
+							}
+							break;
+					}
+				}
+			}
+			//printf(">temps_reception:%lld\n", current_time_us - start_time_us);
+			//printf(">nb_message:%u\n",nb_message);
+		}
 		
 		// Fin du match 
-		if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (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) == 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, int * asser_pos){
+	static enum{
+		PAMI_ATTENTE_TIRETTE,
+		PAMI_ATTENTE_TEMPO,
+		PAMI_FIN_TEMPO_MANUELLE,
+		PAMI_TRAJECTOIRE,
+		PAMI_DANCE,
+	}etat_PAMI;
+	static uint32_t temps_tirette, temps_mouvement;
+	enum etat_action_t etat_action;
+
+	switch (etat_PAMI)
+	{
+	case PAMI_ATTENTE_TIRETTE:
+		if(get_tirette() == 0){
+			etat_PAMI = PAMI_ATTENTE_TEMPO;
+			temps_tirette = Temps_get_temps_ms();
+		}
+		break;
+
+	case PAMI_ATTENTE_TEMPO:
+		if(get_tirette() == 1 && (Temps_get_temps_ms() - temps_tirette > 1000)){
+			etat_PAMI = PAMI_FIN_TEMPO_MANUELLE;
+		}
+		if (Temps_get_temps_ms() - temps_tirette > 5000){
+			etat_PAMI = PAMI_TRAJECTOIRE;
+			temps_mouvement = Temps_get_temps_ms();
+		}
+		break;
+
+	case PAMI_FIN_TEMPO_MANUELLE:
+		if(get_tirette() == 0){
+			etat_PAMI = PAMI_TRAJECTOIRE;
+			temps_mouvement = Temps_get_temps_ms();
+		}
+		break;
+
+	case PAMI_TRAJECTOIRE:
+		if(Temps_get_temps_ms() - temps_mouvement > 15000){
+			etat_PAMI = PAMI_DANCE;
+		}
+		switch (get_identifiant())
+		{
+			case 0: etat_action = Strategie_super_star(step_ms, get_couleur()); break;
+			case 1: etat_action = Strategie_groupie_1(step_ms, get_couleur()); break;
+			case 2: etat_action = Strategie_groupie_2(step_ms, get_couleur()); break;
+			case 3: etat_action = Strategie_groupie_3(step_ms, get_couleur()); break;
+			default: etat_action = ACTION_TERMINEE; break;
+		}
+		if(etat_action == ACTION_TERMINEE){
+			Moteur_Stop();
+			*asser_pos = 0;
+		}
+		break;
+
+	case PAMI_DANCE:
+		Moteur_Stop();
+		if(Temps_get_temps_ms() - temps_tirette > 15000){
+			/*while(1){
+				PAMI_dance(get_identifiant());
+			}*/
+		}
+		break;
+	
+	default:
+		break;
+	}
+
+}
+
 /// @brief Obtient la distance de l'obstacle le plus proche.
 /// @param  
 void gestion_VL53L8CX(void){
@@ -176,7 +285,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(), identifiant_lire(), get_tirette());
+	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), get_identifiant(), get_tirette());
 }
 
 void tension_batterie_init(void){
@@ -216,6 +325,8 @@ void identifiant_init(){
 	gpio_set_dir(COULEUR_PIN, GPIO_IN);
 }
 
+/// @brief Lit la tirette
+/// @return 1 si la tirette est branchée, 0 sinon
 int get_tirette(){
 	return !gpio_get(TIRETTE_PIN);
 	
@@ -227,6 +338,6 @@ int get_couleur(void){
 
 /// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse
 /// @return identifiant du robot (PDI switch)
-uint identifiant_lire(){
+uint get_identifiant(){
 	return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
 }

messagerie.c

@@ -0,0 +1,26 @@
+#include "messagerie.h"
+
+#define NB_MAX_MESSAGE 30
+
+int index_message=0;
+struct message_t message_liste[NB_MAX_MESSAGE];
+
+/// @brief Renvoi 1 si des message sont disponibles
+/// @return 
+bool messagerie_message_disponible(){
+  return (index_message != 0);
+}
+
+/// @brief Renvoi un message à traiter, à n'appeler que si message_disponible() renvoie 1
+/// @return Message
+struct message_t messagerie_get_message(){
+  index_message = index_message - 1;
+  return message_liste[index_message];
+}
+
+void messagerie_put_message(struct message_t message){
+  if(index_message < NB_MAX_MESSAGE-1){
+    message_liste[index_message] = message;
+    index_message = index_message + 1;
+  }
+}

messagerie.h

@@ -0,0 +1,23 @@
+#include "pico/stdlib.h"
+
+#define MESSAGE_TIMEOUT_US 2000
+
+struct message_t{
+  uint8_t type; // 'd' pour demande, 'r' pour une réception de données, 'w' pour écrire des données, '>' pour des logs
+  uint8_t id_carte; // Identifiant de la carte (on reprend les adresses I2C)
+  uint8_t adresse_registre; // Adresse du registre lu
+  uint8_t taille_donnees;
+  uint8_t donnees[255];
+};
+
+struct message_requete_t{
+  uint8_t commande;
+  uint8_t id_carte; // Identifiant de la carte (on reprend les adresses I2C)
+  uint8_t adresse_registre; // Adresse du registre lu
+  uint8_t taille_donnees;
+};
+
+
+bool messagerie_message_disponible();
+struct message_t messagerie_get_message();
+void messagerie_put_message(struct message_t message);