#include "hardware/gpio.h" #include "i2c_annexe.h" #include "Asser_Position.h" #include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "Localisation.h" #include "Monitoring.h" #include "Moteurs.h" #include "Score.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 #define NB_OBJECTIFS 4 struct objectif_t objectifs[NB_OBJECTIFS]; struct objectif_t * objectif_courant; uint32_t Score_cerises_dans_robot=0; void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){ Score_cerises_dans_robot = nb_cerises; } uint32_t Strategie_get_cerise_dans_robot(void){ return Score_cerises_dans_robot; } // 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, uint32_t nb_cerises); enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises); enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian); int Robot_est_dans_quart_haut_gauche(); int Robot_est_dans_quart_haut_droit(); int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur); int Robot_est_dans_zone_cerise_gauche(); int Robot_est_dans_zone_cerise_droite(); enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms); void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ //const uint32_t temps_pre_fin_match = (97000 - 15000); const uint32_t temps_pre_fin_match = (15000); static bool pre_fin_match_active=false; float angle_fin; float recal_pos_x, recal_pos_y; float point_x, point_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, RETOUR_MAISON, DECISION_STRATEGIE, STRATEGIE_FIN }etat_strategie; if(Monitoring_get_erreur_critique()){ etat_strategie=STRATEGIE_FIN; } if(temps_ms > temps_pre_fin_match){ if(etat_strategie != LANCER_PANIER){ etat_strategie = RETOUR_MAISON; } pre_fin_match_active=true; } switch(etat_strategie){ case STRATEGIE_INIT: if(couleur == COULEUR_BLEU){ Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_GAUCHE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; objectifs[2]= objectif_3; objectifs[3]= objectif_4; }else{ Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_DROITE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; objectifs[2]= objectif_3; objectifs[3]= objectif_4; } objectif_courant = &objectifs[0]; Strategie_set_cerise_dans_robot(10); etat_strategie = LANCER_PANIER; break; case ALLER_CERISE_BAS: if(couleur == COULEUR_BLEU){ angle_fin = 30. * DEGRE_EN_RADIAN; point_x = 857; }else{ angle_fin = -150. * DEGRE_EN_RADIAN; point_x = 2000 - 857; } angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); Trajet_config(250, 250); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 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){ Strategie_set_cerise_dans_robot(6); etat_strategie = ALLER_PANIER; } break; case ALLER_CERISE_HAUT: if(couleur == COULEUR_BLEU){ angle_fin = 30. * DEGRE_EN_RADIAN; point_x = 857; }else{ angle_fin = -150. * DEGRE_EN_RADIAN; point_x = 2000 - 857; } angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); Trajet_config(250, 250); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Localisation_get().angle_radian, angle_fin); if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPER_CERISE_HAUT; } break; case ATTRAPER_CERISE_HAUT: recal_pos_y = 3000 - RAYON_ROBOT; if(couleur == COULEUR_BLEU){ longer_direction = LONGER_VERS_A; recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM; }else{ longer_direction = LONGER_VERS_C; 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){ Strategie_set_cerise_dans_robot(6); etat_strategie = ALLER_PANIER; } break; case ALLER_CERISE_GAUCHE: Trajet_config(250, 250); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); if(couleur == COULEUR_BLEU){ if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_GAUCHE; } }else{ if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_GAUCHE; } } if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_GAUCHE; } break; case ATTRAPER_CERISE_GAUCHE: if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){ Strategie_set_cerise_dans_robot(10); etat_strategie = ALLER_PANIER; } break; case ALLER_CERISE_DROITE: Trajet_config(250, 250); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); if(couleur == COULEUR_BLEU){ if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_DROITE; } }else{ if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_DROITE; } } if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_GAUCHE; } break; case ATTRAPER_CERISE_DROITE: if(cerises_attraper_cerises_droite(step_ms) == ACTION_TERMINEE){ Strategie_set_cerise_dans_robot(10); etat_strategie = ALLER_PANIER; } break; case ALLER_PANIER: if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){ etat_strategie = LANCER_PANIER; } break; case LANCER_PANIER: if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){ Score_ajout_cerise(Strategie_get_cerise_dans_robot()); Strategie_set_cerise_dans_robot(0); objectif_courant->etat = FAIT; etat_strategie = DECISION_STRATEGIE; } break; case DECISION_STRATEGIE: // Obtenir l'objectif suivant for(uint m_objectif=0; m_objectif < NB_OBJECTIFS; m_objectif++){ if(objectif_courant->etat == FAIT && objectifs[m_objectif].etat == A_FAIRE){ objectif_courant = &(objectifs[m_objectif]); }else if(objectif_courant->etat == A_FAIRE && objectifs[m_objectif].etat == A_FAIRE){ if(objectif_courant->priorite > objectifs[m_objectif].priorite){ objectif_courant = &(objectifs[m_objectif]); } } } if(objectif_courant->etat == FAIT || pre_fin_match_active){ etat_strategie = STRATEGIE_FIN; }else{ switch(objectif_courant->cible){ case CERISE_BAS: etat_strategie = ALLER_CERISE_BAS; break; case CERISE_HAUT: etat_strategie = ALLER_CERISE_HAUT; break; case CERISE_GAUCHE: etat_strategie = ALLER_CERISE_GAUCHE; break; case CERISE_DROITE: etat_strategie = ALLER_CERISE_DROITE; break; } } break; case STRATEGIE_FIN: i2c_annexe_desactive_propulseur(); commande_vitesse_stop(); i2c_annexe_couleur_balise(0xE0, 0x0FFF); break; } } enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){ struct trajectoire_t trajectoire; float angle_fin; if(couleur == COULEUR_BLEU){ angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 740, 3000 - 550, 510, 3000 - 1580, 500, 1400, Localisation_get().angle_radian, angle_fin); }else{ angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 2000 - 740, 3000 - 550, 2000 - 510, 3000 - 1580, 2000 - 500, 1400, Localisation_get().angle_radian, angle_fin); } return parcourt_trajet_simple(trajectoire, step_ms); } enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms){ struct trajectoire_t trajectoire; float angle_fin; if(couleur == COULEUR_BLEU){ angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 785, 3000 - 550, 1550, 3000 - 785, 1775, 1500, Localisation_get().angle_radian, angle_fin); }else{ angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 2000 - 785, 3000 - 550, 2000 - 1550, 3000 - 785, 2000 - 1775, 1500, Localisation_get().angle_radian, angle_fin); } return parcourt_trajet_simple(trajectoire, step_ms); } enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; struct trajectoire_t trajectoire; Trajet_config(250, 250); // Definition des trajectoires if(couleur == COULEUR_BLEU){ // Si le robot est déjà dans la zone cerise haut // On va en ligne droite if(Robot_est_dans_zone_cerise_haut(couleur)){ Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 180,2800, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); }else if (Robot_est_dans_zone_cerise_gauche()){ Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm + 330, Localisation_get().y_mm - 100, 745, 3000 - 475, 180, 3000 - 200, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); }else{ // Sinon, on a une courbe de bézier Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 485, Localisation_get().y_mm, 465, 857, 465,2830, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); } }else{ // COULEUR_VERT // Si le robot est déjà dans le quart haut droit, // On va en ligne droite if(Robot_est_dans_quart_haut_droit()){ Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 2000-180, 2800, Localisation_get().angle_radian ,Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); }else if (Robot_est_dans_zone_cerise_droite()){ Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm + 330, Localisation_get().y_mm - 100, 2000 - 745, 3000 - 475, 2000 - 180, 3000 - 200, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); }else{ // Sinon, on a une courbe de bézier Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 2000-485, Localisation_get().y_mm, 2000-465, 857, 2000-465, 2830, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); } } // parcours du trajet if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_action = ACTION_TERMINEE; } return etat_action; } /// Fonction de localisation approximatives pour la stratégie. /// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain /// @return 1 si oui, 0 si non. int Robot_est_dans_quart_haut_gauche(){ if(Localisation_get().x_mm < 1000 && Localisation_get().y_mm > 1500){ return 1; } return 0; } /// @brief Renvoi 1 si le robot est dans le quart supérieur droit du terrain /// @return 1 si oui, 0 si non. int Robot_est_dans_quart_haut_droit(){ if(Localisation_get().x_mm > 1000 && Localisation_get().y_mm > 1500){ return 1; } return 0; } int Robot_est_dans_zone_cerise_gauche(){ if(Localisation_get().x_mm < 630 && Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){ return 1; } return 0; } int Robot_est_dans_zone_cerise_droite(){ if(Localisation_get().x_mm > 2000 - 630 && Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){ return 1; } return 0; } int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){ if(couleur == COULEUR_BLEU){ if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){ return 1; } }else{ if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm > 1000){ return 1; } } return 0; } /// @brief Renvoi 1 si le robot intersect une zone de dépose /// @param couleur du robot /// @return 1 si oui, 0 si non. int Robot_est_dans_zone_depose(enum couleur_t couleur){ float x_mm, y_mm; x_mm = Localisation_get().x_mm; y_mm = Localisation_get().y_mm; if(couleur == COULEUR_BLEU){ // Zone 1 if(x_mm < 550 && y_mm > 2450){ return 1; } // Zone 2 if(x_mm < 550 && y_mm > 800 && y_mm < 1450){ return 1; } // Zone 3 if(x_mm > 400 && x_mm < 1050 && y_mm < 550){ return 1; } // Zone 4 if(x_mm > 1450 && y_mm < 550){ return 1; } // Zone 5 if(x_mm > 1450 && y_mm > 1550 && y_mm < 2200){ return 1; } return 0; }else{ // VERT // Zone 1 if(x_mm > (2000 - 550) && y_mm > 2450){ return 1; } // Zone 2 if(x_mm > (2000 - 550) && y_mm > 800 && y_mm < 1450){ return 1; } // Zone 3 if(x_mm < (2000 - 400) && x_mm > 2000 - 1050 && y_mm < 550){ return 1; } // Zone 4 if(x_mm < (2000 - 1450) && y_mm < 550){ return 1; } // Zone 5 if(x_mm < (2000 - 1450) && y_mm > 1550 && y_mm < 2200){ return 1; } return 0; } } enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){ static enum { CALAGE_PANIER_1, RECULE_PANIER, LANCE_DANS_PANIER, }etat_lance_balles_dans_panier; float recal_pos_x, recal_pos_y; float angle_depart, angle_arrivee; 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 - (PETIT_RAYON_ROBOT_MM), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = RECULE_PANIER; } break; case RECULE_PANIER: Trajet_config(120, 250); 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; } angle_depart = Localisation_get().angle_radian; angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, point_y, angle_depart, angle_arrivee); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = LANCE_DANS_PANIER; Trajet_config(250, 250); } break; case LANCE_DANS_PANIER: Asser_Position_maintien(); if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){ etat_action = ACTION_TERMINEE; etat_lance_balles_dans_panier = CALAGE_PANIER_1; } 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, uint32_t nb_cerises){ 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 > nb_cerises){ 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 = 500; } break; } return etat_action; } /// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation /// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites 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; struct trajectoire_t trajectoire; static float y_pos_ref; static enum { CONTACT_AVANT, RECULE_BORDURE, CONTACT_LATERAL, RECALE_X }etat_calage_angle; switch(etat_calage_angle){ case CONTACT_AVANT: if(cerise_accostage() == ACTION_TERMINEE){ position = Localisation_get(); //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); //} y_pos_ref = Localisation_get().y_mm; etat_calage_angle = RECULE_BORDURE; } break; case RECULE_BORDURE: commande_translation_recule_vers_trompe(); position = Localisation_get(); if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){ etat_calage_angle = CONTACT_LATERAL; } break; case CONTACT_LATERAL: if(longer_direction == LONGER_VERS_A){ commande_translation_avance_vers_A(); if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){ etat_calage_angle = RECALE_X; } }else{ commande_translation_avance_vers_C(); if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){ etat_calage_angle = RECALE_X; } } break; case RECALE_X: etat_action = ACTION_TERMINEE; commande_vitesse_stop(); position = Localisation_get(); //if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){ Localisation_set_x(x_mm); //} etat_calage_angle = CONTACT_AVANT; 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; } } /// @brief Gère le chargement des balles au début du match enum etat_action_t Strategie_preparation(){ static enum{ PREP_INIT_V, PREP_INIT_B, PREP_ASPIRE, PREP_ARRET_ASPIRATION, PREP_TEMPO, } etat_prep=PREP_INIT_V; switch(etat_prep){ case PREP_INIT_V: if(lire_couleur()== COULEUR_VERT){ etat_prep = PREP_INIT_B; } break; case PREP_INIT_B: if(lire_couleur()== COULEUR_BLEU){ etat_prep = PREP_ASPIRE; } break; case PREP_ASPIRE: if(lire_couleur()== COULEUR_VERT){ i2c_annexe_ferme_porte(); i2c_annexe_active_turbine(); etat_prep=PREP_ARRET_ASPIRATION; } break; case PREP_ARRET_ASPIRATION: if(lire_couleur()== COULEUR_BLEU){ i2c_annexe_desactive_turbine(); etat_prep=PREP_TEMPO; } break; case PREP_TEMPO: return ACTION_TERMINEE; } return ACTION_EN_COURS; } enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms){ struct trajectoire_t trajectoire; Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, pos_x, pos_y, Localisation_get().angle_radian, Localisation_get().angle_radian); return Strategie_parcourir_trajet(trajectoire, step_ms, ARRET_DEVANT_OBSTACLE); } enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){ static struct objectif_t objectifs_plats[5], *objectif_plat_courant = &objectifs_plats[0]; enum etat_action_t etat_action; struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1}; struct objectif_t objectif_2 = { .priorite = 4, .etat = A_FAIRE, .cible = ZONE_2}; struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_3}; struct objectif_t objectif_4 = { .priorite = 2, .etat = A_FAIRE, .cible = ZONE_4}; struct objectif_t objectif_5 = { .priorite = 5, .etat = A_FAIRE, .cible = ZONE_5}; objectifs_plats[0] = objectif_1; objectifs_plats[1] = objectif_2; objectifs_plats[2] = objectif_3; objectifs_plats[3] = objectif_4; objectifs_plats[4] = objectif_5; etat_action = Strategie_pieds_dans_plat_trajet(objectif_plat_courant, couleur, step_ms); switch(etat_action){ case ACTION_TERMINEE: return ACTION_TERMINEE; case ACTION_EN_COURS: return ACTION_EN_COURS; case ACTION_ECHEC: // 1. Marquer comme bloqué l'objectif en cours objectif_plat_courant->etat = BLOQUE; // 2. Si tous les objectifs sont bloqués, les marquer tous comme A_FAIRE uint8_t non_bloque = 0; for(uint i=0 ; i<5 ; i++){ if(objectifs_plats[i].etat == A_FAIRE){ non_bloque=1; } } if(!non_bloque){ for(uint i=0 ; i<5 ; i++){ if (objectifs_plats[i].etat == BLOQUE){ objectifs_plats[i].etat = A_FAIRE; } } } // 3. Trouver le prochain objectif (priorité la plus basse + etat A_FAIRE) // Si notre objectif est FAIT, on prend le premier objectif "A_FAIRE" // Si notre objectif est A_FAIRE, on prend le nouvel objectif que si sa priorité est plus basse. for(uint i=0; i < 5; i++){ if(objectif_plat_courant->etat == FAIT && objectifs_plats[i].etat == A_FAIRE){ objectif_plat_courant = &(objectifs_plats[i]); }else if(objectif_plat_courant->etat == A_FAIRE && objectifs_plats[i].etat == A_FAIRE){ if(objectif_plat_courant->priorite > objectifs_plats[i].priorite){ objectif_plat_courant = &(objectifs_plats[i]); } } } } } enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms){ float pos_x; float pos_y; switch (objectif_plat_courant->cible){ case ZONE_1: pos_y = 2775; if (couleur == COULEUR_BLEU){ pos_x = 250; }else{ pos_x = 2000 - 250; } return Strategie_aller_a(pos_x, pos_y, step_ms); case ZONE_2: pos_y = 1125; if (couleur == COULEUR_BLEU){ pos_x = 250; }else{ pos_x = 2000 - 250; } return Strategie_aller_a(pos_x, pos_y, step_ms); case ZONE_3: pos_y = 250; if (couleur == COULEUR_BLEU){ pos_x = 725; }else{ pos_x = 2000 - 725; } return Strategie_aller_a(pos_x, pos_y, step_ms); case ZONE_4: pos_y = 250; if (couleur == COULEUR_BLEU){ pos_x = 2000 - 250; }else{ pos_x = 250; } return Strategie_aller_a(pos_x, pos_y, step_ms); case ZONE_5: pos_y = 1850; if (couleur == COULEUR_BLEU){ pos_x = 2000 - 250; }else{ pos_x = 250; } return Strategie_aller_a(pos_x, pos_y, step_ms); } }