diff --git a/Demonstration.c b/Demonstration.c index 1596daa..3069148 100644 --- a/Demonstration.c +++ b/Demonstration.c @@ -1,4 +1,5 @@ #include "Holonome2023.h" +#include "Strategie_2024.h" #include "Demonstration.h" #define TEST_TIMEOUT_US 10000000 @@ -15,6 +16,7 @@ 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(); +void Demonstration_prise_plante(void); void Demonstration_actionneurs(void); uint32_t temps_ms_demo = 0, temps_ms_old; @@ -47,6 +49,7 @@ int Demonstration_menu(void){ printf("D - Trajets enchaines - auto\n"); printf("E - Asservissement angulaire\n");*/ printf("F - Rotation et actionneurs\n"); + printf("G - Prise plante\n"); printf("Z - Sem-automatique\n"); printf("Q - Quitter\n"); rep = getchar_timeout_us(TEST_TIMEOUT_US); @@ -77,12 +80,14 @@ 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"); @@ -90,6 +95,13 @@ int Demonstration_menu(void){ } break; + case 'g': + case 'G': + printf("Début prise plante\n"); + Demonstration_prise_plante(); + printf("Fin prise plante\n"); + break; + case 'q': case 'Q': return 0; @@ -170,14 +182,77 @@ void Demonstration_actionneurs(){ Demonstration_baisse_bras(); } -void Balise_cli_orange_maintenance(){ +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); + } + + } + } + } + + Moteur_Stop(); +} + +void Balise_cli_orange_maintenance(int nb_cli){ 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_1, - CLI_OFF_1, - CLI_ON_2, - CLI_OFF_2, - }etat_led_cli_orange_maintenance=CLI_ON_1; + 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(); @@ -185,32 +260,33 @@ void Balise_cli_orange_maintenance(){ timer_led_ms--; switch(etat_led_cli_orange_maintenance){ - case CLI_ON_1: + case CLI_ON_COURT: if(timer_led_ms<= 0){ i2c_annexe_couleur_balise(0, 0xFFFF); timer_led_ms = 100; - etat_led_cli_orange_maintenance=CLI_OFF_1; + m_nb_cli--; + if(m_nb_cli){ + etat_led_cli_orange_maintenance=CLI_OFF_COURT; + timer_led_ms = 100; + }else{ + etat_led_cli_orange_maintenance=CLI_OFF_LONG; + timer_led_ms = 700; + } } break; - case CLI_OFF_1: + case CLI_OFF_COURT: if(timer_led_ms<= 0){ i2c_annexe_couleur_balise(0b11101000, 0xFFFF); timer_led_ms = 100; - etat_led_cli_orange_maintenance=CLI_ON_2; + etat_led_cli_orange_maintenance=CLI_ON_COURT; } break; - case CLI_ON_2: - if(timer_led_ms<= 0){ - i2c_annexe_couleur_balise(0, 0xFFFF); - timer_led_ms = 800; - etat_led_cli_orange_maintenance=CLI_OFF_2; - } - break; - case CLI_OFF_2: + case CLI_OFF_LONG: + m_nb_cli = nb_cli; if(timer_led_ms<= 0){ i2c_annexe_couleur_balise(0b11101000, 0xFFFF); timer_led_ms = 100; - etat_led_cli_orange_maintenance=CLI_ON_1; + etat_led_cli_orange_maintenance=CLI_ON_COURT; } break; } @@ -267,7 +343,7 @@ enum etat_action_t Demonstration_attente(){ while(true){ Holonome_cyclique(PARAM_NO_MOTORS); - Balise_cli_orange_maintenance(); + Balise_cli_orange_maintenance(2); switch(etat_attente){ case ATTENTE_DETECTION: