#include "hardware/gpio.h" #include "i2c_annexe.h" #include "Asser_Position.h" #include "Balise_VL53L1X.h" #include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "Localisation.h" #include "Moteurs.h" #include "Strategie_prise_cerises.h" #include "Strategie.h" #include "Trajet.h" #include "math.h" #define SEUIL_RECAL_DIST_MM 75 #define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGRE_EN_RADIAN) #define DISTANCE_OBSTACLE_CM 50 #define DISTANCE_PAS_OBSTACLE_MM 2000 // TODO: Peut-être à remetttre en variable locale après float distance_obstacle; enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian); enum etat_action_t lance_balles(uint32_t step_ms); enum etat_homologation_t etat_homologation=STRATEGIE_INIT; void Strategie(enum couleur_t couleur, uint32_t step_ms){ float angle_fin; float recal_pos_x, recal_pos_y; enum longer_direction_t longer_direction; enum etat_action_t etat_action; enum etat_trajet_t etat_trajet; struct trajectoire_t trajectoire; static enum etat_strategie_t { STRATEGIE_INIT, ALLER_CERISE_BAS, ATTRAPER_CERISE_BAS, ALLER_CERISE_HAUT, ATTRAPER_CERISE_HAUT, ALLER_CERISE_GAUCHE, ATTRAPER_CERISE_GAUCHE, ALLER_CERISE_DROITE, ATTRAPER_CERISE_DROITE, ALLER_PANIER, LANCER_PANIER, }etat_strategie; switch(etat_strategie){ case STRATEGIE_INIT: if(couleur == COULEUR_BLEU){ Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); }else{ Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); } etat_strategie = ALLER_CERISE_BAS; break; case ALLER_CERISE_BAS: if(couleur == COULEUR_BLEU){ angle_fin = 30. * DEGRE_EN_RADIAN; }else{ angle_fin = -150. * DEGRE_EN_RADIAN; } Trajet_config(250, 500); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 857, 156, Localisation_get().angle_radian, angle_fin); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPER_CERISE_BAS; } break; case ATTRAPER_CERISE_BAS: recal_pos_y = RAYON_ROBOT; if(couleur == COULEUR_BLEU){ longer_direction = LONGER_VERS_C; recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM; }else{ longer_direction = LONGER_VERS_A; recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM; } etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y); if(etat_action == ACTION_TERMINEE){ etat_strategie = ALLER_PANIER; } break; case ALLER_PANIER: Trajet_config(500, 500); if(couleur == COULEUR_BLEU){ Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 485, Localisation_get().y_mm, 465, 857, 465,2830, +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN); }else{ Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 485, Localisation_get().y_mm, 2000-465, 857, 2000-465,2830, -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN); } if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = LANCER_PANIER; } break; case LANCER_PANIER: if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){ etat_homologation = STRATEGIE_FIN; } break; case STRATEGIE_FIN: i2c_annexe_desactive_propulseur(); commande_vitesse_stop(); break; } } void Homologation(uint32_t step_ms){ enum etat_action_t etat_action; enum etat_trajet_t etat_trajet; struct trajectoire_t trajectoire; switch(etat_homologation){ case STRATEGIE_INIT: Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); etat_homologation = ATTENTE_TIRETTE; break; case ATTENTE_TIRETTE: if(attente_tirette() == 0){ etat_homologation = APPROCHE_CERISE_1_A; } break; case APPROCHE_CERISE_1_A: Trajet_config(250, 500); Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ etat_homologation = ATTRAPE_CERISE_1; } break; case ATTRAPE_CERISE_1: etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, RAYON_ROBOT); if(etat_action == ACTION_TERMINEE){ etat_homologation = APPROCHE_PANIER_1; } break; case APPROCHE_PANIER_1: Trajet_config(500, 500); Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 485, Localisation_get().y_mm, 465, 857, 465,2830, +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_homologation = CALAGE_PANIER_1; } break; case CALAGE_PANIER_1: if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ etat_homologation = RECULE_PANIER; } break; case RECULE_PANIER: Trajet_config(250, 500); Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80, 120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_homologation = LANCE_DANS_PANIER; } break; case LANCE_DANS_PANIER: Asser_Position_maintien(); if(lance_balles(step_ms) == ACTION_TERMINEE){ etat_homologation = STRATEGIE_FIN; } break; case APPROCHE_CERISE_2: Trajet_config(250, 500); Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 830, 3000 - 156, Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_homologation = STRATEGIE_FIN; } break; case ATTRAPPE_CERISE_2: etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, 3000-RAYON_ROBOT); if(etat_action == ACTION_TERMINEE){ etat_homologation = APPROCHE_PANIER_2; } break; case STRATEGIE_FIN: i2c_annexe_desactive_propulseur(); commande_vitesse_stop(); break; } } enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms){ static enum { CALAGE_PANIER_1, RECULE_PANIER, LANCE_DANS_PANIER, }etat_lance_balles_dans_panier; float recal_pos_x, recal_pos_y; enum longer_direction_t longer_direction; float point_x, point_y; enum etat_action_t etat_action = ACTION_EN_COURS; struct trajectoire_t trajectoire; switch(etat_lance_balles_dans_panier){ case CALAGE_PANIER_1: if(couleur == COULEUR_BLEU){ recal_pos_x = RAYON_ROBOT; longer_direction = LONGER_VERS_A; }else{ recal_pos_x = 2000- RAYON_ROBOT; longer_direction = LONGER_VERS_C; } if(calage_angle(longer_direction, recal_pos_x, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = RECULE_PANIER; } break; case RECULE_PANIER: Trajet_config(250, 500); if(couleur == COULEUR_BLEU){ point_x = 180; point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; }else{ point_x = 1735; point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; } Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, point_y, 120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = LANCE_DANS_PANIER; } break; case LANCE_DANS_PANIER: Asser_Position_maintien(); if(lance_balles(step_ms) == ACTION_TERMINEE){ etat_action=ACTION_TERMINEE; } break; } return etat_action; } /// @brief Active le propulseur, ouvre la porte, attend qql secondes. /// @param step_ms : pas de temps. /// @return ACTION_EN_COURS ou ACTION_TERMINEE enum etat_action_t lance_balles(uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; static uint32_t tempo_ms; static uint32_t nb_iteration; static enum{ LANCE_PROPULSEUR_ON, LANCE_TEMPO_PROP_ON, LANCE_PORTE_OUVERTE, LANCE_OUVERTURE_INITIALE, } etat_lance_balle = LANCE_PROPULSEUR_ON; switch(etat_lance_balle){ case LANCE_PROPULSEUR_ON: i2c_annexe_active_propulseur(); tempo_ms = 1000; nb_iteration=0; etat_lance_balle = LANCE_OUVERTURE_INITIALE; break; case LANCE_OUVERTURE_INITIALE: if(temporisation_terminee(&tempo_ms, step_ms)){ i2c_annexe_ouvre_porte(); etat_lance_balle = LANCE_PORTE_OUVERTE; tempo_ms = 250; } break; case LANCE_TEMPO_PROP_ON: if(temporisation_terminee(&tempo_ms, step_ms)){ i2c_annexe_ouvre_porte(); nb_iteration++; if(nb_iteration > 10){ nb_iteration=0; etat_action = ACTION_TERMINEE; etat_lance_balle = LANCE_PROPULSEUR_ON; i2c_annexe_desactive_propulseur(); }else{ etat_lance_balle = LANCE_PORTE_OUVERTE; tempo_ms = 150; } } break; case LANCE_PORTE_OUVERTE: if(temporisation_terminee(&tempo_ms, step_ms)){ i2c_annexe_mi_ferme_porte(); etat_lance_balle = LANCE_TEMPO_PROP_ON; tempo_ms = 750; } break; } return etat_action; } /// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){ enum etat_action_t etat_action = ACTION_EN_COURS; struct position_t position; avance_puis_longe_bordure(longer_direction); if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) || ((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){ etat_action = ACTION_TERMINEE; position = Localisation_get(); if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){ Localisation_set_x(x_mm); } if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){ Localisation_set_y(y_mm); } if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){ Localisation_set_angle(angle_radian); } } return etat_action; } enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; enum etat_trajet_t etat_trajet; float angle_avancement; static enum { PARCOURS_INIT, PARCOURS_AVANCE, } etat_parcourt=PARCOURS_INIT; switch (etat_parcourt){ case PARCOURS_INIT: Trajet_debut_trajectoire(trajectoire); etat_parcourt = PARCOURS_AVANCE; break; case PARCOURS_AVANCE: angle_avancement = Trajet_get_orientation_avance(); distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement); Trajet_set_obstacle_mm(distance_obstacle); etat_trajet = Trajet_avance(step_ms/1000.); if(etat_trajet == TRAJET_TERMINE){ Trajet_set_obstacle_mm(DISTANCE_INVALIDE); etat_action = ACTION_TERMINEE; etat_parcourt = PARCOURS_INIT; } break; } return etat_action; } enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; enum etat_trajet_t etat_trajet; static enum { PARCOURS_INIT, PARCOURS_AVANCE, } etat_parcourt=PARCOURS_INIT; switch (etat_parcourt){ case PARCOURS_INIT: Trajet_debut_trajectoire(trajectoire); Trajet_set_obstacle_mm(distance_pas_obstacle); etat_parcourt = PARCOURS_AVANCE; break; case PARCOURS_AVANCE: etat_trajet = Trajet_avance(step_ms/1000.); if(etat_trajet == TRAJET_TERMINE){ etat_action = ACTION_TERMINEE; etat_parcourt = PARCOURS_INIT; } break; } return etat_action; } /// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette uint attente_tirette(void){ return !gpio_get(TIRETTE); } /// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU enum couleur_t lire_couleur(void){ if (gpio_get(COULEUR)) return COULEUR_VERT; return COULEUR_BLEU; } /// @brief Décremente la temps de step_ms, renvoie 1 si la temporisation est écoulée /// @param tempo_ms /// @param step_ms /// @return 1 si la temporisation est écoulée, 0 sinon. int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){ if(*tempo_ms < step_ms){ return 1; }else{ *tempo_ms -= step_ms; return 0; } }