Demo 15 mars 2025

Demonstration.c

@@ -1,17 +1,30 @@
 #include "Holonome2023.h"
+#include "Strategie_2024.h"
 #include "Demonstration.h"
 
 #define TEST_TIMEOUT_US 10000000
-#define CAPTEUR_POUR_ATTENTE 11
+
+#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);
 enum etat_action_t Demonstration_calage();
 enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm);
 enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees);
+enum etat_action_t Demonstration_tourne(float angle_degrees);
 enum etat_action_t Demonstration_bezier();
 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);
 
 uint32_t temps_ms_demo = 0, temps_ms_old;
 
@@ -22,6 +35,7 @@ void demo_affiche_localisation(){
         printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
         printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie() / 10.);
         printf(">capteur:%d\n", Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE));
+        sleep_ms(50);
     }
 }
 
@@ -29,6 +43,7 @@ int Demonstration_menu(void){
     static int iteration = 2;
     int rep;
     printf("Mode demo - init\n");
+    set_position_avec_gyroscope(false);
     Demonstration_init();
     while(1){
         do{
@@ -40,6 +55,10 @@ int Demonstration_menu(void){
             /*printf("C - Trajets enchaines - manuels\n");
             printf("D - Trajets enchaines - auto\n");
             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);
         }while(rep == 0 || rep == PICO_ERROR_TIMEOUT);
@@ -69,12 +88,35 @@ int Demonstration_menu(void){
             Demonstration_bezier();
             break;
 
+        case 'e':
         case 'E':
             printf("Début attente\n");
             Demonstration_attente();
             printf("Fin attente\n");
             break;
 
+        case 'f':
+        case 'F':
+            while(1){
+                printf("Demo actionneur\n");
+                Demonstration_actionneurs();
+            }
+            break;
+
+        case 'g':
+        case 'G':
+            printf("Début prise plante\n");
+            Demonstration_prise_plante();
+            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;
@@ -82,6 +124,7 @@ int Demonstration_menu(void){
 
         case 'z':
         case 'Z':
+            printf("Demo semi-auto\n");
             Demonstration_semiauto();
             break;
 
@@ -135,6 +178,258 @@ void Demonstration_auto(){
     }
 }
 
+void Demonstration_actionneurs(){
+    Demonstration_attente();
+    Demonstration_attrape_plante();
+    Demonstration_tourne(30);
+    Demonstration_leve_bras(0);
+    Demonstration_tourne(-60);
+    Demonstration_leve_bras(5);
+    Demonstration_tourne(120);
+    Demonstration_leve_bras(1);
+    Demonstration_tourne(-180);
+    Demonstration_leve_bras(4);
+    Demonstration_tourne(240);
+    Demonstration_leve_bras(2);
+    Demonstration_tourne(-300);
+    Demonstration_leve_bras(3);
+    Demonstration_tourne(150);
+    Demonstration_baisse_bras();
+}
+
+void Demonstration_prise_plante(){
+    static int32_t m_temps_ms, m_timer;
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum etat_action_t etat_action_step;
+    static enum {
+        INIT,
+        TIENT_POT,
+        LEVE_POT,
+        ATTRAPE_PLANTE_1,
+        ATTRAPE_PLANTE_2,
+        RETOUR_MAISON
+    } etat_prise_plante = INIT;
+    while(etat_action == ACTION_EN_COURS){
+        Holonome_cyclique(PARAM_DEFAULT);
+        if(m_temps_ms != Temps_get_temps_ms()){
+            m_temps_ms = Temps_get_temps_ms();
+
+            switch (etat_prise_plante){
+                case INIT:
+                    m_timer = 3000;
+                    etat_prise_plante = TIENT_POT;
+                    etat_action = ACTION_EN_COURS;
+
+                case TIENT_POT:
+                    m_timer--;
+                    i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
+                    if(m_timer <= 0){
+                        etat_prise_plante = LEVE_POT;
+                        m_timer = 1000;
+                    }
+                    break;
+                case LEVE_POT:
+                    m_timer--;
+                    i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
+                    if(m_timer <= 0){
+                        etat_prise_plante = ATTRAPE_PLANTE_1;
+                    }
+                    break;
+                case ATTRAPE_PLANTE_1:
+                    etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_1, ZONE_PLANTE_1);
+                    if(etat_action_step == ACTION_TERMINEE){
+                        etat_prise_plante = ATTRAPE_PLANTE_2;
+                    }
+                    break;
+                case ATTRAPE_PLANTE_2:
+                    etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_6, ZONE_PLANTE_1);
+                    if(etat_action_step == ACTION_TERMINEE){
+                        etat_prise_plante = RETOUR_MAISON;
+                    }
+                    break;
+                case RETOUR_MAISON:{
+                    etat_action = Strategie_aller_a(0, 0, EVITEMENT_SANS_EVITEMENT, 1);
+                    if(etat_action == ACTION_TERMINEE){
+                        etat_prise_plante = INIT;
+                    }
+                }
+
+            }
+        }
+    }
+
+    Moteur_Stop();
+}
+
+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{
+        CLI_ON_COURT,
+        CLI_OFF_COURT,
+        CLI_OFF_LONG,
+    }etat_led_cli_orange_maintenance=CLI_OFF_LONG;
+    // Toutes les 1 ms.
+    if(temps_ms_led_cli != Temps_get_temps_ms()){
+        temps_ms_led_cli = Temps_get_temps_ms();
+
+        timer_led_ms--;
+
+        switch(etat_led_cli_orange_maintenance){
+            case CLI_ON_COURT:
+                if(timer_led_ms<= 0){
+                    i2c_annexe_couleur_balise(0, 0xFFFF);
+                    timer_led_ms = temps_court_ms;
+                    m_nb_cli--;
+                    if(m_nb_cli){
+                        etat_led_cli_orange_maintenance=CLI_OFF_COURT;
+                        timer_led_ms = temps_court_ms;
+                    }else{
+                        etat_led_cli_orange_maintenance=CLI_OFF_LONG;
+                        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 = temps_court_ms;
+                    etat_led_cli_orange_maintenance=CLI_ON_COURT;
+                }
+                break;
+            case CLI_OFF_LONG:
+                m_nb_cli = nb_cli;
+                if(timer_led_ms<= 0){
+                    i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
+                    timer_led_ms = temps_court_ms;
+                    etat_led_cli_orange_maintenance=CLI_ON_COURT;
+                }
+                break;
+        }
+   
+    }
+}
+
+void Balise_pulse_vert(){
+    static int32_t temps_ms_led_cli, timer_led_ms = 800;
+    int32_t timer_led_max_ms =800;
+    int32_t vert_led, vert_led_max = 0b111;
+    static enum etat_led_pulse_vert_t{
+        PULSE_RISE,
+        PULSE_FALL,
+    }etat_led_pulse_vert=PULSE_FALL;
+    // Toutes les 1 ms.
+    if(temps_ms_led_cli != Temps_get_temps_ms()){
+        temps_ms_led_cli = Temps_get_temps_ms();
+
+        timer_led_ms--;
+
+        switch(etat_led_pulse_vert){
+            case PULSE_RISE:
+                vert_led = (vert_led_max-1)  - (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
+                vert_led = vert_led & 0b111;
+                i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
+                if(timer_led_ms<= 0){
+                    i2c_annexe_couleur_balise(0, 0xFFFF);
+                    timer_led_ms = timer_led_max_ms;
+                    etat_led_pulse_vert=PULSE_FALL;
+                }
+                break;
+            case PULSE_FALL:
+                vert_led = (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
+                vert_led = vert_led & 0b111;
+                i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
+                if(timer_led_ms<= 0){
+                    timer_led_ms = timer_led_max_ms;
+                    etat_led_pulse_vert=PULSE_RISE;
+                }
+                break;
+        }
+   
+    }
+}
+
+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,
@@ -145,6 +440,7 @@ enum etat_action_t Demonstration_attente(){
 
     while(true){
         Holonome_cyclique(PARAM_NO_MOTORS);
+        Balise_cli_orange_maintenance(2);
         
         switch(etat_attente){
             case ATTENTE_DETECTION:
@@ -376,6 +672,52 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_
     return ACTION_ECHEC;
 }
 
+/// @brief Rotation du robot sur lui-même du robot 
+/// @param angle_degrees : Rotation du robot sur lui-même
+/// @return ACTION_TERMINEE
+enum etat_action_t Demonstration_tourne(float angle_degrees){
+    enum {
+        DEMO_TOURNE,
+        DEMO_TOURNE_TERMINE
+    } etat_avance_puis_tourne = DEMO_TOURNE;
+    int pos_x_init_mm, pos_y_init_mm;
+    while(true){
+        struct trajectoire_t trajectoire;
+
+        Holonome_cyclique(PARAM_DEFAULT);
+
+        // Toutes les 1 ms.
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+
+        
+            switch (etat_avance_puis_tourne)
+            {
+            
+            case DEMO_TOURNE:
+                Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
+                Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
+                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_avance_puis_tourne = DEMO_TOURNE_TERMINE;
+                    Trajet_config(TRAJECT_CONFIG_STD);
+                }
+                break;
+            
+            case DEMO_TOURNE_TERMINE:
+                etat_avance_puis_tourne = DEMO_TOURNE;
+                Moteur_Stop();
+                return ACTION_TERMINEE;
+            
+            default:
+                break;
+            }
+        }
+        
+    }
+    return ACTION_ECHEC;
+}
+
 /// @brief Déplacement suivant deux courbes de Bézier. Recommandé pour une démo sur une planche de 1m x 1,5m
 /// @return ACTION_TERMINEE
 enum etat_action_t Demonstration_bezier(){
@@ -434,3 +776,128 @@ enum etat_action_t Demonstration_bezier(){
     }
     return ACTION_ECHEC;
 }
+
+/// @brief Leve le bras, et agite le doigt
+/// @param bras 
+enum etat_action_t  Demonstration_leve_bras(uint32_t bras){
+    static enum{
+        LB_LEVE_BRAS,
+        LB_TEMPO_BRAS,
+        LB_DOIGT_ATTRAPE_1,
+        LB_DOIGT_LACHE,
+        LB_DOIGT_ATTRAPE_2,
+    } DLB_status;
+    int timer_ms;
+    while(true){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+            timer_ms--;
+
+            switch(DLB_status){
+                case LB_LEVE_BRAS:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
+                    DLB_status = LB_TEMPO_BRAS;
+                    timer_ms = 250;
+                    break;
+
+                case LB_TEMPO_BRAS:
+                    if(timer_ms <= 0){
+                        DLB_status = LB_DOIGT_ATTRAPE_1;
+                        timer_ms=150;
+                    }
+                    break;
+
+                case LB_DOIGT_ATTRAPE_1:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
+                    if(timer_ms <= 0){
+                        DLB_status = LB_DOIGT_LACHE;
+                        timer_ms=200;
+                    }
+                    break;
+
+                case LB_DOIGT_LACHE:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
+                    if(timer_ms <= 0){
+                        DLB_status = LB_DOIGT_ATTRAPE_2;
+                        timer_ms=200;
+                    }
+                    break;
+
+                case LB_DOIGT_ATTRAPE_2:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
+                    if(timer_ms <= 0){
+                        DLB_status = LB_LEVE_BRAS;
+                        return ACTION_TERMINEE;
+                    }
+                    break;
+
+            }
+        }
+        
+        
+    }
+}
+enum etat_action_t  Demonstration_attrape_plante(void){
+    int32_t timer_ms;
+    enum {
+        DAP_ENVOI,
+        DAP_ATTENTE
+    } dap_status=DAP_ENVOI;
+    while(true){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+            timer_ms--;
+
+            switch(dap_status){
+                case DAP_ENVOI:
+                    i2c_annexe_attrape_plante(PLANTE_BRAS_1);
+                    dap_status = DAP_ATTENTE;
+                    timer_ms=5000;
+                    break;
+
+                case DAP_ATTENTE:
+                    if (timer_ms <= 0){
+                        dap_status=DAP_ENVOI;
+                        return ACTION_TERMINEE;
+                    }
+                    break;            
+            }
+            
+        }
+    }
+}
+
+enum etat_action_t  Demonstration_baisse_bras(void){
+    static int32_t timer_ms;
+    enum {
+        DBB_ENVOI,
+        DBB_ATTENTE
+    } dbb_status=DBB_ENVOI;
+    while(true){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+            timer_ms--;
+
+            switch(dbb_status){
+                case DBB_ENVOI:
+                    for(int bras=0; bras < 6; bras++){
+                        i2c_annexe_actionneur_pot(bras, BRAS_PLIE, DOIGT_LACHE);
+                    }
+                    dbb_status = DBB_ATTENTE;
+                    timer_ms=500;
+                    break;
+
+                case DBB_ATTENTE:
+                    if (timer_ms <= 0){
+                        dbb_status = DBB_ENVOI;
+                        return ACTION_TERMINEE;
+                    }
+                    break;            
+            }
+            
+        }
+    }
+}

Demonstration.h

@@ -1,4 +1,5 @@
  int Demonstration_menu(void);
  void Demonstration_semiauto(void);
  void Demonstration_auto(void);
- int Demonstration_init(void);
+ int Demonstration_init(void);
+ void Demonstration_prise_plante(void);

Holonome2023.c

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

Tests_deplacement.c

@@ -345,8 +345,8 @@ int test_aller_retour(){
     switch(lettre){
         case 'b':
         case 'B':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
+            Trajectoire_bezier(&trajectoire, 0, 0, 2500, 0, 250, 1300, 0, 0, 0, 0);
             printf("Trajectoire de Bézier\n");
             break;