From 9023ae64bc3dcdacd0db0fab4721d930826e2ffa Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sat, 22 Feb 2025 16:27:32 +0100
Subject: [PATCH] =?UTF-8?q?D=C3=A9placements=20relatifs=20fonctionnels?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Cerveau/Cerveau.ino | 80 +++++++++++++++++----------------------
Chassis/Chassis.ino | 60 +++++++++--------------------
Chassis/I2C_Slave_lib.ino | 1 -
Readme.md | 11 ++----
4 files changed, 56 insertions(+), 96 deletions(-)
diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino
index 6390d26..68c6dc3 100644
--- a/Cerveau/Cerveau.ino
+++ b/Cerveau/Cerveau.ino
@@ -9,6 +9,9 @@
#define Rad_Deg 57.2957795130823
+#define MOUVEMENT_FINI 0x4
+#define MOUVEMENT_EN_COURS 0x2
+
#define gst_server;
const char* ssid = "riombotique";
const char* password = "password";
@@ -80,15 +83,15 @@ char* statu[] = {"/..","./.","../"};
int index_statu=0;
struct chassis_reception_t {
- char status;
+ unsigned char status;
};
struct chassis_emission_t {
- char status;
+ unsigned char status;
int translation_x, translation_y, rotation_y;
};
-
+void Scan_chassis(struct chassis_reception_t * chassis_reception);
#define FORM \
"<!DOCTYPE html>\n" \
@@ -197,31 +200,9 @@ void loop() {
static int64_t time;
struct chassis_reception_t chassis_reception;
- Serial.println("Scan_chassis");
-
- erreur = Scan_Chassi(&chassis_reception);
- Serial.println("fin Scan_chassis");
-
- // Si le chassis est disponible
- // Attente 1 seconde avant d'envoyer un messa
-
- Serial.println("if 0x04");
- if(chassis_reception.status == 0x04 && wait == 0){
- time = esp_timer_get_time();
- wait = 1;
- }
-
- Serial.println("wait 1s");
- if(wait == 1 && esp_timer_get_time() > time + 1000000){
- Serial.println("send_Chassis");
- send_Chassis();
- wait = 0;
- }
-
-
- //strategie();
- //affichage_resultats();
- //delay(500);
+ strategie();
+ affichage_resultats();
+ delay(500);
}
void affichage_resultats() {
@@ -335,29 +316,34 @@ void strategie(){
break;
case 2 : //---------------------------------------------------------------------------------------------------------
- Scan_Chassi(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis
+ Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis
if(Err_Chassi_com !=0){
- affiche_erreur("case 2", "Err_Chassi_com !=0");
for(int j=0; j<10; j++){
affichage_resultats();
- Scan_Chassi(&chassis_reception);
+ Scan_chassis(&chassis_reception);
//delay(500);
}
// ********************************* ERREUR DE COM FATAL ***********************
}
if(Mvt_finit==0){
+ cmd_chassi_x = xA;
+ cmd_chassi_y = yA;
+ Cmd_Angle = rot;
send_Chassis(); //Envoie des coordonées à atteindre
while(Mvt_finit!=1){
- Scan_Chassi(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********)
+ Serial.println("while Mvt_finit");
+ Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********)
delay(50);
}
- Scan_Chassi(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********)
+ Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********)
}else{
send_Chassis_RAZ(); //Envoie des coordonées à atteindre
}
- //M5.Lcd.clear();
+ //M5.Lcd.clear();
+ Serial.println("case 2 fin1/2");
index_Maitre = 0;
+ Serial.println("case 2 fin2/2");
break;
}
}
@@ -405,29 +391,32 @@ void compar_cinematique(){
}
/// @brief Lit l'état du chassis en I2C
-/// @return renvoi 0 si tout s'est bien passé, un code d'erreur sinon
-int Scan_Chassi(struct chassis_reception_t * chassis_reception){
- unsigned char tampon2[10];
+void Scan_chassis(struct chassis_reception_t * chassis_reception){
+ unsigned char tampon2[14];
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
- //Serial.println("avant lire registre");
- error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 8);
+ Serial.println("avant lire registre");
+ error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
+ Serial.println("apres lire registre");
if (error !=0){
+ Serial.println("Erreur I2C");
Err_Chassi_com =1;IndexErr = 2;
affiche_erreur("Scan_Chassi", "Erreur I2C");
while(1);
}else{
+ Serial.println("I2C OK");
Err_Chassi_com =0;
IndexErr = 0;
int valeur;
- Mvt_finit = (0x04 == tampon2[0] & 0x04);
- char chaine[32];
- sprintf(chaine, "tampon2: %x %x %x %x %x %x %x %x\n", tampon2[0], tampon2[1], tampon2[2], tampon2[3], tampon2[4],
- tampon2[5], tampon2[6], tampon2[7]);
+ Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
+ char chaine[100];
+ sprintf(chaine, "tampon2: %x %x %x %x %x %x %x %x %x %x %x %x %x %x\n", tampon2[0], tampon2[1], tampon2[2], tampon2[3], tampon2[4],
+ tampon2[5], tampon2[6], tampon2[7], tampon2[8], tampon2[9], tampon2[10], tampon2[11], tampon2[12], tampon2[13]);
affiche_msg("Scan_Chassi", chaine);
+ Serial.println("Affect struct");
chassis_reception->status = tampon2[0];
+ Serial.println("apres Affect struct");
}
- return 0;
}
/// @brief Récupère position (X, Y) et l'orientation du robot
@@ -467,9 +456,8 @@ void send_Chassis_RAZ(){
void send_Chassis(){
//if(nbr_essai<=10){
- Cmd_Angle = MemCmd_A;
// Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet
- Mot[0] = 2;
+ Mot[0] = MOUVEMENT_EN_COURS;
//y*=-1;
//y = y*direction;
Mot[1] = cmd_chassi_x >>8; Mot[2] = cmd_chassi_x;
diff --git a/Chassis/Chassis.ino b/Chassis/Chassis.ino
index 4a09ee4..c1b0d36 100644
--- a/Chassis/Chassis.ino
+++ b/Chassis/Chassis.ino
@@ -17,7 +17,7 @@
#define serialP;
#define gst_I2C;
-byte * I2C_memory;
+volatile byte * I2C_memory;
//*********************************************************************
//******************** Integration et config Moteur *******************
@@ -52,9 +52,9 @@ int com[] = {0, 0, 0};
// -------------- Direction des moteurs ( pour un sens de marche commun )
int dirX= +1;
-int dirY= -1;
+int dirY= +1;
int dirZ= +1;
-int dirA= -1;
+int dirA= +1;
int cli_led=0; // initial value of input
int Move = 0;
@@ -84,17 +84,10 @@ int h=0;
void setup()
{
pinMode(Men,OUTPUT);
- digitalWrite(Men, LOW);// Low Level Enable
+ stop();
#ifdef serialP
Serial.begin(115200);
#endif
- pinMode(LED_BUILTIN, OUTPUT);
- for(int i=0; i<6;i++){
- digitalWrite(LED_BUILTIN, LOW);
- delay(150);
- digitalWrite(LED_BUILTIN, HIGH);
- delay(150);
- }
// X
stepperX.setMaxSpeed(vitesse); stepperX.setAcceleration(accel);
@@ -109,18 +102,17 @@ void setup()
//Serial.println(Men);
I2C_Slave_init(0x55);
I2C_memory = get_i2c_data();
- I2C_memory[0] = 0x1;
- I2C_memory[1] = 0x2;
- I2C_memory[2] = 0x3;
- I2C_memory[3] = 0x4;
- I2C_memory[4] = 0x5;
- I2C_memory[5] = 0x6;
- I2C_memory[6] = 0x7;
- I2C_memory[7] = 0x8;
//digitalWrite(Men, LOW);// Low Level Enable
+ pinMode(pul1, OUTPUT);
+ pinMode(pul2, OUTPUT);
+ pinMode(pul3, OUTPUT);
pinMode(pul4, OUTPUT);
+ pinMode(dir1, OUTPUT);
+ pinMode(dir2, OUTPUT);
+ pinMode(dir3, OUTPUT);
+ pinMode(dir4, OUTPUT);
}
@@ -135,17 +127,6 @@ void loop()
}
Serial.printf("\n");
- // Reset octet 0 à 0 au bout d'une seconede
- if(I2C_memory[0] !=0 && wait == 0){
- time = esp_timer_get_time();
- wait = 1;
- }
- if(esp_timer_get_time() > time + 1000000 && wait == 1){
- I2C_memory[0] = 0x4;
- wait = 0;
- }
-
- /*
if(((I2C_memory[0] & 0x02)==2) && Modif_Mvt==0){
Modif_Mvt = 1;
@@ -162,6 +143,7 @@ void loop()
}
Position_Calculation(); //************* Calcul du Vecteur XY (incrémental)
Mvmt(); //************* Envoie de la position à ateindre pour chaque moteur + Execution du mouvement
+ stop(); // Servo Off
I2C_memory[0] = 0x04;
I2C_envoi_8bits(I2C_memory[0] ,0);
@@ -180,17 +162,17 @@ void loop()
}
//Dé-asservissement des moteurs
- stop(); // Servo Off
- */
+
+
}
void Position_Calculation(){
PosX = Target[0];
AxeX[0] = {PosX}; //x
- AxeX[1] = {-PosX}; //y
+ AxeX[1] = {PosX}; //y
AxeX[2] = {-PosX}; //z
- AxeX[3] = {PosX}; //a
+ AxeX[3] = {-PosX}; //a
PosY = Target[1];
AxeY[0] = {PosY}; //x
AxeY[1] = {PosY}; //y
@@ -199,8 +181,8 @@ void Position_Calculation(){
Rot = Target[2];
AxeRot[0] = {-Rot}; //x
AxeRot[1] = {Rot}; //y
- AxeRot[2] = {-Rot}; //z
- AxeRot[3] = {Rot}; //a
+ AxeRot[2] = {Rot}; //z
+ AxeRot[3] = {-Rot}; //a
vitesse = Target[3];
accel = Target[4];
@@ -261,14 +243,8 @@ void Mvmt(){
// Serial.print(stepperX.distanceToGo());Serial.print("/");Serial.print(stepperY.distanceToGo());Serial.print("/");Serial.print(stepperZ.distanceToGo());Serial.print("/");Serial.print(stepperA.distanceToGo());Serial.print("\t");Serial.print(Interrupt);Serial.print("\t");Serial.println(Modif_Mvt);
// delay(100);
#endif
- if(esp_timer_get_time() > time + 100000){
- time = esp_timer_get_time();
- digitalWrite(LED_BUILTIN, cli_led);
- cli_led = !cli_led;
- }
}
}
- digitalWrite(LED_BUILTIN, HIGH);
// if(Modif_Mvt == 1){
// Position_Calculation();
// Serial.println("\t\tRe Calculation !!!");
diff --git a/Chassis/I2C_Slave_lib.ino b/Chassis/I2C_Slave_lib.ino
index 9b41738..1cf38b4 100644
--- a/Chassis/I2C_Slave_lib.ino
+++ b/Chassis/I2C_Slave_lib.ino
@@ -20,7 +20,6 @@ void onRequest(){
}
void onReceive(int len){
- digitalWrite(LED_BUILTIN, LOW);
memoire_I2C_index = Wire.read();
while(Wire.available()){
nouveau_message=true;
diff --git a/Readme.md b/Readme.md
index 7dd1087..f99cf83 100644
--- a/Readme.md
+++ b/Readme.md
@@ -1,10 +1,7 @@
-Test de la liaison I2C entre le châssis et le cerveau
-=====================================================
+Déplacement fonctionnel
+=======================
-Ce code permet de tester la communication I2C entre le châssis et le cerveau. Le cerveau demande 8 octet au châssis.
-Si l'octet 0 vaut 4, au bout d'une seconde, le cerveau envoi une trame pour réinitialise l'octet à 2.
+Côté cerveau, un serveur web qui permet d'envoyer un ordre de déplacement relatif au châssis.
-Le châssis scrute l'octet 0 et si celui-ci vaut 2, il attend une seconde puis le passe à 1.
+Calibration des sens des moteurs pour les translations en X, Y et pour la rotation.
-Le châssis envoie l'état de sa mémoire par la liaison USB.
-Le cerveau affiche l'état de sa mémoire sur l'écran.