29 lines
885 B
C++
29 lines
885 B
C++
//#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.;
|
|
|
|
|
|
|
|
}
|
|
}
|