52 lines
1.5 KiB
C++
52 lines
1.5 KiB
C++
//#include "Chassis.h"
|
|
#include <Arduino.h>
|
|
#include <HardwareSerial.h>
|
|
#include "Com_gradins.h"
|
|
|
|
|
|
int Detect_gradin(struct detect_gradin_t * detect_gradin){
|
|
return Detect_gradin(detect_gradin, true);
|
|
}
|
|
|
|
/// @brief Lit les capteurs VL53L1X
|
|
int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){
|
|
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");
|
|
if (blocking){
|
|
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.;
|
|
}
|
|
return error;
|
|
}
|
|
|
|
int lire_tirette(char * tirette){
|
|
return lire_tirette(tirette, true);
|
|
}
|
|
|
|
|
|
int lire_tirette(char * tirette, bool continuous_try){
|
|
unsigned char tampon;
|
|
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
|
|
error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
|
|
while(error !=0 && continuous_try){
|
|
affiche_erreur("lire_tirette", "Erreur I2C");
|
|
error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
|
|
}
|
|
*tirette = tampon;
|
|
Serial.printf(">tirette:%d", *tirette);
|
|
return error;
|
|
|
|
}
|