#include "Commande_vitesse.h" #include "Strategie_2024_plante.h" #include "Geometrie_robot.h" #include "Localisation.h" #include "i2c_annexe.h" #include #include #define ASSER_ANGLE_GAIN_PLANTE_P 1.5 #define ASSER_DISTANCE_GAIN_PLANTE_P 10 #define RAYON_ZONE_PLANTE_MM 180 struct position_t liste_zone_plante[]= { {.x_mm = 1500, .y_mm = 1500 }, {.x_mm = 1000, .y_mm = 1300 }, {.x_mm = 1000, .y_mm = 700 }, {.x_mm = 1500, .y_mm = 500 }, {.x_mm = 2000, .y_mm = 700 }, {.x_mm = 2000, .y_mm = 1300 } }; enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms){ struct position_t position_robot, position_zone_plante; float angle; position_zone_plante = liste_zone_plante[zone_plante]; position_robot = Localisation_get(); angle = atan2f(position_zone_plante.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm); return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms); } /// @brief Indique si la plante se trouve dans la zone de récole (en comptant une marge) /// @param position_plante : position de la plante /// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante /// @return 1 si la plante est dans la zone, 0 si le robot ne trouve pas la plante ou qu'elle est hors zone. bool est_dans_zone(struct position_t position_plante, enum zone_plante_t zone_plante){ struct position_t position_zone_plante; float distance_plante_zone; position_zone_plante = liste_zone_plante[zone_plante]; distance_plante_zone = sqrtf( powf(position_zone_plante.x_mm - position_zone_plante.x_mm, 2) + powf(position_zone_plante.y_mm - position_zone_plante.y_mm, 2)); if(distance_plante_zone < RAYON_ZONE_PLANTE_MM){ return 1; } return 0; } /// @brief Déplace le robot vers une plante, vérifie que la plante est bien dans la zone plante qu'on vise /// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante /// @return ACTION_SUCCESS si le robot est prêt à attraper la plante, ACTION_ECHEC si la plante enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){ static enum { SAAP_INIT_DETECTION, SAAP_ASSERV } etat_aller_a_plante = SAAP_INIT_DETECTION; enum validite_vl53l8_t validite; float angle_rad, distance_mm, distance_obstacle, commande_vitesse_plante; float distance_contrainte_obstacle, vitesse_max_contrainte_obstacle; const float acceleration_mm_ss_obstacle=500; static float distance_min_mm; static int tempo_ms, tempo_asserv; static bool entree_dans_zone; switch(etat_aller_a_plante){ case SAAP_INIT_DETECTION: i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT); i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT); i2c_annexe_mi_ferme_doigt_plante(); i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE); tempo_ms = 2000; distance_min_mm = 2000; tempo_asserv = 500; entree_dans_zone=false; etat_aller_a_plante = SAAP_ASSERV; break; case SAAP_ASSERV: i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm); if(validite == VL53L8_PLANTE){ tempo_ms = 2000; // 1 on s'assure que la plante est dans la zone qu'on recherche ! if(zone_plante != ZONE_PLANTE_AUCUNE){ // Cas où on checher une plante dans une zone struct position_t position_robot, position_plante; bool robot_dans_zone; position_robot = Localisation_get(); position_robot.angle_radian += ANGLE_PINCE + angle_rad; position_plante = Geometrie_deplace(position_robot, distance_mm); if( !est_dans_zone(position_plante, zone_plante)){ etat_aller_a_plante = SAAP_INIT_DETECTION; return ACTION_ECHEC; } robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante); if(entree_dans_zone == true){ if(robot_dans_zone == false){ // Le robot est sorti de la zone etat_aller_a_plante = SAAP_INIT_DETECTION; return ACTION_ECHEC; } } if(robot_dans_zone == true){ entree_dans_zone = true; } } // 2 on s'assure qu'il n'y a pas de robot en face (TODO) distance_contrainte_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(ANGLE_PINCE); if(distance_contrainte_obstacle != DISTANCE_INVALIDE){ vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle); } // 2 bis, on s'assure que le robot se rapproche de la plante. Si ce n'est pas le cas, on arrête if(distance_mm < distance_min_mm){ distance_min_mm = distance_mm; tempo_asserv = 500; }else{ tempo_asserv--; if(tempo_asserv <= 0){ commande_vitesse(0, 0, 0); etat_aller_a_plante = SAAP_INIT_DETECTION; return ACTION_ECHEC; } } // 3 on asservi commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P; if(commande_vitesse_plante > 150){ commande_vitesse_plante = 150; } if(commande_vitesse_plante <= 0){ commande_vitesse_stop(); i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); etat_aller_a_plante = SAAP_INIT_DETECTION; return ACTION_TERMINEE; } // On limite la vitesse avec l'obstacle if (commande_vitesse_plante > vitesse_max_contrainte_obstacle){ commande_vitesse_plante = vitesse_max_contrainte_obstacle; } commande_vitesse(cosf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , sinf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , (angle_rad - 0.04) * ASSER_ANGLE_GAIN_PLANTE_P); }else{ tempo_ms--; if(tempo_ms <= 0){ etat_aller_a_plante = SAAP_INIT_DETECTION; return ACTION_ECHEC; } } break; } return ACTION_EN_COURS; } /// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé /// @param step_ms /// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6 /// @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, PDP_PRE_PRISE_RECULE, PDP_ATTRAPE_PLANTE, PDP_RECULE, PDP_TEMPO, } etat_plante_dans_pot = PDP_ALLER_PLANTE; static int tempo_ms; struct position_t position_recule, position_initiale; enum validite_vl53l8_t validite; enum etat_action_t etat_action; float angle_rad, distance_mm; static int 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){ etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE; }else if (etat_action == ACTION_ECHEC){ i2c_annexe_ouvre_doigt_plante(); return ACTION_ECHEC; } break; case PDP_PRE_PRISE_PLANTE: i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE); i2c_annexe_ferme_doigt_plante(); tempo_ms = 250; etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO; break; case PDP_PRE_PRISE_TEMPO: tempo_ms--; if(tempo_ms <= 0){ tempo_ms=500; etat_plante_dans_pot=PDP_PRE_PRISE_RECULE; } break; case PDP_PRE_PRISE_RECULE: i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm); tempo_ms--; if(tempo_ms <= 0){ i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT); i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT); } position_initiale = Localisation_get(); 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, 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; } } commande_vitesse_stop(); i2c_annexe_ouvre_doigt_plante(); i2c_annexe_attrape_plante(bras_depose_pot); tempo_ms = 5000; i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); etat_plante_dans_pot=PDP_ATTRAPE_PLANTE; } break; case PDP_ATTRAPE_PLANTE: tempo_ms--; if(tempo_ms <= 0){ etat_plante_dans_pot=PDP_TEMPO; if(bras_depose_pot == PLANTE_BRAS_1){ i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT); }else{ i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT); } tempo_ms = 250; } break; case PDP_TEMPO: tempo_ms--; if(tempo_ms <= 0){ etat_plante_dans_pot=PDP_RECULE; } break; case PDP_RECULE: tempo_ms--; position_initiale = Localisation_get(); 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, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){ commande_vitesse_stop(); etat_plante_dans_pot=PDP_ALLER_PLANTE; return ACTION_TERMINEE; } break; default: break; } return ACTION_EN_COURS; }