Premier essai concluant de la séquence départ + empile

Actionneurs/Actionneurs.ino

@@ -5,9 +5,9 @@
 #define SERVO_PINCE_GAUCHE 8
 #define SERVO_PINCE_DROITE 9
 
-#define FOURCHE_TRANSPORT 5, 287
-#define FOURCHE_LEVEE     5, 217
-#define FOURCHE_PRISE     5, 327
+#define FOURCHE_TRANSPORT 5, 266
+#define FOURCHE_LEVEE     5, 206
+#define FOURCHE_PRISE     5, 308
 
 #define DOIGT_PINCE_GAUCHE_FERME 3, 1530
 #define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
@@ -17,11 +17,11 @@
 #define DOIGT_PINCE_DROIT_OUVRE 4, 2365
 #define DOIGT_PINCE_DROIT_SEUIL 1575
 
-#define AIMANT_PINCE_DROIT_TIENT 8, 147
-#define AIMANT_PINCE_DROIT_LACHE 8, 337
+#define AIMANT_PINCE_DROIT_TIENT 8, 176
+#define AIMANT_PINCE_DROIT_LACHE 8, 366
 
-#define AIMANT_PINCE_GAUCHE_TIENT 9, 417
-#define AIMANT_PINCE_GAUCHE_LACHE 9, 217
+#define AIMANT_PINCE_GAUCHE_TIENT 9, 366
+#define AIMANT_PINCE_GAUCHE_LACHE 9, 236
 
 #define I2C_CDE_DEMANDE 0x00
 #define I2C_CDE_REALISE 0x01
@@ -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]){
@@ -280,6 +279,9 @@ void loop()
       Servo_set(DOIGT_PINCE_GAUCHE_FERME);
       Servo_set(DOIGT_PINCE_DROIT_FERME);
     }
+    if(inByte == 'f'){
+      Servo_set(FOURCHE_LEVEE);
+    }
     if(inByte == '1'){
       reglage_servo(8);
     }
@@ -490,6 +492,7 @@ enum etat_action_t Actionneur_empile(){
     case ACTIONNEUR_LEVE:
       Serial.println("ACTIONNEUR_LEVE");
       etat_actionneur = ACTIONNEUR_LEVE_1;
+      Servo_set(FOURCHE_PRISE);
       Ascenseur_monte();
       break;
 
@@ -527,6 +530,8 @@ enum etat_action_t Actionneur_empile(){
         etat_actionneur = ACTIONNEUR_DESCEND;
         Servo_set(DOIGT_PINCE_DROIT_FERME);
         Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        Servo_set(AIMANT_PINCE_DROIT_LACHE);
+        Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
         Ascenseur_descend();
       }
       break;

Actionneurs/Ascenseur.ino

@@ -86,7 +86,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       if(abs(pos_d - old_pos_d) < 10 ){*/
       my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
       printf(">load_d:%d\n", my_load);
-      if(abs(my_load) > 250 ){
+      if(abs(my_load) > 200 ){
         sms_sts.EnableTorque(ID_FEETECH_ASC_D, 0);
         position_basse_droit = sms_sts.ReadPos(ID_FEETECH_ASC_D);
         Serial.printf("position_basse_droit:%d\n", position_basse_droit);
@@ -99,7 +99,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       printf(">delta_pos_g:%d\n",pos_g - old_pos_g);*/
       my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
       printf(">load_g:%d\n", my_load);
-      if(abs(my_load) > 250 ){
+      if(abs(my_load) > 200 ){
         sms_sts.EnableTorque(ID_FEETECH_ASC_G, 0);
         position_basse_gauche = sms_sts.ReadPos(ID_FEETECH_ASC_G);
         Serial.printf("position_basse_gauche:%d\n", position_basse_gauche);

Cerveau/Cerveau.ino

@@ -281,6 +281,7 @@ void gestion_match(){
   struct detect_gradin_t detect_gradin;
   enum etat_action_t etat_action;
   static int translation_x_mm, translation_y_mm;
+  static char tirette_enlevee=1;
   static float rotation_rad;
   static int couleur;
 
@@ -320,10 +321,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");
@@ -344,6 +344,7 @@ void gestion_match(){
         }
         delay(200);
         IHM_attente_match(&couleur);
+        tirette_enlevee = 0;
 
         index_Maitre = MATCH_EN_COURS;
         
@@ -370,8 +371,14 @@ void gestion_match(){
       break;
 
     case MATCH_EN_COURS:
-      if(Strategie(couleur) == ACTION_TERMINEE){
-        index_Maitre = ATTENTE_ORDRE;
+      Serial.printf(">tirette_m:%d\n", tirette_enlevee);
+      if(tirette_enlevee == 0){
+        lire_tirette(&tirette_enlevee);
+      }
+      if(tirette_enlevee == 1){
+        if(Strategie(couleur) == ACTION_TERMINEE){
+          index_Maitre = ATTENTE_ORDRE;
+        }
       }
       break;
 
@@ -432,9 +439,10 @@ void IHM_attente_match(int * couleur){
 enum etat_action_t Strategie(int couleur){
   static enum {
     STRAT_RECULE_BANDEROLE, // Deplacement relatif
-    STRAT_ALLER_GRADINS_1_A,  // Déplacement absolu
-    STRAT_ALLER_GRADINS_1_B,  // Déplacement relatif
+    STRAT_ALLER_GRADINS_1_A,  // Déplacement relatif
+    STRAT_ALLER_GRADINS_1_B,  // Cherche gradin
     STRAT_ALLER_GRADINS_1_C,  // Déplacement relatif
+    STRAT_DEPOSE_GRADIN_1, // Empile gradin
     STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
     STRAT_ALLER_BACKSTAGE // Déplacement relatif
 
@@ -446,7 +454,7 @@ enum etat_action_t Strategie(int couleur){
   switch(etat_strategie){
     case STRAT_RECULE_BANDEROLE:
       // Déplacement en X
-      translation_x_mm = -450;
+      translation_x_mm = -300;
       translation_y_mm = 0;
       rotation_rad = 0;
       etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
@@ -456,25 +464,35 @@ enum etat_action_t Strategie(int couleur){
       break;
 
     case STRAT_ALLER_GRADINS_1_A:
+      translation_x_mm = -500;
+      rotation_rad = 0;
       if(couleur == COULEUR_JAUNE){
-        etat_action = deplacement_absolu(800, 800, -M_PI/2., 1);
+        translation_y_mm = -500;
       }else{
-        etat_action = deplacement_absolu(3000 - 800, 800, -M_PI/2., 1);
+        translation_y_mm = 500;
       }
+      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_ALLER_GRADINS_1_B;
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_B:
-      etat_action = deplacement_relatif(300, 0, 0, 0);
+      etat_action = gradin_approche();
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_ALLER_GRADINS_1_C;
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_C:
-      etat_action = deplacement_relatif(-250, 0, 0, 0);
+      etat_action = deplacement_relatif(120, 0, 0, 0);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_DEPOSE_GRADIN_1;
+      }
+      break;
+
+    case STRAT_DEPOSE_GRADIN_1:
+      etat_action = actionneur_action(ACTIONNEUR_EMPILE);
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
       }
@@ -513,7 +531,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 +550,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 +640,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 +653,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 +887,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);

Cerveau/Com_actionneur.h

@@ -0,0 +1,17 @@
+#ifndef COM_ACTIONNEUR_H
+#define COM_ACTIONNEUR_H
+
+#define I2C_SLAVE_actionneur 0x20
+
+#define ACTIONNEUR_NO_ACTION 0
+#define ACTIONNEUR_POS_INITIAL 1
+#define ACTIONNEUR_DEPLACEMENT 2
+#define ACTIONNEUR_PREPARE_PRISE 3
+#define ACTIONNEUR_PRISE 4
+#define ACTIONNEUR_EMPILE 5
+
+struct com_actionneur_t{
+  char demande_action, action_terminee;
+};
+
+#endif //COM_ACTIONNEUR_H

Cerveau/Com_actionneur.ino

@@ -0,0 +1,40 @@
+#include "Com_actionneur.h"
+#include <Arduino.h>
+#include <HardwareSerial.h>
+
+
+
+int Actionneur_scan(struct com_actionneur_t * com_actionneur){
+  return Actionneur_scan(com_actionneur, true);
+}
+/// @brief Commande l'actionneur
+int Actionneur_scan(struct com_actionneur_t * com_actionneur, bool continuous_try){
+  unsigned char tampon[4];
+  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
+  error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
+  while(error !=0 && continuous_try){
+    affiche_erreur("Actionneur_scan", "Erreur I2C");
+    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];
+  
+  return error;
+}
+
+int Actionneur_send(struct com_actionneur_t * com_actionneur){
+  
+  unsigned char tampon[1];
+  
+
+  tampon[0] = com_actionneur->demande_action;
+  
+  error = I2C_ecrire_registre(I2C_SLAVE_actionneur, 0, tampon, 1);
+  if (error !=0){
+    affiche_erreur("Actionneur_send", "Erreur I2C");
+    while(1);
+  }
+  return error;
+}

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;

Cerveau/Com_gradins.h

@@ -9,4 +9,6 @@ struct detect_gradin_t{
   float angle_rad;
 };
 
+int lire_tirette(void);
+
 #endif

Cerveau/Com_gradins.ino

@@ -30,3 +30,22 @@ int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){
   }
   return error;
 }
+
+int lire_tirette(char * tirette){
+  return lire_tirette(tirette, true);
+}
+
+
+int lire_tirette(char * tirette, bool continuous_try){
+  unsigned char tampon;
+  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
+  error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
+  while(error !=0 && continuous_try){
+    affiche_erreur("lire_tirette", "Erreur I2C");
+    error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
+  }
+  *tirette = tampon;
+  Serial.printf(">tirette:%d", *tirette);
+  return error;
+
+}

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