70 lines
2.0 KiB
C++
70 lines
2.0 KiB
C++
//#include "Chassis.h"
|
|
#include <Arduino.h>
|
|
#include <HardwareSerial.h>
|
|
|
|
|
|
/// @brief Lit l'état du chassis en I2C
|
|
void Scan_chassis(struct chassis_reception_t * chassis_reception){
|
|
unsigned char tampon2[14];
|
|
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
|
|
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
|
|
if (error !=0){
|
|
Err_Chassi_com =1;IndexErr = 2;
|
|
affiche_erreur("Scan_Chassi", "Erreur I2C");
|
|
while(1);
|
|
}else{
|
|
Err_Chassi_com =0;
|
|
IndexErr = 0;
|
|
|
|
Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
|
|
chassis_reception->status = tampon2[0];
|
|
}
|
|
}
|
|
|
|
void send_Chassis(struct chassis_emission_t * chassis_emission){
|
|
//if(nbr_essai<=10){
|
|
// Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet
|
|
int nb_pas_x, nb_pas_y, nb_pas_rot;
|
|
|
|
// Conversion des mm ou radian en pas
|
|
|
|
nb_pas_x = chassis_emission->translation_x_mm * 4.049;
|
|
nb_pas_y = chassis_emission->translation_y_mm * 4.953;
|
|
nb_pas_rot = chassis_emission->rotation_z_rad * 791.;
|
|
|
|
Mot[0] = chassis_emission->status;
|
|
//y*=-1;
|
|
//y = y*direction;
|
|
Mot[1] = nb_pas_x >>8;
|
|
Mot[2] = nb_pas_x;
|
|
Mot[3] = nb_pas_y >>8;
|
|
Mot[4] = nb_pas_y;
|
|
//Serial.println(y);
|
|
Mot[5] = nb_pas_rot >>8;
|
|
Mot[6] = nb_pas_rot;
|
|
Mot[7] = chassis_emission->vitesse >>8;
|
|
Mot[8] = chassis_emission->vitesse;
|
|
Mot[9] = chassis_emission->acceleration >>8;
|
|
Mot[10] = chassis_emission->acceleration;
|
|
|
|
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
|
|
if (error !=0){Err_Cha_send =1; IndexErr = 5;}
|
|
else{Err_Cha_send =0;IndexErr = 0;}
|
|
if(error==0){ //si pas d'erreur de transmission alors RAZ des valeurs
|
|
nbr_essai ++;
|
|
cmd_chassi_x = 0;
|
|
cmd_chassi_y = 0;
|
|
Cmd_Angle = 0;
|
|
vit = 0;
|
|
acc = 0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void send_Chassis_RAZ(){
|
|
uint8_t RAZ_Message[11]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
|
|
|
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, RAZ_Message, 11);
|
|
}
|