#include "hardware/gpio.h" #include "i2c_annexe.h" #include "Asser_Position.h" #include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "Localisation.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{ int priorite; enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat; enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE} cible; } 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(); 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); enum etat_homologation_t etat_homologation=STRATEGIE_INIT; void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ 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, DECISION_STRATEGIE, }etat_strategie; 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 = 2, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_3 = { .priorite = 1, .etat = A_FAIRE, .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 = 2, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_3 = { .priorite = 1, .etat = A_FAIRE, .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; } Trajet_config(250, 500); 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; } Trajet_config(250, 500); 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(300, 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; Trajet_config(500, 500); } 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(300, 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; Trajet_config(500, 500); } 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->priorite > objectifs[m_objectif].priorite){ objectif_courant = &(objectifs[m_objectif]); } } if(objectif_courant->etat == FAIT){ 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, 180, 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 - 180, 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(500, 500); // 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-465, 2830, -150. * DEGRE_EN_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, -150. * DEGRE_EN_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 = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 120. * DEGRE_EN_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(500, 500); } 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 void 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: break; } }