From 3e37992ee2a661fbf989746f5fb4ef7c405be050 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 12 Apr 2025 14:16:30 +0200 Subject: [PATCH] =?UTF-8?q?Gestion=20des=20configurations=20des=20balises?= =?UTF-8?q?=20+=20d=C3=A9but=20d'un=20traitement=20des=20donn=C3=A9es=20de?= =?UTF-8?q?=20la=20detections=20de=20l'adversaire?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Cerveau/Cerveau.ino | 101 ++++++++++++++++++++++++++++---- Cerveau/Com_triangulation.ino | 9 +++ Triangulation/Triangulation.ino | 42 +++++++++++-- 3 files changed, 136 insertions(+), 16 deletions(-) diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index d2f14d5..9caa08f 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -21,6 +21,9 @@ #define VITESSE_STANDARD 1000 #define ACCELERATION_STANDARD 500 +#define COULEUR_BLEU 0 +#define COULEUR_JAUNE 1 + #define gst_server; struct triangulation_reception_t { @@ -41,7 +44,8 @@ IPAddress subnet(255, 255, 255, 0); // IPAddress gateway(192, 168, 1, 1); // IPAddress subnet(255, 255, 255, 0); - +const unsigned char configuration_match_jaune = 0; +const unsigned char configuration_match_bleu = 1; const int16_t I2C_MASTER = 0x42; const int16_t I2C_SLAVE_trian = 0x30; @@ -265,6 +269,7 @@ void gestion_match(){ struct triangulation_reception_t triangulation_reception; static int translation_x_mm, translation_y_mm; static float rotation_rad; + static int couleur; enum etat_strategie{ ATTENTE_ORDRE=0, @@ -302,7 +307,7 @@ void gestion_match(){ translation_y_mm = 0; rotation_rad = 0; - //index_Maitre = DEPLACEMENT_RELATIF; + index_Maitre = DEPLACEMENT_RELATIF; Scan_Triangulation(&triangulation_reception); } if(M5.BtnB.read() == 1){ @@ -319,6 +324,11 @@ void gestion_match(){ translation_y_mm = 0; rotation_rad = 100; Triangulation_send_immobile(1); + while(M5.BtnC.read()){ + M5.update(); + } + delay(200); + IHM_attente_match(&couleur); index_Maitre = MATCH_EN_COURS; @@ -345,7 +355,7 @@ void gestion_match(){ break; case MATCH_EN_COURS: - if(Strategie() == ACTION_TERMINEE){ + if(Strategie(couleur) == ACTION_TERMINEE){ index_Maitre = ATTENTE_ORDRE; } break; @@ -358,7 +368,30 @@ void gestion_match(){ } } -enum etat_action_t Strategie(void){ +void IHM_attente_match(int * couleur){ + int pret = 0; + int m_couleur = COULEUR_BLEU; + affiche_msg("Choix couleur", "En attente"); + while(!pret){ + M5.update(); + if(M5.BtnA.read() == 1){ + affiche_msg("Couleur", " BLEU "); + Triangulation_send_config(configuration_match_bleu); + m_couleur = COULEUR_BLEU; + } + if(M5.BtnB.read() == 1){ + affiche_msg("Couleur", " JAUNE "); + Triangulation_send_config(configuration_match_jaune); + m_couleur = COULEUR_JAUNE; + } + if(M5.BtnC.read() == 1){ + pret = 1; + *couleur = m_couleur; + } + } +} + +enum etat_action_t Strategie(int couleur){ static enum { STRAT_RECULE_BANDEROLE, // Deplacement relatif STRAT_ALLER_GRADINS_1_A, // Déplacement absolu @@ -378,14 +411,18 @@ enum etat_action_t Strategie(void){ translation_x_mm = -450; translation_y_mm = 0; rotation_rad = 0; - etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 0); + etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1); if(etat_action == ACTION_TERMINEE){ etat_strategie = STRAT_ALLER_GRADINS_1_A; } break; case STRAT_ALLER_GRADINS_1_A: - etat_action = deplacement_absolu(800, 800, -M_PI/2., 0); + if(couleur == COULEUR_JAUNE){ + etat_action = deplacement_absolu(800, 800, -M_PI/2., 1); + }else{ + etat_action = deplacement_absolu(3000 - 800, 800, -M_PI/2., 1); + } if(etat_action == ACTION_TERMINEE){ etat_strategie = STRAT_ALLER_GRADINS_1_B; } @@ -406,7 +443,11 @@ enum etat_action_t Strategie(void){ break; case STRAT_ALLER_PREPA_BACKSTAGE: - etat_action = deplacement_absolu(550, 1150, M_PI/2., 0); + if(couleur == COULEUR_JAUNE){ + etat_action = deplacement_absolu(550, 1150, M_PI/2., 0); + }else{ + etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0); + } if(etat_action == ACTION_TERMINEE){ etat_strategie = STRAT_ALLER_BACKSTAGE; } @@ -532,6 +573,36 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, floa return ACTION_EN_COURS; } +/// @brief Récupère les données de la carte de détection de l'advesraire et renoi 1 s'il y aun adversaire +/// Test les capteurs dans la direction d'avancement +/// @param angle_deplacement direction dans laquelle avance le robot, dans le référentiel du robot +int detection_adversaire(float angle_deplacement){ + int capteur_central, capteur_precedant, capteur_suivant; + // On ramène l'angle entre 0 et 2 PI. + while(angle_deplacement < 0){ + angle_deplacement += 2 * M_PI; + } + while(angle_deplacement > 2 * M_PI){ + angle_deplacement -= 2 * M_PI; + } + // On obtient le capteur central. + capteur_central = angle_deplacement / (M_PI * 2) * 12; + capteur_precedant = capteur_central - 1; + if(capteur_precedant < 0){ + capteur_precedant = 11; + } + capteur_suivant = capteur_central + 1; + if(capteur_suivant > 11){ + capteur_suivant = 0; + } + if(detect_adv_reception.distance_cm[capteur_central] < 50 || + detect_adv_reception.distance_cm[capteur_precedant] < 50 || + detect_adv_reception.distance_cm[capteur_suivant] < 50 ){ + return 1; + } + return 0; +} + /// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire /// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){ @@ -563,10 +634,18 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo // On lit les capteurs Detect_adv_lire(&detect_adv_reception); // On analyse les valeurs - TODO : créer une fonction plus évoluée - if(detect_adv_reception.distance_cm[0] < 50){ - // Arrêt du mouvement - chassis_emission.status = MOUVEMENT_INTERRUPTION; - send_Chassis(&chassis_emission); + if(distance_x_mm > 0){ + if(detect_adv_reception.distance_cm[0] < 50){ + // Arrêt du mouvement + chassis_emission.status = MOUVEMENT_INTERRUPTION; + send_Chassis(&chassis_emission); + } + }else if(distance_x_mm < 0){ + if(detect_adv_reception.distance_cm[6] < 50){ + // Arrêt du mouvement + chassis_emission.status = MOUVEMENT_INTERRUPTION; + send_Chassis(&chassis_emission); + } } } diff --git a/Cerveau/Com_triangulation.ino b/Cerveau/Com_triangulation.ino index 03b01b5..3a3f548 100644 --- a/Cerveau/Com_triangulation.ino +++ b/Cerveau/Com_triangulation.ino @@ -47,4 +47,13 @@ void Triangulation_send_immobile(int immobile){ affiche_erreur("Send_Triangulation", "Erreur I2C"); while(1); } +} + + +void Triangulation_send_config(unsigned char configuration){ + error = I2C_ecrire_registre(I2C_SLAVE_trian, 14, &configuration, 1); // si errror != de 0 alors erreur de communication + if (error !=0){ + affiche_erreur("Send_Triangulation", "Erreur I2C"); + while(1); + } } \ No newline at end of file diff --git a/Triangulation/Triangulation.ino b/Triangulation/Triangulation.ino index a5d3e59..934a83d 100644 --- a/Triangulation/Triangulation.ino +++ b/Triangulation/Triangulation.ino @@ -20,7 +20,7 @@ String Lecture; #define pi2 6.283185307179586 #define ALIGNEMENT_RAD (245. / 180. *M_PI) -uint8_t * data_i2C; +uint8_t * data_i2c; int Balise[4][2]; // 4 balises potentielles i de 0 a 3 // Frequence modulation individuelle balises // Balise[i][0] est la frequence de la balise @@ -51,6 +51,8 @@ int Frequence_0 = 5000; // frequence balise 0 int Frequence_1 = 6000; // frequence balise 1 int Frequence_2 = 4000; // frequence balise 2 int Bande_P = 200; // bande passante balises + +// Attention, les absisces des balises sont redéfines plus bas (dans loop) int X1 = -90; // abcisse balise 0 int X2 = -90; // abcisse balise 1 int X3 = 3090; // abcisse balise 2 @@ -194,8 +196,8 @@ void setup() { WiFi.begin(ssid, password); // Pour accéder aux données de l'I2C - data_i2C = get_i2c_data(); - data_i2C[13] = 1; // On dit que le robot est immobile. + data_i2c = get_i2c_data(); + data_i2c[13] = 1; // On dit que le robot est immobile. int test_wifi = 0; @@ -277,8 +279,9 @@ void setup() { } void loop() { - //Effectue a chaque tour ........................ - if(data_i2C[13] != 1){ + + // On invalide les balises si le robot bouge + if(data_i2c[13] != 1){ Nb_Balises = 0; Balise_0 = false; Balise_1 = false; @@ -287,6 +290,35 @@ void loop() { uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3); I2C_envoi_8bits(etat_balises,0); } + + // Configuration des balises + if(data_i2c[14] == 0){ + // Match côté Jaune + X1 = -90; // abcisse balise 0 + X2 = -90; // abcisse balise 1 + X3 = 3090; // abcisse balise 2 + Y1 = 1950; // ordonnee balise 0 + Y2 = 50; // ordonnee balise 1 + Y3 = 1000; // ordonnee balise 2 + Xp1 = X1 - X2; + Yp1 = Y1 - Y2; + Xp3 = X3 - X2; + Yp3 = Y3 - Y2; + }else if(data_i2c[14] == 1){ + // Match côté Jaune + X1 = 3090; // abcisse balise 0 + X2 = 3090; // abcisse balise 1 + X3 = -90; // abcisse balise 2 + Y1 = 1950; // ordonnee balise 0 + Y2 = 50; // ordonnee balise 1 + Y3 = 1000; // ordonnee balise 2 + Xp1 = X1 - X2; + Yp1 = Y1 - Y2; + Xp3 = X3 - X2; + Yp3 = Y3 - Y2; + } + + //Effectue a chaque tour ........................ if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){ Old_Nb_tours = Nb_tours; if (!Balise_Valide){