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

Calcul de l'orientation du robot

Browse Source
This commit is contained in:
Samuel 2025-04-01 18:37:28 +02:00
parent bce282cc61
commit a7be11a7b1
2 changed files with 30 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Cerveau/Cerveau.ino
View File

@ -419,7 +419,7 @@ void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_ori
if(abs(compar_rotation) > tolerance_orientation) { if(abs(compar_rotation) > tolerance_orientation) {
chassis_emission->translation_x_mm = 0; chassis_emission->translation_x_mm = 0;
chassis_emission->translation_y_mm = 0; chassis_emission->translation_y_mm = 0;
chassis_emission->rotation_z_rad = compar_rotation; chassis_emission->rotation_z_rad = -compar_rotation;
chassis_emission->status = MOUVEMENT_EN_COURS; chassis_emission->status = MOUVEMENT_EN_COURS;
}else{ }else{
chassis_emission->status = MOUVEMENT_FINI; chassis_emission->status = MOUVEMENT_FINI;

38
Triangulation/Triangulation.ino
View File

@ -18,6 +18,7 @@ String Lecture;
#define CONV2 1.570796326794896619 #define CONV2 1.570796326794896619
#define pi 3.141592653589793 #define pi 3.141592653589793
#define pi2 6.283185307179586 #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 int Balise[4][2]; // 4 balises potentielles i de 0 a 3
@ -292,7 +293,7 @@ void loop() {
Balise_Valide = true; Balise_Valide = true;
rapport(); rapport();
} }
/*
//_____________________________________________ gestion I2C et Data sent //_____________________________________________ gestion I2C et Data sent
if(Xr>0){ if(Xr>0){
@ -309,18 +310,37 @@ void loop() {
Angle_Robot_RAD = calc2/360*pi2; // en radian Angle_Robot_RAD = calc2/360*pi2; // en radian
Angle_Robot_Deg_int = calc2; // arrondi des degres dans un entier Angle_Robot_Deg_int = calc2; // arrondi des degres dans un entier
Angle_Robot_Deg = calc2; // angle d'orientation robot en degres (0 sur l'axe Y) Angle_Robot_Deg = calc2; // angle d'orientation robot en degres (0 sur l'axe Y)
} }*/
uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3); uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
//_____________________________________________
if (Nb_Balises == 3){
traitement_donnees(); //calcul des coordonnees si 3 balises
// Calcul de l'orientation du robot
// * Angle du vecteur Robot - Balise
float angle_vecteur_robot_balise;
Angle_B1 = Balise[0][1] * CONV; // lorsque le robot est orienté vers la balise, angle codeur = 0
angle_vecteur_robot_balise = atan2f(X1 - Xr, Y1 - Yr);
Angle_Robot_RAD = -(angle_vecteur_robot_balise + Angle_B1 - ALIGNEMENT_RAD);
while(Angle_Robot_RAD > M_PI){
Angle_Robot_RAD -= 2 * M_PI;
}
while(Angle_Robot_RAD < -M_PI){
Angle_Robot_RAD += 2 * M_PI;
}
Angle_Robot_Deg_int = Angle_Robot_RAD * 180 / M_PI;
}
I2C_envoi_8bits(etat_balises,0); I2C_envoi_8bits(etat_balises,0);
// TODO: Mettre Xr et Yr dans la mémoire I2C ################################ // TODO: Mettre Xr et Yr dans la mémoire I2C ################################
I2C_envoi_32bits(Xr, 1); I2C_envoi_32bits(Xr, 1);
I2C_envoi_32bits(Yr, 5); I2C_envoi_32bits(Yr, 5);
//I2C_envoi_32bits(Angle_Robot_RAD, 9); //I2C_envoi_32bits(Angle_Robot_RAD, 9);
I2C_envoi_32bits(Angle_Robot_Deg_int, 9); I2C_envoi_32bits(Angle_Robot_Deg_int, 9);
//_____________________________________________
if (Nb_Balises == 3) traitement_donnees(); //calcul des coordonnees si 3 balises
affichage_resultats(); affichage_resultats();
checkForClient(); checkForClient();
Nb_Balises = 0; Nb_Balises = 0;
@ -401,12 +421,12 @@ void affichage_resultats() {
//M5.Lcd.setCursor(200,150); //M5.Lcd.setCursor(200,150);
//M5.Lcd.print(Balise[2][0]); //M5.Lcd.print(Balise[2][0]);
//M5.Lcd.print(" "); //M5.Lcd.print(" ");
M5.Lcd.setCursor(150,60); M5.Lcd.setCursor(140,60);
M5.Lcd.print(Angle_B1); M5.Lcd.print(Angle_B1);
M5.Lcd.setCursor(150,80); M5.Lcd.setCursor(140,80);
M5.Lcd.print(Angle_Ref_Theorique); M5.Lcd.print(Angle_Ref_Theorique);
M5.Lcd.setCursor(150,100); M5.Lcd.setCursor(140,100);
M5.Lcd.print(Angle_Robot_Deg); M5.Lcd.print(Angle_Robot_Deg_int);
M5.Lcd.print(" ° "); M5.Lcd.print(" ° ");
M5.Lcd.setCursor(200,180); M5.Lcd.setCursor(200,180);
M5.Lcd.print(Xr); M5.Lcd.print(Xr);