Secoue les planches, v1.2

Actionneurs/Actionneurs.ino

@@ -5,7 +5,7 @@
 #define SERVO_PINCE_GAUCHE 8
 #define SERVO_PINCE_DROITE 9
 
-#define FOURCHE_TRANSPORT 5, 266
+#define FOURCHE_TRANSPORT 5, 277
 #define FOURCHE_LEVEE     5, 206
 #define FOURCHE_PRISE     5, 308
 
@@ -13,7 +13,7 @@
 #define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
 #define DOIGT_PINCE_GAUCHE_SEUIL 1545
 
-#define DOIGT_PINCE_DROIT_FERME 4, 2475
+#define DOIGT_PINCE_DROIT_FERME 4, 2500
 #define DOIGT_PINCE_DROIT_OUVRE 4, 2365
 #define DOIGT_PINCE_DROIT_SEUIL 1575
 
@@ -90,6 +90,7 @@ void setup()
   
 
   Translateur_init();
+
   
 
   /*while(1){
@@ -114,6 +115,7 @@ void setup()
 
   }*/
   Ascenseur_init();  
+
 }
 
 void reglage_servo(int pin_servo){
@@ -199,8 +201,8 @@ void loop()
         break;
         
       case 3:
-        // Position de pré-prise
-        if(Actionneur_prepare_prise() == ACTION_TERMINEE){
+        // Position de prise planche
+        if(Actionneur_prepare_prise_planche() == ACTION_TERMINEE){
           I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
         }
         break;
@@ -219,7 +221,21 @@ void loop()
           I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
         }
         break;
+
+      case 6:
+        while(Actionneur_depile_planche() != ACTION_TERMINEE);
+        I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        break;
       
+      case 7:
+        while(Actionneur_prepare_prise_extern() != ACTION_TERMINEE);
+        I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        break;
+
+      case 8:
+        while(Actionneur_fourche_transport() != ACTION_TERMINEE);
+        I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        break;      
 
     }
 
@@ -282,6 +298,9 @@ void loop()
     if(inByte == 'f'){
       Servo_set(FOURCHE_LEVEE);
     }
+    if(inByte == 'y'){
+      while(Actionneur_depile_planche() == ACTION_EN_COURS);
+    }
     if(inByte == '1'){
       reglage_servo(8);
     }
@@ -301,7 +320,7 @@ void loop()
       while(Actionneur_empile() == ACTION_EN_COURS);
     }
     if(inByte == 'n'){
-      while(Actionneur_prepare_prise() == ACTION_EN_COURS);
+      while(Actionneur_prepare_prise_planche() == ACTION_EN_COURS);
     }
     /*
     if(inByte == 'u'){
@@ -341,34 +360,6 @@ void loop()
   Serial.printf(">OUT4:%d\n",digitalRead(4));
 
   Ascenseur_gestion();
-  /*
-  if(sms_sts.FeedBack(ID)!=-1){
-    Pos = sms_sts.ReadPos(-1);
-    Speed = sms_sts.ReadSpeed(-1);
-    Load = sms_sts.ReadLoad(-1);
-    Voltage = sms_sts.ReadVoltage(-1);
-    Temper = sms_sts.ReadTemper(-1);
-    Move = sms_sts.ReadMove(-1);
-    Current = sms_sts.ReadCurrent(-1);
-    Serial.print(">Position:");
-    Serial.println(Pos);
-    Serial.print(">Speed:");
-    Serial.println(Speed);
-    Serial.print(">Load:");
-    Serial.println(Load);
-    Serial.print(">Voltage:");
-    Serial.println(Voltage);
-    Serial.print(">Temper:");
-    Serial.println(Temper);
-    Serial.print(">Move:");
-    Serial.println(Move);
-    Serial.print(">Current:");
-    Serial.println(Current);
-    delay(50);
-  }else{
-    Serial.println("FeedBack err");
-    delay(500);
-  }*/
 
   delay(25);
 }
@@ -406,7 +397,24 @@ enum etat_action_t Actionneur_deplacement(void){
 
 }
 
-enum etat_action_t Actionneur_prepare_prise(void){
+enum etat_action_t Actionneur_prepare_prise_extern(void){
+  Servo_set(FOURCHE_TRANSPORT);
+  // Ascenseur en bas
+  Ascenseur_descend();
+  // Pinces internes fermées
+  Servo_set(DOIGT_PINCE_DROIT_FERME);
+  Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+  // Aimant sortis
+  Servo_set(AIMANT_PINCE_DROIT_LACHE);
+  Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+  // Translateur à l'arrière
+  Translateur_recule();
+
+  return ACTION_TERMINEE;
+
+}
+
+enum etat_action_t Actionneur_prepare_prise_planche(void){
   Servo_set(FOURCHE_PRISE);
   // Ascenseur en bas
   Ascenseur_descend();
@@ -423,20 +431,32 @@ enum etat_action_t Actionneur_prepare_prise(void){
 
 }
 
+enum etat_action_t Actionneur_depile_planche(void){
+  Servo_set(AIMANT_PINCE_DROIT_LACHE);
+  Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+  delay(1000);
+  ledcWrite(FOURCHE_LEVEE);
+  delay(1000);
+  for(int i=0;i<20;i++){
+    ledcWrite(FOURCHE_PRISE);
+    delay(50);
+    ledcWrite(FOURCHE_LEVEE);
+    delay(200);
+  }
+  
+  return ACTION_TERMINEE;
+
+}
+
 enum etat_action_t Actionneur_prise_initiale(void){
   static enum etat_actionneur_t{
-    ACTIONNEUR_DEPILE,
     ACTIONNEUR_PRISE_INTERNE_1,
     ACTIONNEUR_PRISE_INTERNE_2,
     ACTIONNEUR_RECULE_PRISE_INTERNE_1,
     ACTIONNEUR_RECULE_PRISE_INTERNE_2
-  } etat_actionneur=ACTIONNEUR_DEPILE;
+  } etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
 
   switch(etat_actionneur){
-    case ACTIONNEUR_DEPILE:
-      Servo_set(FOURCHE_LEVEE);
-      etat_actionneur = ACTIONNEUR_PRISE_INTERNE_1;
-      break;
 
     case ACTIONNEUR_PRISE_INTERNE_1:
       Serial.println("ACTIONNEUR_PRISE_INTERNE_1");
@@ -467,8 +487,8 @@ enum etat_action_t Actionneur_prise_initiale(void){
     case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
       Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
       if(Translateur_etat() == ACTION_TERMINEE){
-        Servo_set(FOURCHE_TRANSPORT);
-        etat_actionneur=ACTIONNEUR_DEPILE;
+        //Servo_set(FOURCHE_TRANSPORT);
+        etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
         return ACTION_TERMINEE;
       }
       break;
@@ -478,6 +498,11 @@ enum etat_action_t Actionneur_prise_initiale(void){
 
 }
 
+enum etat_action_t Actionneur_fourche_transport(){
+  Servo_set(FOURCHE_TRANSPORT);
+  return ACTION_TERMINEE;
+}
+
 enum etat_action_t Actionneur_empile(){
   static enum etat_actionneur_t{
     ACTIONNEUR_LEVE,

Cerveau/Cerveau.ino

@@ -338,11 +338,15 @@ void gestion_match(){
         translation_x_mm = 0;
         translation_y_mm = 0;
         rotation_rad = 100;
+        Serial.println("C1");
         Triangulation_send_immobile(1);
+        Serial.println("C2");
         while(M5.BtnC.read()){
           M5.update();
         }
+        Serial.println("C3");
         delay(200);
+        Serial.println("C4");
         IHM_attente_match(&couleur);
         tirette_enlevee = 0;
 
@@ -438,39 +442,49 @@ void IHM_attente_match(int * couleur){
 
 enum etat_action_t Strategie(int couleur){
   static enum {
+    STRAT_INIT, // On attend de ne pas voir la tirette dans la detection de l'adversaire
     STRAT_RECULE_BANDEROLE, // Deplacement 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_1A, // Empile gradin
     STRAT_DEPOSE_GRADIN_1B, // Recule du gradin
-    STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
-    STRAT_ALLER_BACKSTAGE // Déplacement relatif
+    STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement relaif
+    STRAT_ATTENTE_BACKSTAGE, // Attente
+    STRAT_ALLER_BACKSTAGE, // Déplacement relatif
+    STRAT_FIN
 
-  }etat_strategie = STRAT_RECULE_BANDEROLE;
+  }etat_strategie = STRAT_INIT;
   enum etat_action_t etat_action;
   int translation_x_mm, translation_y_mm;
+  static unsigned long temps_timer;
   float rotation_rad;
 
   switch(etat_strategie){
+    case STRAT_INIT:
+      temps_timer = millis();
+      etat_strategie = STRAT_RECULE_BANDEROLE;
+      break;
     case STRAT_RECULE_BANDEROLE:
-      // Déplacement en X
-      translation_x_mm = -300;
-      translation_y_mm = 0;
-      rotation_rad = 0;
-      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
-      if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_ALLER_GRADINS_1_A;
+      if(millis() - temps_timer >300){
+        // Déplacement en X
+        translation_x_mm = -300;
+        translation_y_mm = 0;
+        rotation_rad = 0;
+        etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
+        if(etat_action == ACTION_TERMINEE){
+          etat_strategie = STRAT_ALLER_GRADINS_1_A;
+        }
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_A:
-      translation_x_mm = -500;
+      translation_x_mm = -450;
       rotation_rad = 0;
       if(couleur == COULEUR_JAUNE){
-        translation_y_mm = -500;
+        translation_y_mm = -450;
       }else{
-        translation_y_mm = 500;
+        translation_y_mm = 450;
       }
       etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
       if(etat_action == ACTION_TERMINEE){
@@ -480,13 +494,13 @@ enum etat_action_t Strategie(int couleur){
 
     case STRAT_ALLER_GRADINS_1_B:
       etat_action = gradin_approche();
-      if(etat_action == ACTION_TERMINEE){
+      if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
         etat_strategie = STRAT_ALLER_GRADINS_1_C;
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_C:
-      etat_action = deplacement_relatif(120, 0, 0, 0);
+      etat_action = deplacement_relatif(100, 0, 0, 0);
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_DEPOSE_GRADIN_1A;
       }
@@ -511,22 +525,31 @@ enum etat_action_t Strategie(int couleur){
 
     case STRAT_ALLER_PREPA_BACKSTAGE:
       if(couleur == COULEUR_JAUNE){
-        etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
+        etat_action = deplacement_relatif(0, 0, -15. / 180 * M_PI, 0);
       }else{
-        etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0);
+        etat_action = deplacement_relatif(0, 0, 15. / 180 * M_PI, 0);
       }
       if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ATTENTE_BACKSTAGE;
+      }
+      break;
+
+    case STRAT_ATTENTE_BACKSTAGE:
+      if (millis() - temps_timer > 95000){
         etat_strategie = STRAT_ALLER_BACKSTAGE;
       }
       break;
 
     case STRAT_ALLER_BACKSTAGE:
-      etat_action = deplacement_relatif(500, 0, 0, 0);
+      etat_action = deplacement_relatif(-1140, 0, 0, 1);
       if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_RECULE_BANDEROLE;
-        return ACTION_TERMINEE;
+        etat_strategie = STRAT_FIN;
       }
       break;
+
+    case STRAT_FIN:
+      return ACTION_TERMINEE;
+      break;
   }
 
   return ACTION_EN_COURS;
@@ -610,7 +633,6 @@ enum etat_action_t gradin_approche(void){
 
     case GA_GOTO_EN_FACE:
     case GA_GOTO_LARGE:
-      Detect_gradin(&detect_gradin);
       if(detect_gradin.status != 2){
         nb_erreur++;
         if(nb_erreur > 100){
@@ -648,7 +670,8 @@ enum etat_action_t gradin_approche(void){
         statu_approche_gradin = GA_CHERCHE_GAUCHE;
       }else if(detect_gradin.centre_y_mm < 180){
         statu_approche_gradin = GA_RECULE;
-      }else if(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD){
+      }else if((fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD * 6) && (detect_gradin.centre_y_mm >= 240) ||
+              (fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD) && (detect_gradin.centre_y_mm < 240)){
         angle_mem = detect_gradin.angle_rad;
       }else{
         if(detect_gradin.centre_y_mm > 240){
@@ -677,11 +700,24 @@ enum etat_action_t gradin_approche(void){
 
     case GA_GOTO_PREND_2:
       // On positionne la fourche
-      while(actionneur_action(ACTIONNEUR_PREPARE_PRISE)!=ACTION_TERMINEE);
+      while(actionneur_action(ACTIONNEUR_PREPARE_PRISE_PLANCHE)!=ACTION_TERMINEE);
       // On Avance de 15 cm
       while(deplacement_relatif(150, 0, 0, 0) != ACTION_TERMINEE);
-      // On attrape
+      // Depile les planches
+      while(actionneur_action(ACTIONNEUR_DEPILE)!=ACTION_TERMINEE);
+      // Recule de 4 cm
+      while(deplacement_relatif(-40, 0, 0, 0) != ACTION_TERMINEE);
+      // Prepa prise externe
+      while(actionneur_action(ACTIONNEUR_PRISE_EXTERNE)!=ACTION_TERMINEE);
+      // Avance de 4,4 cm
+      while(deplacement_relatif(65, 0, 0, 0) != ACTION_TERMINEE);
+      // On attrape les canettes internes
       while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE);
+      // On recule pour ne pas faire tomber la fourche sur les cannettes
+      //while(deplacement_relatif(-20, 0, 0, 0) != ACTION_TERMINEE);
+      // On baisse la fourche
+      //while(actionneur_action(ACTIONNEUR_FOURCHE_TRANSPORT)!=ACTION_TERMINEE);
+
       statu_approche_gradin = GA_CHERCHE_GAUCHE;
       return ACTION_TERMINEE;
       break;

Cerveau/Com_actionneur.h

@@ -6,9 +6,12 @@
 #define ACTIONNEUR_NO_ACTION 0
 #define ACTIONNEUR_POS_INITIAL 1
 #define ACTIONNEUR_DEPLACEMENT 2
-#define ACTIONNEUR_PREPARE_PRISE 3
+#define ACTIONNEUR_PREPARE_PRISE_PLANCHE 3
 #define ACTIONNEUR_PRISE 4
 #define ACTIONNEUR_EMPILE 5
+#define ACTIONNEUR_DEPILE 6
+#define ACTIONNEUR_PRISE_EXTERNE 7
+#define ACTIONNEUR_FOURCHE_TRANSPORT 8
 
 struct com_actionneur_t{
   char demande_action, action_terminee;

Cerveau/Com_chassis.ino

@@ -8,23 +8,23 @@ int Scan_chassis(struct chassis_reception_t * chassis_reception){
 }
 
 /// @brief Lit l'état du chassis en I2C
-int Scan_chassis(struct chassis_reception_t * chassis_reception, bool blocking){
+int Scan_chassis(struct chassis_reception_t * chassis_reception, bool continuous_try){
   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){
+  while (error !=0 && continuous_try){
     Err_Chassi_com =1;IndexErr = 2;
     affiche_erreur("Scan_Chassi", "Erreur I2C");
-    if(blocking){
-      while(1);
-    }
-  }else{
+    error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);  
+  }
+  if(error == 0){
     Err_Chassi_com =0;
     IndexErr = 0;
 
     Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
     chassis_reception->status = tampon2[0];
   }
+
   return error;
 }
 
@@ -55,8 +55,11 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
     Mot[10] = chassis_emission->acceleration; 
 
     error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
-    if (error !=0){Err_Cha_send =1; IndexErr = 5;}
-    else{Err_Cha_send =0;IndexErr = 0;}
+    while (error !=0){
+      error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
+      Err_Cha_send =1; IndexErr = 5;
+    }
+    Err_Cha_send =0;IndexErr = 0;
     if(error==0){   //si pas d'erreur de transmission alors RAZ des valeurs
       nbr_essai ++;
       cmd_chassi_x = 0;

Cerveau/Com_detection_adversaire.ino

@@ -6,17 +6,14 @@ 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
-int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool blocking){
+int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool continuous_try){
   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){
+  while (error !=0 && continuous_try){
     affiche_erreur("Scan_Detect_adversaire", "Erreur I2C");
-    if(blocking){
-      while(1);
-    }
-  }else{
-    Serial.println("I2C OK");
+    error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
+    
   }
   return error;
 }