diff --git a/Demonstration.c b/Demonstration.c index 3069148..67e9727 100644 --- a/Demonstration.c +++ b/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, diff --git a/Demonstration.h b/Demonstration.h index f015555..b751a4f 100644 --- a/Demonstration.h +++ b/Demonstration.h @@ -1,4 +1,5 @@ int Demonstration_menu(void); void Demonstration_semiauto(void); void Demonstration_auto(void); - int Demonstration_init(void); \ No newline at end of file + int Demonstration_init(void); + void Demonstration_prise_plante(void); \ No newline at end of file diff --git a/Holonome2023.c b/Holonome2023.c index dd9a598..aa5000e 100644 --- a/Holonome2023.c +++ b/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();