From f13a5ec45845a5cc3a38dfa5582a9ac7c00e476a Mon Sep 17 00:00:00 2001 From: Samuel Date: Tue, 1 Apr 2025 17:32:34 +0200 Subject: [PATCH] =?UTF-8?q?D=C3=A9placement=20absolu=20fonctionnel=20-=20g?= =?UTF-8?q?rosse=20bidouille=20pour=20avoir=20une=20valeur=20de=20position?= =?UTF-8?q?=20fiable=20:=20attente=20de=203=20secondes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Cerveau/Cerveau.ino | 48 ++++++++++++++++++++++++--------- Cerveau/Com_triangulation.ino | 3 ++- Cerveau/Communication_chassis.h | 3 ++- 3 files changed, 40 insertions(+), 14 deletions(-) diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 123af75..038b6f9 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -12,6 +12,8 @@ #define Rad_Deg 57.2957795130823 +#define MOUVEMEMENT_ROTATION 0x10 +#define MOUVEMEMENT_TRANSLATION 0x8 #define MOUVEMENT_FINI 0x4 #define MOUVEMENT_EN_COURS 0x2 #define MOUVEMENT_INTERRUPTION 0x1 @@ -90,6 +92,7 @@ int index_Maitre = 0; bool Mvt_tolerance_OK =false; bool Balises_OK = 0; int tolerance_position =100; +float tolerance_orientation =0.05; // 3° char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"}; @@ -293,7 +296,7 @@ void gestion_match(){ index_Maitre = DEPLACEMENT_RELATIF; } if(M5.BtnA.read() == 1){ - Serial.println("BtnB"); + Serial.println("BtnA"); // Déplacement en X translation_x_mm = 500; translation_y_mm = 0; @@ -353,15 +356,31 @@ void gestion_match(){ } /// @brief : compare la position actuelle et la position lue par la balise /// Note : Pour l'instant, on ne déclenche un mouvment qu'en cas d'ecart sur la distance, pas sur l'orientation. -void compar_cinematique(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad, +void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_orientation_rad, struct triangulation_reception_t triangulation_reception, struct chassis_emission_t * chassis_emission){ + + float compar_rotation; compar_X = consigne_x_mm - triangulation_reception.pos_x_mm; //compar de la position théoriquement atteinte avec la position actuel compar_Y = consigne_y_mm - triangulation_reception.pos_y_mm; //YR : position actuel Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique) - printf("compar_X:%d\ncompar_y:%d\n", compar_X, compar_Y); + compar_rotation = consigne_orientation_rad - triangulation_reception.angle_rad; + while(compar_rotation < -M_PI){ + compar_rotation += 2* M_PI; + } + while(compar_rotation > M_PI){ + compar_rotation -= 2* M_PI; + } + printf("compar_X:%d\tcompar_y:%d\tcompar_rot:%f\n", compar_X, compar_Y, compar_rotation); if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){ - chassis_emission->status = MOUVEMENT_FINI; + if(abs(compar_rotation) > tolerance_orientation) { + chassis_emission->translation_x_mm = 0; + chassis_emission->translation_y_mm = 0; + chassis_emission->rotation_z_rad = compar_rotation; + chassis_emission->status = MOUVEMENT_EN_COURS; + }else{ + chassis_emission->status = MOUVEMENT_FINI; + } }else{ // print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue @@ -380,7 +399,8 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int static enum{ DA_INIT, DA_COMPARE_POSITIONS, - DA_MVT_EN_COUR, + DA_MVT_EN_COURS, + DA_ATTENTE, } etat_deplacement = DA_INIT; static int mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad; static struct chassis_emission_t chassis_emission; @@ -397,7 +417,6 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int break; case DA_COMPARE_POSITIONS: - Serial.printf("Scan_Triangulation\n"); Scan_Triangulation(&triangulation_reception); //Prise de la position actuel if(triangulation_reception.validite == true){ Serial.printf("Compare cinematique\n"); @@ -407,11 +426,11 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int // C'est que la fonction compar_cinematique indique qu'on doit se déplacer // Les valeurs du déplacement sont renseignées dans "chassis_emission". Serial.printf("DA_MVT_EN_COUR\n"); - Serial.printf("pos_x:%d\tpos_y:%d\tOrientation:%d\n",triangulation_reception.pos_x_mm, triangulation_reception.pos_y_mm, + Serial.printf("pos_x:%d\tpos_y:%d\tOrientation:%f\n",triangulation_reception.pos_x_mm, triangulation_reception.pos_y_mm, triangulation_reception.angle_rad); - Serial.printf("trans_x:%d\ttrans_y:%d\trot:%d\n",chassis_emission.translation_x_mm, + Serial.printf("trans_x:%d\ttrans_y:%d\trot:%f\n",chassis_emission.translation_x_mm, chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad); - etat_deplacement = DA_MVT_EN_COUR; + etat_deplacement = DA_MVT_EN_COURS; }else{ // Alors nous sommes arrivés // On réinitialise la mahcine à état @@ -421,15 +440,20 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int } break; - case DA_MVT_EN_COUR: + case DA_MVT_EN_COURS: Scan_Triangulation(&triangulation_reception); //Prise de la position actuel etat_deplacement_relatif = deplacement_relatif(- chassis_emission.translation_x_mm, -chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad, evitement); if(etat_deplacement_relatif == ACTION_TERMINEE){ Serial.printf("DA_COMPARE_POSITIONS\n"); - etat_deplacement = DA_COMPARE_POSITIONS; + etat_deplacement = DA_ATTENTE; } break; + + case DA_ATTENTE: + delay(3000); + etat_deplacement = DA_COMPARE_POSITIONS; + break; } return ACTION_EN_COURS; @@ -437,7 +461,7 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int /// @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, int rotation_rad, int evitement){ +enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){ static enum{ DR_INIT, DR_MVT_EN_COUR, diff --git a/Cerveau/Com_triangulation.ino b/Cerveau/Com_triangulation.ino index 87e7947..03b01b5 100644 --- a/Cerveau/Com_triangulation.ino +++ b/Cerveau/Com_triangulation.ino @@ -18,7 +18,8 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti Angle_Robot_DEG_int = tampon2[9]<< 24 | tampon2[10]<< 16 | tampon2[11]<< 8 | tampon2[12] ; // Serial.print(tampon2[9], BIN);Serial.print(" ");Serial.print(tampon2[10], BIN);Serial.print(" ");Serial.print(tampon2[11], BIN);Serial.print(" ");Serial.println(tampon2[12], BIN); Angle_Robot_RAD = Angle_Robot_DEG_int * M_PI / 180.; - Serial.printf("Scan triangulation 0: 0x%X\n", tampon2[0]); + triangulation_reception->angle_rad = Angle_Robot_RAD; + lec_Balise_1 = tampon2[0] & 0x01; lec_Balise_2 = (tampon2[0] >>1) & 0x01; lec_Balise_3 = (tampon2[0] >>2 )& 0x01; diff --git a/Cerveau/Communication_chassis.h b/Cerveau/Communication_chassis.h index 5b55d85..2bdc3d4 100644 --- a/Cerveau/Communication_chassis.h +++ b/Cerveau/Communication_chassis.h @@ -9,7 +9,8 @@ struct chassis_reception_t { struct chassis_emission_t { unsigned char status; - int translation_x_mm, translation_y_mm, rotation_z_rad; + int translation_x_mm, translation_y_mm; + float rotation_z_rad; int vitesse, acceleration; };