Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

I2C Cerveau: diagnostic du réseau, Actionneur: logique d'empilement

Browse Source
This commit is contained in:
Samuel 2025-05-25 08:30:12 +02:00
parent b956e3a563
commit b405fbdb76
13 changed files with 331 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
Actionneurs/Actionneurs.ino
View File

@ -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);

2
Actionneurs/Ascenseur.h
View File

@ -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);

62
Actionneurs/Ascenseur.ino
View File

@ -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
View 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
View 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);
}*/
}

10
Cerveau/Com_chassis.ino
View File

@ -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;

3
Cerveau/Com_detection_adversaire.h
View File

@ -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

12
Cerveau/Com_detection_adversaire.ino
View File

@ -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;
}

14
Cerveau/Com_gradins.ino
View File

@ -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;
}

13
Cerveau/Com_triangulation.ino
View File

@ -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){

13
Cerveau/ServerWeb.ino
View File

@ -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);
}

BIN
Doc/Communication I2C.odt
View File

Binary file not shown.

BIN
Doc/Communication I2C.pdf
View File

Binary file not shown.