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

Deplacement absolu fonctionnel - mais un problème d'echelle mm <=> pas le rend pas très efficace

Browse Source
This commit is contained in:
Samuel 2025-03-26 21:12:07 +01:00
parent 33ff8f17b3
commit 5fb503883c
2 changed files with 92 additions and 43 deletions

134
Cerveau/Cerveau.ino
View File

@ -92,7 +92,7 @@ bool Balises_OK = 0;
int tolerance_position =100; int tolerance_position =100;
char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position"}; char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"};
char* statu[] = {"/..","./.","../"}; char* statu[] = {"/..","./.","../"};
int index_statu=0; int index_statu=0;
@ -152,7 +152,7 @@ void loop() {
static int64_t time; static int64_t time;
struct chassis_reception_t chassis_reception; struct chassis_reception_t chassis_reception;
strategie(); gestion_match();
affichage_resultats(); affichage_resultats();
delay(10); delay(10);
} }
@ -256,7 +256,7 @@ void affiche_msg(char * chaine1, char * chaine2){
M5.Lcd.print(chaine2); M5.Lcd.print(chaine2);
} }
void strategie(){ void gestion_match(){
struct chassis_reception_t chassis_reception; struct chassis_reception_t chassis_reception;
struct chassis_emission_t chassis_emission; struct chassis_emission_t chassis_emission;
struct triangulation_reception_t triangulation_reception; struct triangulation_reception_t triangulation_reception;
@ -267,6 +267,8 @@ void strategie(){
ATTENTE_ORDRE=0, ATTENTE_ORDRE=0,
LECTURE_TRIANGULATION=1, LECTURE_TRIANGULATION=1,
DEPLACEMENT_RELATIF=2, DEPLACEMENT_RELATIF=2,
MATCH_EN_COURS=3,
TEST_DEPLACEMENT_ABSOLU=4,
}; };
switch(index_Maitre){ switch(index_Maitre){
@ -292,19 +294,21 @@ void strategie(){
} }
if(M5.BtnA.read() == 1){ if(M5.BtnA.read() == 1){
// Déplacement en X // Déplacement en X
translation_x_mm = 10000; translation_x_mm = 2000;
translation_y_mm = 0; translation_y_mm = 0;
rotation_rad = 0; rotation_rad = 0;
index_Maitre = DEPLACEMENT_RELATIF; index_Maitre = DEPLACEMENT_RELATIF;
} }
if(M5.BtnB.read() == 1){ if(M5.BtnB.read() == 1){
Serial.println("BtnB");
// Déplacement en Y // Déplacement en Y
translation_x_mm = 0; translation_x_mm = 0;
translation_y_mm = 2000; translation_y_mm = 2000;
rotation_rad = 0; rotation_rad = 0;
index_Maitre = DEPLACEMENT_RELATIF; index_Maitre = TEST_DEPLACEMENT_ABSOLU;
} }
if(M5.BtnC.read() == 1){ if(M5.BtnC.read() == 1){
@ -336,49 +340,96 @@ void strategie(){
index_Maitre = ATTENTE_ORDRE; index_Maitre = ATTENTE_ORDRE;
} }
break; break;
case MATCH_EN_COURS:
break;
case TEST_DEPLACEMENT_ABSOLU:
if(deplacement_absolu(1000, 1000, 0, 0) == ACTION_TERMINEE){
index_Maitre = ATTENTE_ORDRE;
}
break;
}
}
/// @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,
struct triangulation_reception_t triangulation_reception, struct chassis_emission_t * chassis_emission){
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);
if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){
chassis_emission->status = MOUVEMENT_FINI;
}else{
// print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue
// Distance à parcourir
float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y));
float angle_robot_vers_destination = M_PI_2 - atan2(compar_Y, compar_X);
chassis_emission->translation_x_mm = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
chassis_emission->translation_y_mm = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
chassis_emission->rotation_z_rad = 0;
chassis_emission->status = MOUVEMENT_EN_COURS;
} }
} }
void compar_cinematique(){ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad, int evitement){
static enum{
// Consigne de position à atteindre en X, Y et angle par rapport à la position actuel DA_INIT,
// dans le repère du terrain DA_COMPARE_POSITIONS,
//Position à atteindre théorique DA_MVT_EN_COUR,
} 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;
struct triangulation_reception_t triangulation_reception; struct triangulation_reception_t triangulation_reception;
enum etat_action_t etat_deplacement_relatif;
X_futur = MemCmd_X; switch(etat_deplacement){
Y_futur = MemCmd_Y; case DA_INIT:
mem_consigne_x_mm = consigne_x_mm;
mem_consigne_y_mm = consigne_y_mm;
mem_consigne_orientation_rad = consigne_orientation_rad;
etat_deplacement = DA_COMPARE_POSITIONS;
Serial.printf("DA_INIT\n");
break;
Scan_Triangulation(&triangulation_reception); //Prise de la position actuel case DA_COMPARE_POSITIONS:
if(Balises_OK == true && error == 0){ //triangulation calcul valide ************** a prendre sur I2c Serial.printf("Scan_Triangulation\n");
compar_X = X_futur - Position_actuelle_X; //compar de la position théoriquement atteinte avec la position actuel Scan_Triangulation(&triangulation_reception); //Prise de la position actuel
compar_Y = Y_futur - Position_actuelle_Y; //YR : position actuel Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique) if(triangulation_reception.validite == true){
Serial.printf("Compare cinematique\n");
if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){ compar_cinematique(mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad,
Mvt_tolerance_OK = true; triangulation_reception, &chassis_emission);
}else{ if(chassis_emission.status == MOUVEMENT_EN_COURS){
// print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue // 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".
// Distance à parcourir Serial.printf("DA_MVT_EN_COUR\n");
float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y)); Serial.printf("trans_x:%d\ntrans_y:%d\nrot:%d\n",chassis_emission.translation_x_mm,
float angle_robot_vers_destination = M_PI_2 - atan2(compar_Y, compar_X); chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad);
etat_deplacement = DA_MVT_EN_COUR;
float distance_Y_ref_robot = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; }else{
float distance_X_ref_robot = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; // Alors nous sommes arrivés
// On réinitialise la mahcine à état
if(corrige == false){ etat_deplacement = DA_INIT;
mem_x = distance_X_ref_robot; // faire une memoire et travailler avec return ACTION_TERMINEE;
mem_y = distance_Y_ref_robot; }
corrige = true;
} }
Mvt_tolerance_OK = false; break;
}
}else{ case DA_MVT_EN_COUR:
compar_X =0 ; //compar de la position théoriquement atteinte avec la position actuel etat_deplacement_relatif = deplacement_relatif(- chassis_emission.translation_x_mm,
compar_Y =0 ; -chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad, evitement);
mem_x =0 ; //compar de la position théoriquement atteinte avec la position actuel if(etat_deplacement_relatif == ACTION_TERMINEE){
mem_y =0 ; Serial.printf("DA_COMPARE_POSITIONS\n");
etat_deplacement = DA_COMPARE_POSITIONS;
}
break;
} }
return ACTION_EN_COURS;
} }
/// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire /// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
@ -437,6 +488,7 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
unsigned char tampon2[14]; unsigned char tampon2[14];
lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0; lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée) //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
triangulation_reception->validite = false;
error = I2C_lire_registre(I2C_SLAVE_trian, 0, tampon2, 13); // si errror != de 0 alors erreur de communication error = I2C_lire_registre(I2C_SLAVE_trian, 0, tampon2, 13); // si errror != de 0 alors erreur de communication
if (error !=0){ if (error !=0){
Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0; Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
@ -464,8 +516,6 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
} }
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){ if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
triangulation_reception->validite = true; triangulation_reception->validite = true;
}else{
triangulation_reception->validite = false;
} }
} }

1
Cerveau/Communication_chassis.ino
View File

@ -13,7 +13,6 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){
affiche_erreur("Scan_Chassi", "Erreur I2C"); affiche_erreur("Scan_Chassi", "Erreur I2C");
while(1); while(1);
}else{ }else{
Serial.println("I2C OK");
Err_Chassi_com =0; Err_Chassi_com =0;
IndexErr = 0; IndexErr = 0;