abandonne au bout de 3 essais de prise de plante
This commit is contained in:
parent
7adacfddb8
commit
34f0a45041
@ -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;
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user