I2C Cerveau: diagnostic du réseau, Actionneur: logique d'empilement
This commit is contained in:
parent
b956e3a563
commit
b405fbdb76
@ -25,6 +25,11 @@
|
||||
#define AIMANT_PINCE_GAUCHE_TIENT 9, 500
|
||||
#define AIMANT_PINCE_GAUCHE_LACHE 9, 400
|
||||
|
||||
#define I2C_CDE_DEMANDE 0x00
|
||||
#define I2C_DCE_REALISE 0x01
|
||||
|
||||
volatile byte * I2C_memory;
|
||||
|
||||
SMS_STS sms_sts;
|
||||
|
||||
int ID_Feetech = 4;
|
||||
@ -34,12 +39,7 @@ struct position_t{
|
||||
uint position;
|
||||
} cible_haute, cible_basse, actuelle;
|
||||
|
||||
|
||||
|
||||
|
||||
enum etat_action_t Actionneur_empile();
|
||||
|
||||
|
||||
enum etat_action_t Actionneur_empile(void);
|
||||
|
||||
void setup()
|
||||
{
|
||||
@ -64,6 +64,9 @@ void setup()
|
||||
//ledcWrite(5, 227); // Position levée (butée haute)
|
||||
//ledcWrite(5, 345); // Position de prise
|
||||
|
||||
I2C_Slave_init(0x20);
|
||||
I2C_memory = get_i2c_data();
|
||||
|
||||
/*
|
||||
while(1){
|
||||
ledcWrite(FOURCHE_PRISE);
|
||||
@ -211,6 +214,56 @@ void loop()
|
||||
int Move;
|
||||
int Current;
|
||||
|
||||
if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_DCE_REALISE]){
|
||||
switch (I2C_memory[I2C_CDE_DEMANDE]){
|
||||
case 0:
|
||||
// Aucune commande
|
||||
I2C_memory[I2C_DCE_REALISE] = 0;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
// Position de départ
|
||||
break;
|
||||
|
||||
case 2:
|
||||
// Position de déplacement
|
||||
// Fourche levée pour ne pas géner les capteurs
|
||||
Servo_set(FOURCHE_LEVEE);
|
||||
// Ascenseur en bas
|
||||
Ascenseur_descend();
|
||||
// Pinces internes fermées
|
||||
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
||||
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
||||
// Aimant rentrés
|
||||
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
||||
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
||||
// Translateur à l'arrière
|
||||
Translateur_recule();
|
||||
I2C_memory[I2C_DCE_REALISE] = 2;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
// Prise initiale
|
||||
// Fourche en position de prise
|
||||
Servo_set(FOURCHE_PRISE);
|
||||
// Ascenseur en bas
|
||||
Ascenseur_descend();
|
||||
// Pinces internes fermées
|
||||
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
||||
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
||||
// Aimant rentrés
|
||||
Servo_set(AIMANT_PINCE_DROIT_TIENT);
|
||||
Servo_set(AIMANT_PINCE_GAUCHE_TIENT);
|
||||
// Translateur à l'arrière
|
||||
Translateur_recule();
|
||||
I2C_memory[I2C_DCE_REALISE] = 3;
|
||||
|
||||
case 4:
|
||||
// Position de transport
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (Serial.available() > 0) {
|
||||
// get incoming byte:
|
||||
@ -252,6 +305,9 @@ void loop()
|
||||
if(inByte == 'u'){
|
||||
while(Actionneur_empile() == ACTION_EN_COURS);
|
||||
}
|
||||
if(inByte == 'v'){
|
||||
Ascenseur_cycle();
|
||||
}
|
||||
if(inByte == 'c'){
|
||||
Serial.println("Cycle translateur");
|
||||
while(Serial.available() > 0){
|
||||
@ -264,8 +320,6 @@ void loop()
|
||||
}
|
||||
Serial.printf(">GPIO2:%d\n",digitalRead(2));
|
||||
Serial.printf(">GPIO10:%d\n",digitalRead(10));
|
||||
Serial.printf(">OUT3:%d\n",digitalRead(3));
|
||||
Serial.printf(">OUT4:%d\n",digitalRead(4));
|
||||
delay(25);
|
||||
}
|
||||
}
|
||||
@ -365,7 +419,7 @@ enum etat_action_t Actionneur_empile(){
|
||||
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
||||
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
||||
// Attente avance
|
||||
delay(5000);
|
||||
delay(2000);
|
||||
etat_actionneur = ACTIONNEUR_DEPILE;
|
||||
break;
|
||||
|
||||
@ -386,6 +440,7 @@ enum etat_action_t Actionneur_empile(){
|
||||
case ACTIONNEUR_PRISE_INTERNE_2:
|
||||
Serial.println("ACTIONNEUR_PRISE_INTERNE_2");
|
||||
if(Translateur_etat() == ACTION_TERMINEE){
|
||||
delay(500);
|
||||
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
||||
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
||||
delay(500);
|
||||
@ -396,7 +451,6 @@ enum etat_action_t Actionneur_empile(){
|
||||
case ACTIONNEUR_RECULE_PRISE_INTERNE_1:
|
||||
Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_1");
|
||||
Translateur_recule();
|
||||
delay(1000);
|
||||
etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_2;
|
||||
break;
|
||||
|
||||
@ -419,28 +473,35 @@ enum etat_action_t Actionneur_empile(){
|
||||
case ACTIONNEUR_AVANCE_PRISE_INTERNE:
|
||||
Serial.println("ACTIONNEUR_AVANCE_PRISE_INTERNE");
|
||||
if(Translateur_etat() == ACTION_TERMINEE){
|
||||
delay(500);
|
||||
Ascenseur_depose();
|
||||
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
|
||||
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
|
||||
etat_actionneur = ACTIONNEUR_DEPOSE_PRISE_INTERNE;
|
||||
}
|
||||
break;
|
||||
|
||||
case ACTIONNEUR_DEPOSE_PRISE_INTERNE:
|
||||
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
|
||||
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
|
||||
delay(500);
|
||||
Translateur_recule();
|
||||
etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
|
||||
Serial.println("ACTIONNEUR_DEPOSE_PRISE_INTERNE");
|
||||
if(Ascenseur_get_etat() == ACTION_TERMINEE){
|
||||
delay(500);
|
||||
Translateur_recule();
|
||||
etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
|
||||
}
|
||||
break;
|
||||
|
||||
case ACTIONNEUR_RANGE_TRANSLATEUR:
|
||||
Serial.println("ACTIONNEUR_RANGE_TRANSLATEUR");
|
||||
if(Translateur_etat() == ACTION_TERMINEE){
|
||||
etat_actionneur = ACTIONNEUR_DESCEND;
|
||||
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
||||
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
||||
Ascenseur_monte();
|
||||
Ascenseur_descend();
|
||||
}
|
||||
break;
|
||||
|
||||
case ACTIONNEUR_DESCEND:
|
||||
Serial.println("ACTIONNEUR_DESCEND");
|
||||
if(Ascenseur_get_etat() == ACTION_TERMINEE){
|
||||
etat_actionneur = ACTIONNEUR_DEPLACEMENT;
|
||||
Servo_set(FOURCHE_TRANSPORT);
|
||||
|
@ -10,4 +10,6 @@ int Ascenseur_descend(void);
|
||||
void Ascenseur_step_up(void);
|
||||
void Ascenseur_step_down(void);
|
||||
void Ascenseur_gestion(void);
|
||||
void Ascenseur_cycle(void);
|
||||
int Ascenseur_depose(void);
|
||||
enum etat_action_t Ascenseur_get_etat(void);
|
@ -9,6 +9,7 @@ enum etat_ascenseur_t{
|
||||
ASCENSEUR_ACTIF,
|
||||
ASCENSEUR_MONTE,
|
||||
ASCENSEUR_DESCENT,
|
||||
ASCENSEUR_DEPOSE,
|
||||
ASCENSEUR_BUSY,
|
||||
}etat_ascenseur=ASCENSEUR_INIT;
|
||||
|
||||
@ -139,6 +140,11 @@ enum etat_action_t Ascenseur_cherche_butees(void){
|
||||
/// @return 1 si l'ascenseur n'est pas prêt
|
||||
int Ascenseur_monte(void){
|
||||
if(etat_ascenseur == ASCENSEUR_ACTIF){
|
||||
tab_position[0] = position_haute_droit;
|
||||
tab_position[1] = position_haute_gauche;
|
||||
tab_vitesses_u[0] = 2000;
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
etat_ascenseur = ASCENSEUR_MONTE;
|
||||
return 0;
|
||||
}
|
||||
@ -149,11 +155,30 @@ int Ascenseur_monte(void){
|
||||
/// @return 1 si l'ascenseur n'est pas prêt
|
||||
int Ascenseur_descend(void){
|
||||
if(etat_ascenseur == ASCENSEUR_ACTIF){
|
||||
tab_position[0] = position_basse_droit;
|
||||
tab_position[1] = position_basse_gauche;
|
||||
tab_vitesses_u[0] = 2000;
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
etat_ascenseur = ASCENSEUR_DESCENT;
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/// @brief Commande l'ascenseur en position de dépose
|
||||
/// @return 1 si l'ascenseur n'est pas prêt
|
||||
int Ascenseur_depose(void){
|
||||
if(etat_ascenseur == ASCENSEUR_ACTIF){
|
||||
tab_position[0] = position_haute_droit*0.9 + position_basse_droit * 0.1;
|
||||
tab_position[1] = position_haute_gauche * 0.9 + position_basse_gauche * 0.1;
|
||||
tab_vitesses_u[0] = 2000;
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
etat_ascenseur = ASCENSEUR_DEPOSE;
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
enum etat_action_t Ascenseur_get_etat(void){
|
||||
if(etat_ascenseur == ASCENSEUR_ACTIF){
|
||||
@ -195,6 +220,24 @@ void Ascenseur_update_step(void){
|
||||
|
||||
}
|
||||
|
||||
void Ascenseur_cycle(){
|
||||
while(!Serial.available()){
|
||||
Serial.println("Ascenseur_monte");
|
||||
Ascenseur_monte();
|
||||
while(Ascenseur_get_etat() != ACTION_TERMINEE){
|
||||
Ascenseur_gestion();
|
||||
}
|
||||
Serial.println("Ascenseur_descend");
|
||||
Ascenseur_descend();
|
||||
while(Ascenseur_get_etat() != ACTION_TERMINEE){
|
||||
Ascenseur_gestion();
|
||||
}
|
||||
}
|
||||
while(Serial.available() > 0){
|
||||
Serial.read();
|
||||
}
|
||||
}
|
||||
|
||||
void Ascenseur_gestion(void){
|
||||
|
||||
bool erreur;
|
||||
@ -218,27 +261,24 @@ void Ascenseur_gestion(void){
|
||||
break;
|
||||
|
||||
case ASCENSEUR_MONTE:
|
||||
tab_position[0] = position_haute_droit;
|
||||
tab_position[1] = position_haute_gauche;
|
||||
tab_vitesses_u[0] = 2000;
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
delay(200);
|
||||
etat_ascenseur = ASCENSEUR_BUSY;
|
||||
break;
|
||||
|
||||
case ASCENSEUR_DESCENT:
|
||||
tab_position[0] = position_basse_droit;
|
||||
tab_position[1] = position_basse_gauche;
|
||||
tab_vitesses_u[0] = 2000;
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
delay(200);
|
||||
etat_ascenseur = ASCENSEUR_BUSY;
|
||||
break;
|
||||
|
||||
case ASCENSEUR_DEPOSE:
|
||||
delay(200);
|
||||
etat_ascenseur = ASCENSEUR_BUSY;
|
||||
break;
|
||||
|
||||
case ASCENSEUR_BUSY:
|
||||
mov_g = sms_sts.ReadMove(ID_FEETECH_ASC_G);
|
||||
mov_d = sms_sts.ReadMove(ID_FEETECH_ASC_D);
|
||||
if(!mov_g && !mov_d){
|
||||
if(!mov_g && !mov_d) {
|
||||
// Fin du mouvement
|
||||
etat_ascenseur = ASCENSEUR_ACTIF;
|
||||
|
||||
|
61
Actionneurs/I2C_Slave_lib.ino
Normal file
61
Actionneurs/I2C_Slave_lib.ino
Normal file
@ -0,0 +1,61 @@
|
||||
#define TAILLE_MEMOIRE_I2C 256
|
||||
#define TAILLE_MESSAGE_ENVOI_MAX 32
|
||||
byte memoire_I2C[TAILLE_MEMOIRE_I2C];
|
||||
byte memoire_I2C_index=0;
|
||||
|
||||
bool nouveau_message=false;
|
||||
|
||||
uint8_t * get_i2c_data(){
|
||||
return memoire_I2C;
|
||||
}
|
||||
void onRequest(){
|
||||
uint32_t taille_envoi;
|
||||
taille_envoi = min (TAILLE_MEMOIRE_I2C-memoire_I2C_index, TAILLE_MESSAGE_ENVOI_MAX);
|
||||
|
||||
Wire.write(&memoire_I2C[memoire_I2C_index], taille_envoi);
|
||||
memoire_I2C_index += taille_envoi;
|
||||
if(memoire_I2C_index>=TAILLE_MEMOIRE_I2C){
|
||||
Serial.printf("memoire_I2C_index>=TAILLE_MEMOIRE_I2C\n");
|
||||
}
|
||||
}
|
||||
|
||||
void onReceive(int len){
|
||||
memoire_I2C_index = Wire.read();
|
||||
while(Wire.available()){
|
||||
nouveau_message=true;
|
||||
memoire_I2C[memoire_I2C_index] = Wire.read();
|
||||
memoire_I2C_index++;
|
||||
}
|
||||
//Serial.printf("I2C reg: %d, val %d\n", memoire_I2C_index, memoire_I2C[memoire_I2C_index]);
|
||||
}
|
||||
|
||||
void I2C_Slave_init(int addr){
|
||||
Wire.onReceive(onReceive);
|
||||
Wire.onRequest(onRequest);
|
||||
Wire.begin(addr);
|
||||
}
|
||||
|
||||
bool I2C_Slave_nouveau_message(){
|
||||
if(nouveau_message){
|
||||
nouveau_message=false;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void I2C_envoi_8bits(byte value, char adresse){
|
||||
//printf("I2C_envoi_8bits a:%d v:%d %x %b\n",adresse, value, value, value);
|
||||
memoire_I2C[adresse] = value;
|
||||
}
|
||||
|
||||
void I2C_envoi_16bits(int16_t value, char adresse){
|
||||
memoire_I2C[adresse] = (value >> 8) & 0xFF;
|
||||
memoire_I2C[adresse+1] = value & 0xFF;
|
||||
}
|
||||
|
||||
void I2C_envoi_32bits(int32_t value, char adresse){
|
||||
memoire_I2C[adresse] = value >> 24;
|
||||
memoire_I2C[adresse+1] = (value >> 16) & 0xFF;
|
||||
memoire_I2C[adresse+2] = (value >> 8) & 0xFF;
|
||||
memoire_I2C[adresse+3] = value & 0xFF;
|
||||
}
|
92
Actionneurs/Translateur.ino
Normal file
92
Actionneurs/Translateur.ino
Normal file
@ -0,0 +1,92 @@
|
||||
#define DUREE_MVT_TRANSLATEUR_MS 1000
|
||||
enum translateur_action_t{
|
||||
TRANSLATEUR_INIT,
|
||||
TRANSLATEUR_AVANCE,
|
||||
TRANSLATEUR_RECULE,
|
||||
}translateur_action = TRANSLATEUR_INIT;
|
||||
|
||||
unsigned long temps_debut_action = 0;
|
||||
|
||||
void Translateur_init(void){
|
||||
// Moteur
|
||||
/*ledcAttach(3, 500, 8);
|
||||
ledcWrite(3, 0);
|
||||
ledcAttach(4, 500, 8);
|
||||
ledcWrite(4, 0);*/
|
||||
pinMode(3, OUTPUT);
|
||||
pinMode(4, OUTPUT);
|
||||
// Etat des contacteurs de fin de course
|
||||
// relecture de 3 sur 2 (Avant)
|
||||
// relecture de 4 sur 10 (Arrière)
|
||||
pinMode(2, INPUT);
|
||||
pinMode(10, INPUT);
|
||||
}
|
||||
|
||||
|
||||
enum etat_action_t Translateur_etat(void){
|
||||
if(temps_debut_action + DUREE_MVT_TRANSLATEUR_MS < millis()){
|
||||
if(translateur_action == TRANSLATEUR_AVANCE){
|
||||
if(digitalRead(2) == 0){
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}else if(translateur_action == TRANSLATEUR_RECULE){
|
||||
if(digitalRead(10) == 0){
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}else{
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}
|
||||
return ACTION_EN_COURS;
|
||||
}
|
||||
|
||||
|
||||
void Translateur_cycle(void){
|
||||
if(Translateur_etat() == ACTION_TERMINEE){
|
||||
if(translateur_action == TRANSLATEUR_RECULE){
|
||||
Translateur_avance();
|
||||
}else{
|
||||
Translateur_recule();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Translateur_avance(){
|
||||
Moteur_set(-255);
|
||||
temps_debut_action = millis();
|
||||
translateur_action = TRANSLATEUR_AVANCE;
|
||||
}
|
||||
|
||||
void Translateur_recule(){
|
||||
Moteur_set(255);
|
||||
temps_debut_action = millis();
|
||||
translateur_action = TRANSLATEUR_RECULE;
|
||||
}
|
||||
|
||||
/// @brief pilote la vitesse des moteurs
|
||||
/// @param vitesse vitesse signée, sera saturée à 255
|
||||
void Moteur_set(int vitesse){
|
||||
if(vitesse < 0){
|
||||
digitalWrite(3, 1);
|
||||
digitalWrite(4, 0);
|
||||
}else{
|
||||
digitalWrite(3, 0);
|
||||
digitalWrite(4, 1);
|
||||
}
|
||||
/*ledcWrite(3, 0);
|
||||
ledcWrite(4, 0);
|
||||
if(vitesse < 0){
|
||||
vitesse = -vitesse;
|
||||
if(vitesse > 255){
|
||||
vitesse = 255;
|
||||
}
|
||||
ledcWrite(3, vitesse);
|
||||
}else{
|
||||
if(vitesse > 255){
|
||||
vitesse = 255;
|
||||
}
|
||||
ledcWrite(4, vitesse);
|
||||
}*/
|
||||
}
|
||||
|
@ -3,15 +3,21 @@
|
||||
#include <HardwareSerial.h>
|
||||
|
||||
|
||||
/// @brief Lit l'état du chassis en I2C
|
||||
int Scan_chassis(struct chassis_reception_t * chassis_reception){
|
||||
return Scan_chassis(chassis_reception, true);
|
||||
}
|
||||
|
||||
/// @brief Lit l'état du chassis en I2C
|
||||
int Scan_chassis(struct chassis_reception_t * chassis_reception, bool blocking){
|
||||
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);
|
||||
if(blocking){
|
||||
while(1);
|
||||
}
|
||||
}else{
|
||||
Err_Chassi_com =0;
|
||||
IndexErr = 0;
|
||||
|
@ -7,6 +7,7 @@ struct detect_adv_reception_t {
|
||||
unsigned char distance_cm[12];
|
||||
};
|
||||
|
||||
void Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception);
|
||||
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool);
|
||||
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception);
|
||||
|
||||
#endif
|
||||
|
@ -2,17 +2,21 @@
|
||||
#include <Arduino.h>
|
||||
#include <HardwareSerial.h>
|
||||
|
||||
|
||||
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){
|
||||
return Detect_adv_lire(detect_adv_reception, true);
|
||||
}
|
||||
/// @brief Lit les capteurs VL53L1X
|
||||
void Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){
|
||||
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool blocking){
|
||||
unsigned char tampon2[14];
|
||||
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
|
||||
error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
|
||||
if (error !=0){
|
||||
affiche_erreur("Scan_Detect_adversaire", "Erreur I2C");
|
||||
while(1);
|
||||
if(blocking){
|
||||
while(1);
|
||||
}
|
||||
}else{
|
||||
Serial.println("I2C OK");
|
||||
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
@ -4,8 +4,12 @@
|
||||
#include "Com_gradins.h"
|
||||
|
||||
|
||||
int Detect_gradin(struct detect_gradin_t * detect_gradin){
|
||||
return Detect_gradin(detect_gradin, true);
|
||||
}
|
||||
|
||||
/// @brief Lit les capteurs VL53L1X
|
||||
void Detect_gradin(struct detect_gradin_t * detect_gradin){
|
||||
int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){
|
||||
unsigned char tampon[14];
|
||||
char chaine[200];
|
||||
int angle_mrad;
|
||||
@ -13,7 +17,9 @@ void Detect_gradin(struct detect_gradin_t * detect_gradin){
|
||||
error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
|
||||
if (error !=0){
|
||||
affiche_erreur("Detect_gradin", "Erreur I2C");
|
||||
while(1);
|
||||
if (blocking){
|
||||
while(1);
|
||||
}
|
||||
}else{
|
||||
|
||||
detect_gradin->status = tampon[0];
|
||||
@ -21,8 +27,6 @@ void Detect_gradin(struct detect_gradin_t * detect_gradin){
|
||||
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;
|
||||
}
|
||||
|
@ -1,5 +1,10 @@
|
||||
int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
|
||||
return Scan_Triangulation(triangulation_reception, true);
|
||||
}
|
||||
|
||||
|
||||
/// @brief Récupère position (X, Y) et l'orientation du robot
|
||||
void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
|
||||
int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception, bool blocking){
|
||||
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)
|
||||
@ -8,7 +13,10 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
|
||||
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);
|
||||
if(blocking){
|
||||
while(1);
|
||||
}
|
||||
return error;
|
||||
}
|
||||
else{Err_Tri_com =0;IndexErr = 0;}
|
||||
if (error ==0){
|
||||
@ -35,6 +43,7 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
|
||||
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
|
||||
triangulation_reception->validite = true;
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
void Triangulation_send_immobile(int immobile){
|
||||
|
@ -105,12 +105,19 @@ void showStatus() {
|
||||
char message_statu[500];
|
||||
const char message_statu_tmplt[] = "<!DOCTYPE html><html><head><meta charset=\"UTF-8\"><title>A simple form</title></head>\
|
||||
<body>\n<h1>Statu du robot</h1>\n\
|
||||
<p>Chassis: %s</p><p>Gradin: %d</p><p>Triangulation: %d</p><p>Detection_adversaire: %d</p>";
|
||||
<p>Chassis: %s</p><p>Gradin: %s</p><p>Triangulation: %s</p><p>Detection_adversaire: %s</p>";
|
||||
struct chassis_reception_t chassis_reception;
|
||||
struct detect_gradin_t detect_gradin;
|
||||
struct triangulation_reception_t triangulation_reception;
|
||||
struct detect_adv_reception_t detect_adv_reception;
|
||||
|
||||
statu_to_string(chassis, !Scan_chassis(&chassis_reception) );
|
||||
statu_to_string(chassis, !Scan_chassis(&chassis_reception, false) );
|
||||
statu_to_string(gradin, !Detect_gradin(&detect_gradin, false) );
|
||||
statu_to_string(triangulation, !Scan_Triangulation(&triangulation_reception, false) );
|
||||
statu_to_string(detection_adversaire, !Detect_adv_lire(&detect_adv_reception, false) );
|
||||
|
||||
sprintf(message_statu, message_statu_tmplt, chassis, 0, 0, 0);
|
||||
|
||||
sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire);
|
||||
server.send(200, "text/html", message_statu);
|
||||
|
||||
}
|
||||
|
Binary file not shown.
Binary file not shown.
Loading…
Reference in New Issue
Block a user