From 7f435ccf924d2ac6fc43f16e91b9f656ba3de5ad Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 25 May 2025 22:43:27 +0200 Subject: [PATCH] On attrappe un gradin en autonome ! --- Actionneurs/Actionneurs.ino | 5 ++--- Cerveau/Cerveau.ino | 42 +++++++++++++++++++++---------------- Cerveau/Com_actionneur.ino | 4 ++-- Cerveau/Com_chassis.ino | 4 ++-- Cerveau/I2C_Master_lib.ino | 2 +- 5 files changed, 31 insertions(+), 26 deletions(-) diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino index 851a028..9d2acce 100644 --- a/Actionneurs/Actionneurs.ino +++ b/Actionneurs/Actionneurs.ino @@ -162,10 +162,11 @@ void loop() static int m_pos=0; static unsigned long myTime=0; - if(millis() > myTime + 100){ + if(millis() > myTime + 30){ myTime = millis(); Serial.print(">millis:"); Serial.println(millis()); + Serial.printf(">I2C_c:%d\n>I2C_t:%d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]); } @@ -177,8 +178,6 @@ void loop() int Move; int Current; - //Serial.printf("I2C: %d - %d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]); - if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_CDE_REALISE]){ switch (I2C_memory[I2C_CDE_DEMANDE]){ diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index c5bae67..e171e6e 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -320,10 +320,9 @@ void gestion_match(){ translation_x_mm = 500; translation_y_mm = 0; rotation_rad = 0; - - actionneur_action_generique(5); + //index_Maitre = DEPLACEMENT_RELATIF; - //index_Maitre = TEST_APPROCHE_GRADIN; + index_Maitre = TEST_APPROCHE_GRADIN; } if(M5.BtnB.read() == 1){ Serial.println("BtnB"); @@ -513,7 +512,8 @@ enum etat_action_t gradin_approche(void){ GA_GOTO_EN_FACE, GA_TOURNE_LARGE, GA_GOTO_PROCHE, - GA_GOTO_PREND, + GA_GOTO_PREND_1, + GA_GOTO_PREND_2, GA_RECULE } statu_approche_gradin = GA_INIT; static float angle_parcouru, angle_mem; @@ -531,6 +531,7 @@ enum etat_action_t gradin_approche(void){ switch(statu_approche_gradin){ case GA_INIT: + while(actionneur_action(ACTIONNEUR_DEPLACEMENT)!=ACTION_TERMINEE); angle_parcouru = 0; statu_approche_gradin = GA_CHERCHE_GAUCHE; break; @@ -620,7 +621,7 @@ enum etat_action_t gradin_approche(void){ if(fabs(detect_gradin.centre_x_mm) > 10){ statu_approche_gradin = GA_GOTO_LARGE; }else{ - statu_approche_gradin = GA_GOTO_PREND; + statu_approche_gradin = GA_GOTO_PREND_1; } } } @@ -633,14 +634,24 @@ enum etat_action_t gradin_approche(void){ } break; - case GA_GOTO_PREND: - while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE); - statu_approche_gradin = GA_INIT; - delay(5000); - return ACTION_TERMINEE; - + case GA_GOTO_PREND_1: + while(deplacement_relatif((detect_gradin.centre_y_mm - 25 - 110), 0, 0, 0) != ACTION_TERMINEE); + statu_approche_gradin = GA_GOTO_PREND_2; break; + case GA_GOTO_PREND_2: + // On positionne la fourche + while(actionneur_action(ACTIONNEUR_PREPARE_PRISE)!=ACTION_TERMINEE); + // On Avance de 15 cm + while(deplacement_relatif(150, 0, 0, 0) != ACTION_TERMINEE); + // On attrape + while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE); + statu_approche_gradin = GA_CHERCHE_GAUCHE; + return ACTION_TERMINEE; + break; + + + case GA_RECULE: while(deplacement_relatif((detect_gradin.centre_y_mm - 220), 0, 0, 0) != ACTION_TERMINEE); statu_approche_gradin = GA_CHERCHE_GAUCHE; @@ -857,26 +868,21 @@ enum etat_action_t actionneur_pos_initiale(){ } -enum etat_action_t actionneur_action_generique(int action){ +enum etat_action_t actionneur_action(int action){ struct com_actionneur_t com_actionneur_env, com_actionneur_rec; int action_terminee=0; com_actionneur_env.demande_action = action; com_actionneur_rec.action_terminee = 0; - Serial.printf("Demande action %d\n", com_actionneur_env.demande_action); Actionneur_send(&com_actionneur_env); - Serial.printf("Lecture action...\n"); do{ Actionneur_scan(&com_actionneur_rec); - Serial.printf("Lecture action... %d %d\n", com_actionneur_rec.action_terminee, com_actionneur_env.demande_action); - }while(com_actionneur_env.action_terminee != com_actionneur_env.demande_action); + }while(com_actionneur_rec.action_terminee != com_actionneur_env.demande_action); // On réinitialise la machine à état com_actionneur_env.demande_action = 0; - Serial.printf("Demande action %d\n", com_actionneur_env.demande_action); Actionneur_send(&com_actionneur_env); - Serial.printf("Lecture action...\n"); do{ Actionneur_scan(&com_actionneur_rec); diff --git a/Cerveau/Com_actionneur.ino b/Cerveau/Com_actionneur.ino index 1f403fb..f5f7326 100644 --- a/Cerveau/Com_actionneur.ino +++ b/Cerveau/Com_actionneur.ino @@ -17,9 +17,9 @@ int Actionneur_scan(struct com_actionneur_t * com_actionneur, bool continuous_tr error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2); } + Serial.printf(">r0:%d\n>r1:%d\n", tampon[0], tampon[1]); com_actionneur->action_terminee = tampon[1]; - //com_actionneur->demande_action = tampon[0]; - Serial.println("I2C OK"); + com_actionneur->demande_action = tampon[0]; return error; } diff --git a/Cerveau/Com_chassis.ino b/Cerveau/Com_chassis.ino index dab0b59..9db6fa7 100644 --- a/Cerveau/Com_chassis.ino +++ b/Cerveau/Com_chassis.ino @@ -35,9 +35,9 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){ // Conversion des mm ou radian en pas - nb_pas_x = chassis_emission->translation_x_mm * 4.049; + 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.; + nb_pas_rot = chassis_emission->rotation_z_rad * -791.; Mot[0] = chassis_emission->status; //y*=-1; diff --git a/Cerveau/I2C_Master_lib.ino b/Cerveau/I2C_Master_lib.ino index 7662822..130830e 100644 --- a/Cerveau/I2C_Master_lib.ino +++ b/Cerveau/I2C_Master_lib.ino @@ -26,7 +26,7 @@ uint8_t I2C_lire_registre(int adresse_i2c, uint adresse_registre, unsigned char WIRE.requestFrom(adresse_i2c, taille_donnees + 1); // ceci contient l'adresse - if(adresse_i2c == I2C_SLAVE_chassi){ + if(adresse_i2c == I2C_SLAVE_chassi || adresse_i2c == I2C_SLAVE_actionneur){ Wire.readBytes(&temp, 1); } // Et ceci les données