50 lines
2.2 KiB
C++
50 lines
2.2 KiB
C++
/// @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.;
|
|
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;
|
|
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);
|
|
}
|
|
} |