On attrappe un gradin en autonome !

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

Cerveau/Cerveau.ino

@@ -320,10 +320,9 @@ void gestion_match(){
         translation_x_mm = 500;
         translation_y_mm = 0;
         rotation_rad = 0;
+        //index_Maitre = DEPLACEMENT_RELATIF;
         
-        actionneur_action_generique(5);
+        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:
+    case GA_GOTO_PREND_1:
-      while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE);
+      while(deplacement_relatif((detect_gradin.centre_y_mm - 25 - 110), 0, 0, 0) != ACTION_TERMINEE);
-      statu_approche_gradin = GA_INIT;
+      statu_approche_gradin = GA_GOTO_PREND_2;
-      delay(5000);
-      return ACTION_TERMINEE;
-  
       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_rec.action_terminee != com_actionneur_env.demande_action);
-  }while(com_actionneur_env.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.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];
+  com_actionneur->demande_action = tampon[0];
-  Serial.println("I2C OK");
   
   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/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