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

Annulation de la validité de la position de la triangulation tant que le robot est en mouvement

Browse Source
This commit is contained in:
Samuel 2025-03-30 21:03:55 +02:00
parent 5fb503883c
commit 2b6695a2d0
7 changed files with 115 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
Cerveau/Cerveau.ino
View File

@ -293,31 +293,31 @@ void gestion_match(){
index_Maitre = DEPLACEMENT_RELATIF;
}
if(M5.BtnA.read() == 1){
Serial.println("BtnB");
// Déplacement en X
translation_x_mm = 2000;
translation_x_mm = 500;
translation_y_mm = 0;
rotation_rad = 0;
index_Maitre = DEPLACEMENT_RELATIF;
//index_Maitre = DEPLACEMENT_RELATIF;
Scan_Triangulation(&triangulation_reception);
}
if(M5.BtnB.read() == 1){
Serial.println("BtnB");
// Déplacement en Y
translation_x_mm = 0;
translation_y_mm = 2000;
rotation_rad = 0;
// Déplacement en Y
//Triangulation_send_immobile(0);
index_Maitre = TEST_DEPLACEMENT_ABSOLU;
}
if(M5.BtnC.read() == 1){
// Rotation
Serial.println("BtnC");
translation_x_mm = 0;
translation_y_mm = 0;
rotation_rad = 100;
Triangulation_send_immobile(1);
index_Maitre = DEPLACEMENT_RELATIF;
//index_Maitre = DEPLACEMENT_RELATIF;
}
break;
@ -345,7 +345,7 @@ void gestion_match(){
break;
case TEST_DEPLACEMENT_ABSOLU:
if(deplacement_absolu(1000, 1000, 0, 0) == ACTION_TERMINEE){
if(deplacement_absolu(800, 800, 0, 0) == ACTION_TERMINEE){
index_Maitre = ATTENTE_ORDRE;
}
break;
@ -407,7 +407,9 @@ 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("trans_x:%d\ntrans_y:%d\nrot:%d\n",chassis_emission.translation_x_mm,
Serial.printf("pos_x:%d\tpos_y:%d\tOrientation:%d\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,
chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad);
etat_deplacement = DA_MVT_EN_COUR;
}else{
@ -420,6 +422,7 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int
break;
case DA_MVT_EN_COUR:
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){
@ -454,6 +457,7 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int
chassis_emission.acceleration = ACCELERATION_STANDARD;
send_Chassis(&chassis_emission);
Triangulation_send_immobile(0);
etat_deplacement = DR_MVT_EN_COUR;
break;
@ -472,6 +476,7 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int
Scan_chassis(&chassis_reception);
if(chassis_reception.status == MOUVEMENT_FINI){
Triangulation_send_immobile(1);
etat_deplacement = DR_INIT;
return ACTION_TERMINEE;
}
@ -481,45 +486,6 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int
return ACTION_EN_COURS;
}
/// @brief
/// @brief Récupère position (X, Y) et l'orientation du robot
void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
unsigned char tampon2[14];
lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
//(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
if (error !=0){
Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
affiche_erreur("Scan_Triangulation", "Erreur I2C");
while(1);
}
else{Err_Tri_com =0;IndexErr = 0;}
if (error ==0){
triangulation_reception->pos_x_mm = tampon2[1]<< 24 | tampon2[2]<< 16 | tampon2[3]<< 8 | tampon2[4] ;
triangulation_reception->pos_y_mm = tampon2[5]<< 24 | tampon2[6]<< 16 | tampon2[7]<< 8 | tampon2[8] ;
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.;
lec_Balise_1 = tampon2[0] & 0x01;
lec_Balise_2 = (tampon2[0] >>1) & 0x01;
lec_Balise_3 = (tampon2[0] >>2 )& 0x01;
lec_Calcul_ok = (tampon2[0] >>3 )& 0x01;
if(Position_actuelle_X < PosLimNeg | Position_actuelle_X > PosLimPos){
triangulation_reception->pos_x_mm = 9999;
}
if(Position_actuelle_Y < PosLimNeg | Position_actuelle_Y > PosLimPos){
triangulation_reception->pos_y_mm = 9999;
}
}
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
triangulation_reception->validite = true;
}
}
void scan_I2C_bus(){
char error, address;

49
Cerveau/Com_triangulation.ino Normal file
View File

@ -0,0 +1,49 @@
/// @brief Récupère position (X, Y) et l'orientation du robot
void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
unsigned char tampon2[14];
lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
//(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
if (error !=0){
Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
affiche_erreur("Scan_Triangulation", "Erreur I2C");
while(1);
}
else{Err_Tri_com =0;IndexErr = 0;}
if (error ==0){
triangulation_reception->pos_x_mm = tampon2[1]<< 24 | tampon2[2]<< 16 | tampon2[3]<< 8 | tampon2[4] ;
triangulation_reception->pos_y_mm = tampon2[5]<< 24 | tampon2[6]<< 16 | tampon2[7]<< 8 | tampon2[8] ;
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]);
lec_Balise_1 = tampon2[0] & 0x01;
lec_Balise_2 = (tampon2[0] >>1) & 0x01;
lec_Balise_3 = (tampon2[0] >>2 )& 0x01;
lec_Calcul_ok = (tampon2[0] >>3 )& 0x01;
if(Position_actuelle_X < PosLimNeg | Position_actuelle_X > PosLimPos){
triangulation_reception->pos_x_mm = 9999;
}
if(Position_actuelle_Y < PosLimNeg | Position_actuelle_Y > PosLimPos){
triangulation_reception->pos_y_mm = 9999;
}
}
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
triangulation_reception->validite = true;
}
}
void Triangulation_send_immobile(int immobile){
unsigned char donnee=0;
if(immobile){
donnee = 1;
}
error = I2C_ecrire_registre(I2C_SLAVE_trian, 13, &donnee, 1); // si errror != de 0 alors erreur de communication
if (error !=0){
affiche_erreur("Send_Triangulation", "Erreur I2C");
while(1);
}
}

20
Cerveau/Communication_chassis.ino
View File

@ -24,16 +24,24 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){
void send_Chassis(struct chassis_emission_t * chassis_emission){
//if(nbr_essai<=10){
// Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet
int nb_pas_x, nb_pas_y, nb_pas_rot;
// Conversion des mm ou radian en pas
nb_pas_x = chassis_emission->translation_x_mm * 4.049;
nb_pas_y = chassis_emission->translation_y_mm * 4.953;
nb_pas_rot = chassis_emission->rotation_z_rad * 791.;
Mot[0] = chassis_emission->status;
//y*=-1;
//y = y*direction;
Mot[1] = chassis_emission->translation_x_mm >>8;
Mot[2] = chassis_emission->translation_x_mm;
Mot[3] = chassis_emission->translation_y_mm >>8;
Mot[4] = chassis_emission->translation_y_mm;
Mot[1] = nb_pas_x >>8;
Mot[2] = nb_pas_x;
Mot[3] = nb_pas_y >>8;
Mot[4] = nb_pas_y;
//Serial.println(y);
Mot[5] = chassis_emission->rotation_z_rad >>8;
Mot[6] = chassis_emission->rotation_z_rad;
Mot[5] = nb_pas_rot >>8;
Mot[6] = nb_pas_rot;
Mot[7] = chassis_emission->vitesse >>8;
Mot[8] = chassis_emission->vitesse;
Mot[9] = chassis_emission->acceleration >>8;

2
Cerveau/ServerWeb.ino
View File

@ -91,7 +91,7 @@ void handleForm() {
// y= myString1.toInt() * coef_mvt/10;
chassis_emission_web.translation_y_mm = myString1.toInt();
String myString2 = server.arg("R"); //positon de cmd en Rotation Deg °
chassis_emission_web.rotation_z_rad = myString2.toInt() * 13.88888;
chassis_emission_web.rotation_z_rad = myString2.toInt() / 180 * M_PI;
String myString3 = server.arg("V"); // Vitesse de cmd en
chassis_emission_web.vitesse = myString3.toInt();
String myString4 = server.arg("A"); // Acceleration de cmd

BIN
Doc/Communication I2C.odt
View File

Binary file not shown.

4
Triangulation/I2C_Slave_lib.ino
View File

@ -5,6 +5,10 @@ byte memoire_I2C_index=0;
bool nouveau_message=false;
uint8_t * get_i2c_data(){
return memoire_I2C;
}
void onRequest(){
uint32_t taille_envoi;
taille_envoi = min (TAILLE_MEMOIRE_I2C-memoire_I2C_index, TAILLE_MESSAGE_ENVOI_MAX);

31
Triangulation/Triangulation.ino
View File

@ -19,6 +19,7 @@ String Lecture;
#define pi 3.141592653589793
#define pi2 6.283185307179586
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
@ -190,7 +191,13 @@ void setup() {
//Initialisation wifi
WiFi.config(local_IP, gateway, subnet);
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.
int test_wifi = 0;
while (WiFi.status() != WL_CONNECTED){
delay(300);
M5.Lcd.print(".");
@ -260,6 +267,8 @@ void setup() {
M5.Lcd.print("a RAD = ");
M5.Lcd.setCursor(0,100);
M5.Lcd.print("angle = ");
M5.Lcd.setCursor(10,120);
M5.Lcd.print("Status : ");
M5.Lcd.setCursor(160,180);
M5.Lcd.print("X = ");
M5.Lcd.setCursor(160,210);
@ -268,6 +277,15 @@ void setup() {
void loop() {
//Effectue a chaque tour ........................
if(data_i2C[13] != 1){
Nb_Balises = 0;
Balise_0 = false;
Balise_1 = false;
Balise_2 = false;
Calcul_Valide = false;
uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
I2C_envoi_8bits(etat_balises,0);
}
if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){
Old_Nb_tours = Nb_tours;
if (!Balise_Valide){
@ -316,6 +334,10 @@ void loop() {
}
vitesse_moteur();
}
// Si le robot n'est pas immobile, on invalide les balises.
//________________________________________
}
void rapport(){
@ -360,6 +382,13 @@ void affichage_resultats() {
M5.Lcd.print(" ");
M5.Lcd.setCursor(140,40);
M5.Lcd.print(Balise[2][1]);
M5.Lcd.print(" ");
M5.Lcd.setCursor(140,120);
M5.Lcd.print(Balise_0);
M5.Lcd.setCursor(150,120);
M5.Lcd.print(Balise_1);
M5.Lcd.setCursor(160,120);
M5.Lcd.print(Balise_2);
// M5.Lcd.print(" / ");
// M5.Lcd.print(Angle_B[2][3]);
M5.Lcd.print(" ");
@ -447,3 +476,5 @@ void checkForClient(){
}
}