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


void Triangulation_send_config(unsigned char configuration){
  error = I2C_ecrire_registre(I2C_SLAVE_trian, 14, &configuration, 1);    // si errror != de 0 alors erreur de communication
  if (error !=0){
    affiche_erreur("Send_Triangulation", "Erreur I2C");
    while(1);
  }
}