Demo 15 mars 2025

Demonstration.c

@@ -3,7 +3,12 @@
 #include "Demonstration.h"
 
 #define TEST_TIMEOUT_US 10000000
+
 #define CAPTEUR_POUR_ATTENTE 6
+#define CAPTEUR_POUR_ATTENTE_DEVANT 6
+#define CAPTEUR_POUR_ATTENTE_GAUCHE 9
+#define CAPTEUR_POUR_ATTENTE_DROIT 3
+#define CAPTEUR_POUR_ATTENTE_ARRIERE 0
 
 
 int Demonstration_init(void);
@@ -16,6 +21,8 @@ enum etat_action_t Demonstration_attente();
 enum etat_action_t Demonstration_leve_bras(uint32_t bras);
 enum etat_action_t  Demonstration_baisse_bras(void);
 enum etat_action_t Demonstration_attrape_plante();
+enum etat_action_t Demonstration_attente_capteur(int capteur);
+void Demonstration_menu_balise(void);
 void Demonstration_prise_plante(void);
 void Demonstration_actionneurs(void);
 
@@ -50,6 +57,7 @@ int Demonstration_menu(void){
             printf("E - Asservissement angulaire\n");*/
             printf("F - Rotation et actionneurs\n");
             printf("G - Prise plante\n");
+            printf("M - Menu Balise\n");
             printf("Z - Sem-automatique\n");
             printf("Q - Quitter\n");
             rep = getchar_timeout_us(TEST_TIMEOUT_US);
@@ -102,6 +110,13 @@ int Demonstration_menu(void){
             printf("Fin prise plante\n");
             break;
 
+        case 'm':
+        case 'M':
+            printf("Menu balise\n");
+            Demonstration_menu_balise();
+            printf("Fin menu balise\n");
+            break;
+
         case 'q':
         case 'Q':
             return 0;
@@ -236,6 +251,9 @@ void Demonstration_prise_plante(){
                     break;
                 case RETOUR_MAISON:{
                     etat_action = Strategie_aller_a(0, 0, EVITEMENT_SANS_EVITEMENT, 1);
+                    if(etat_action == ACTION_TERMINEE){
+                        etat_prise_plante = INIT;
+                    }
                 }
 
             }
@@ -246,6 +264,7 @@ void Demonstration_prise_plante(){
 }
 
 void Balise_cli_orange_maintenance(int nb_cli){
+    int32_t temps_court_ms = 150, temps_long_ms = 700;
     static int32_t temps_ms_led_cli, timer_led_ms = 0;
     static int32_t m_nb_cli;
     static enum etat_led_cli_orange_maintenance_t{
@@ -263,21 +282,21 @@ void Balise_cli_orange_maintenance(int nb_cli){
             case CLI_ON_COURT:
                 if(timer_led_ms<= 0){
                     i2c_annexe_couleur_balise(0, 0xFFFF);
-                    timer_led_ms = 100;
+                    timer_led_ms = temps_court_ms;
                     m_nb_cli--;
                     if(m_nb_cli){
                         etat_led_cli_orange_maintenance=CLI_OFF_COURT;
-                        timer_led_ms = 100;
+                        timer_led_ms = temps_court_ms;
                     }else{
                         etat_led_cli_orange_maintenance=CLI_OFF_LONG;
-                        timer_led_ms = 700;
+                        timer_led_ms = temps_long_ms;
                     }
                 }
                 break;
             case CLI_OFF_COURT:
                 if(timer_led_ms<= 0){
                     i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
-                    timer_led_ms = 100;
+                    timer_led_ms = temps_court_ms;
                     etat_led_cli_orange_maintenance=CLI_ON_COURT;
                 }
                 break;
@@ -285,7 +304,7 @@ void Balise_cli_orange_maintenance(int nb_cli){
                 m_nb_cli = nb_cli;
                 if(timer_led_ms<= 0){
                     i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
-                    timer_led_ms = 100;
+                    timer_led_ms = temps_court_ms;
                     etat_led_cli_orange_maintenance=CLI_ON_COURT;
                 }
                 break;
@@ -333,6 +352,84 @@ void Balise_pulse_vert(){
     }
 }
 
+void Demonstration_menu_balise(void){
+    int nb_cli=2;
+    while(1){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        Balise_cli_orange_maintenance(nb_cli);
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DROIT) == ACTION_TERMINEE){
+            nb_cli++;
+            printf(">nb_cli%d\n",nb_cli);
+        }
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_GAUCHE) == ACTION_TERMINEE){
+            nb_cli--;
+            printf(">nb_cli%d\n",nb_cli);
+        }
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DEVANT) == ACTION_TERMINEE){
+            return;
+        }
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_ARRIERE) == ACTION_TERMINEE){
+            switch(nb_cli){
+                case 2:
+                    Demonstration_actionneurs();
+                    break;
+                case 3:
+                    Demonstration_prise_plante();
+                    break;
+            }
+        }
+
+    }
+}
+
+/// @brief Comme la fonction demonstration attente, mais en choisissant le capteur. Fonction non-bloquante
+/// @param capteur: numéro du capteur, 0 à l'arrière, 3 à droite, 6 devant, 9, à gauche
+/// @return ACTION_TERMINEE si nous avons un signal pour ce capteur
+enum etat_action_t Demonstration_attente_capteur(int capteur){
+    static enum {
+        ATTENTE_DETECTION,
+        DETECTION_PROCHE,
+        FIN_ATTENTE
+    } etat_attente[12] = {ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,
+    ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION};
+    static uint32_t temps_debut_tempo[12];
+    uint32_t duree_tempo_ms = 50;
+    
+    switch(etat_attente[capteur]){
+        case ATTENTE_DETECTION:
+            if(Balise_VL53L1X_get_capteur_cm(capteur) < 15 && Balise_VL53L1X_get_capteur_cm(capteur) > 1){
+                /// Sans obstacle, le capteur peut renvoyer 0;
+                etat_attente[capteur]=DETECTION_PROCHE;
+                temps_debut_tempo[capteur] = time_us_32();
+            }
+            break;
+        
+        case DETECTION_PROCHE:
+            if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
+                // On a perdu la detection avant le temps écoulé
+                etat_attente[capteur]=ATTENTE_DETECTION;
+            }
+            if((temps_debut_tempo[capteur] + (duree_tempo_ms * 1000)) < time_us_32()){
+                // temps écoulé
+                etat_attente[capteur]=FIN_ATTENTE;
+            }
+            break;
+
+        case FIN_ATTENTE:
+            if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
+                // On a perdu la detection après le temps écoulé
+                etat_attente[capteur]=ATTENTE_DETECTION;
+                return ACTION_TERMINEE;
+            }
+            break;
+            
+    }
+    return ACTION_EN_COURS;
+    //sleep_ms(20);
+    
+
+}
+
 enum etat_action_t Demonstration_attente(){
     enum {
         ATTENTE_DETECTION,

Demonstration.h

@@ -2,3 +2,4 @@
  void Demonstration_semiauto(void);
  void Demonstration_auto(void);
  int Demonstration_init(void);
+ void Demonstration_prise_plante(void);

Holonome2023.c

@@ -44,7 +44,8 @@ int main() {
 
     stdio_init_all();
     
-    //Demonstration_init();Demonstration_auto();
+    //Demonstration_init();Demonstration_prise_plante();
+    //while(1);
     while(mode_test());
     //test_pseudo_homologation();
     Holonome2023_init();