diff --git a/Strategie_2024_plante.c b/Strategie_2024_plante.c index 2186527..38fa6f7 100644 --- a/Strategie_2024_plante.c +++ b/Strategie_2024_plante.c @@ -161,6 +161,7 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){ /// @return ACTION_EN_COURS ou ACTION_TERMINEE ou ACTION_ECHEC (plante tombée devant le robot) enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante){ static enum{ + PDP_INIT, PDP_ALLER_PLANTE, PDP_PRE_PRISE_PLANTE, PDP_PRE_PRISE_TEMPO, @@ -174,9 +175,13 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot enum validite_vl53l8_t validite; enum etat_action_t etat_action; float angle_rad, distance_mm; + static nb_essai_prise; switch (etat_plante_dans_pot) { + case PDP_INIT: + nb_essai_prise=0; + // Pas besoin de break ici case PDP_ALLER_PLANTE: etat_action = Strat_2024_aller_a_plante(zone_plante); if (etat_action == ACTION_TERMINEE){ @@ -212,13 +217,17 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot position_initiale.angle_radian += ANGLE_PINCE; position_recule = Geometrie_deplace(position_initiale, -100); Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); - if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){ + if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){ if(validite == VL53L8_PLANTE){ if(distance_mm > 90){ // Nous n'avons pas attrapé la plante avec le doigt, on recommence: etat_plante_dans_pot = PDP_ALLER_PLANTE; i2c_annexe_ouvre_doigt_plante(); i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); + nb_essai_prise++; + if(nb_essai_prise >= 3){ + return ACTION_ECHEC; + } break; } }