//#include "Chassis.h"
#include <Arduino.h>
#include <HardwareSerial.h>
#include "Com_gradins.h"


/// @brief Lit les capteurs VL53L1X
void Detect_gradin(struct detect_gradin_t * detect_gradin){
  unsigned char tampon[14];
  char chaine[200];
  int angle_mrad;
  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
  error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
  if (error !=0){
    affiche_erreur("Detect_gradin", "Erreur I2C");
    while(1);
  }else{
    
    detect_gradin->status = tampon[0];
    detect_gradin->centre_x_mm = tampon[1] << 24 | tampon[2] << 16 | tampon[3] << 8 | tampon[4];
    detect_gradin->centre_y_mm = tampon[5] << 24 | tampon[6] << 16 | tampon[7] << 8 | tampon[8];
    angle_mrad = tampon[9] << 24 | tampon[10] << 16 | tampon[11] << 8 | tampon[12];
    detect_gradin->angle_rad = angle_mrad / 1000.;
    

    
  }
}