#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.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); 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); enum etat_action_t Strategie_calage(enum couleur_t couleur); void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ const uint32_t temps_pre_fin_match = (97000 - 10000); 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 && pre_fin_match_active == false){ if(etat_strategie != LANCER_PANIER){ etat_strategie = RETOUR_MAISON; } pre_fin_match_active=true; } switch(etat_strategie){ case STRATEGIE_INIT: if(Strategie_calage_bas(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){ etat_strategie = STRATEGIE_FIN; } break; case RETOUR_MAISON: if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){ etat_strategie=STRATEGIE_FIN; } break; case STRATEGIE_FIN: pre_fin_match_active=true; commande_vitesse_stop(); i2c_annexe_couleur_balise(0xE0, 0x0FFF); break; } } /// 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; } int Robot_est_dans_zone_depose_panier(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; } }else{ // Zone 1 if(x_mm > 2000 - 550 && y_mm > 2450){ 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; } } /// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette uint attente_tirette(void){ return !gpio_get(TIRETTE); } /// @brief Renvoi COULEUR_JAUNE ou COULEUR_BLEU enum couleur_t lire_couleur(void){ if (gpio_get(COULEUR)) return COULEUR_JAUNE; 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; } } enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, enum evitement_t evitement, 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, evitement); } enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,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, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian)); return Strategie_parcourir_trajet(trajectoire, step_ms, evitement); } /// @brief Avance puis tourne /// @param pos_x /// @param pos_y /// @param angle_radian /// @param /// @param step_ms /// @return enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){ struct trajectoire_t trajectoire; static enum { AAPT_ALLER, AAPT_TOURNER, }etat_aller_a_puis_tourner=AAPT_ALLER; switch(etat_aller_a_puis_tourner){ case AAPT_ALLER: Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, pos_x, pos_y, Localisation_get().angle_radian, Localisation_get().angle_radian); if(Strategie_parcourir_trajet(trajectoire, step_ms, evitement) == ACTION_TERMINEE){ etat_aller_a_puis_tourner = AAPT_TOURNER; } break; case AAPT_TOURNER: Trajet_config(TRAJECT_CONFIG_ROTATION_PURE); Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian)); if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_aller_a_puis_tourner = AAPT_ALLER; return ACTION_TERMINEE; } break; } return ACTION_EN_COURS; } enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms){ struct trajectoire_t trajectoire; Trajet_config(TRAJECT_CONFIG_ROTATION_PURE); Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian)); return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT); } 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=NULL; enum etat_action_t etat_action; //Trajet_config(500,500); //750, 500 => Ne marche pas if(objectif_plat_courant==NULL){ objectif_plat_courant = &objectifs_plats[0]; struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1}; struct objectif_t objectif_2 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_2}; struct objectif_t objectif_3 = { .priorite = 5, .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 = 4, .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 == BLOQUE && 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]); } } } return ACTION_EN_COURS; } } 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, EVITEMENT_ARRET_DEVANT_OBSTACLE, 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, EVITEMENT_ARRET_DEVANT_OBSTACLE, 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, EVITEMENT_ARRET_DEVANT_OBSTACLE, 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, EVITEMENT_ARRET_DEVANT_OBSTACLE, 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, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); } } enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms){ // 1 Envoyer la commande pour détecter la bordure // 2 Si la valeur de la bordure est valide, lire l'angle et la distance. // Recaler la distance Y // 3 Se tourner du bon angle (en fonction de la couleur et de l'angle lu) // 4 Si la valeur de la bordure est valide, lire l'angle et la distance. // 5 Se positionner à (250, 250), PINCE orientée à 45°. static enum{ CD_ENVOI_CDE_BORDURE, CD_LECTURE_BORDURE_Y, CD_ROTATION_VERS_X, CD_LECTURE_BORDURE_X, CD_ALLER_POSITION_INIT, CD_ROTATION_POSITION_INIT, }etat_calage_debut=CD_ENVOI_CDE_BORDURE; enum validite_vl53l8_t validite; struct trajectoire_t trajectoire; enum etat_action_t etat_action; static int tempo_ms; float angle, distance; switch(etat_calage_debut){ case CD_ENVOI_CDE_BORDURE: i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN); tempo_ms = 2000; etat_calage_debut = CD_LECTURE_BORDURE_Y; break; case CD_LECTURE_BORDURE_Y: tempo_ms--; i2c_annexe_get_VL53L8(&validite, &angle, &distance); if(validite == VL53L8_DISTANCE_LOIN){ i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); commande_vitesse_stop(); Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR); Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); etat_calage_debut = CD_ROTATION_VERS_X; } if(tempo_ms <= 0){ etat_calage_debut=CD_ENVOI_CDE_BORDURE; return ACTION_ECHEC; } break; case CD_ROTATION_VERS_X: Trajet_config(TRAJECT_CONFIG_ROTATION_PURE); if(couleur == COULEUR_BLEU){ Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, (-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE); }else{ Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, (0 * DEGRE_EN_RADIAN) - ANGLE_PINCE); } if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN); etat_calage_debut = CD_LECTURE_BORDURE_X; tempo_ms = 2000; } break; case CD_LECTURE_BORDURE_X: tempo_ms--; i2c_annexe_get_VL53L8(&validite, &angle, &distance); if(validite == VL53L8_DISTANCE_LOIN){ i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); commande_vitesse_stop(); if(couleur == COULEUR_BLEU){ Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR); Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); }else{ Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR)); Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); } etat_calage_debut = CD_ROTATION_POSITION_INIT; return ACTION_TERMINEE; } if(tempo_ms <= 0){ etat_calage_debut=CD_ENVOI_CDE_BORDURE; return ACTION_ECHEC; } break; } return ACTION_EN_COURS; } enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms){ // Le robot est positionné avec une cale à 70 du bord // Si l'angle avec la bordure est négatif : bleu, sinon jaune // On en déduit X static enum{ CD_ENVOI_CDE_BORDURE, CD_LECTURE_BORDURE_Y, CD_ROTATION_VERS_X, CD_LECTURE_BORDURE_X, CD_ALLER_POSITION_INIT, CD_ROTATION_POSITION_INIT, }etat_calage_debut=CD_ENVOI_CDE_BORDURE; enum validite_vl53l8_t validite; struct trajectoire_t trajectoire; enum etat_action_t etat_action; float angle, distance; switch(etat_calage_debut){ case CD_ENVOI_CDE_BORDURE: i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); etat_calage_debut = CD_LECTURE_BORDURE_Y; break; case CD_LECTURE_BORDURE_Y: i2c_annexe_get_VL53L8(&validite, &angle, &distance); if(validite == VL53L8_BORDURE){ i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); commande_vitesse_stop(); if(couleur == COULEUR_BLEU){ Localisation_set_x(215.); Localisation_set_y(2000 - (distance + DISTANCE_CENTRE_CAPTEUR)); //Localisation_set_angle((90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); Localisation_set_angle((210. * DEGRE_EN_RADIAN)); }else{ Localisation_set_x(3000 - 215.); Localisation_set_y(2000 - (distance + DISTANCE_CENTRE_CAPTEUR)); Localisation_set_angle((270. * DEGRE_EN_RADIAN)); } etat_calage_debut = CD_ALLER_POSITION_INIT; return ACTION_TERMINEE; } break; } return ACTION_EN_COURS; }