Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Déplacement absolu fonctionnel - grosse bidouille pour avoir une valeur de position fiable : attente de 3 secondes

Browse Source
This commit is contained in:
Samuel 2025-04-01 17:32:34 +02:00
parent 2b6695a2d0
commit f13a5ec458
3 changed files with 40 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
Cerveau/Cerveau.ino
View File

@ -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,

3
Cerveau/Com_triangulation.ino
View File

@ -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;

3
Cerveau/Communication_chassis.h
View File

@ -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;
};