Secoue les planches, v1

Actionneurs/Actionneurs.ino

@@ -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
+        // Position de prise planche
-        if(Actionneur_prepare_prise() == ACTION_TERMINEE){
+        if(Actionneur_prepare_prise_planche() == ACTION_TERMINEE){
           I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
         }
         break;
@@ -219,6 +221,16 @@ 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;
       
 
     }
@@ -301,7 +313,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 +353,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 +390,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_PRISE);
+  // 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_TIENT);
+  Servo_set(AIMANT_PINCE_GAUCHE_TIENT);
+  // 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 +424,37 @@ 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);
+  ledcWrite(FOURCHE_PRISE);
+  delay(100);
+  ledcWrite(FOURCHE_LEVEE);
+  delay(250);
+  ledcWrite(FOURCHE_PRISE);
+  delay(100);
+  ledcWrite(FOURCHE_LEVEE);
+  delay(250);
+  ledcWrite(FOURCHE_PRISE);
+  delay(100);
+  ledcWrite(FOURCHE_LEVEE);
+
+  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");
@@ -444,6 +462,7 @@ enum etat_action_t Actionneur_prise_initiale(void){
       
       Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
       Servo_set(DOIGT_PINCE_DROIT_OUVRE);
+      Servo_set(FOURCHE_LEVEE);
       etat_actionneur = ACTIONNEUR_PRISE_INTERNE_2;
       break;
 
@@ -468,7 +487,7 @@ enum etat_action_t Actionneur_prise_initiale(void){
       Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
       if(Translateur_etat() == ACTION_TERMINEE){
         Servo_set(FOURCHE_TRANSPORT);
-        etat_actionneur=ACTIONNEUR_DEPILE;
+        etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
         return ACTION_TERMINEE;
       }
       break;

Cerveau/Cerveau.ino

@@ -425,12 +425,12 @@ void IHM_attente_match(int * couleur){
     M5.update();
     if(M5.BtnA.read() == 1){
       affiche_msg("Couleur", "  BLEU     ");
-      //Triangulation_send_config(configuration_match_bleu);
+      Triangulation_send_config(configuration_match_bleu);
       m_couleur = COULEUR_BLEU;
     }
     if(M5.BtnB.read() == 1){
       affiche_msg("Couleur", "  JAUNE    ");
-      //Triangulation_send_config(configuration_match_jaune);
+      Triangulation_send_config(configuration_match_jaune);
       m_couleur = COULEUR_JAUNE;
     }
     if(M5.BtnC.read() == 1){
@@ -449,12 +449,6 @@ enum etat_action_t Strategie(int couleur){
     STRAT_ALLER_GRADINS_1_C,  // Déplacement relatif
     STRAT_DEPOSE_GRADIN_1A, // Empile gradin
     STRAT_DEPOSE_GRADIN_1B, // Recule du gradin
-    STRAT_ALLER_GRADINS_2_A, // Tourne 80 °
-    STRAT_ALLER_GRADINS_2_B, // Avance 20 cm
-    STRAT_ALLER_GRADINS_2_C, // Cherche gradin
-    STRAT_ALLER_GRADINS_2_D,
-    //
-
     STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement relaif
     STRAT_ATTENTE_BACKSTAGE, // Attente
     STRAT_ALLER_BACKSTAGE, // Déplacement relatif
@@ -506,7 +500,7 @@ enum etat_action_t Strategie(int couleur){
       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;
       }
@@ -528,44 +522,6 @@ enum etat_action_t Strategie(int couleur){
         etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
       }
 
-    case  STRAT_ALLER_GRADINS_2_A: // Tourne 80 °
-      translation_x_mm = 0;
-      if(couleur == COULEUR_JAUNE){
-        rotation_rad = 80. * M_PI /180.;
-      }else{
-        rotation_rad = -80. * M_PI /180.;
-      }
-      translation_y_mm = 0;
-      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
-      if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_ALLER_GRADINS_2_B;
-      }
-      break;
-
-    case STRAT_ALLER_GRADINS_2_B: // Avance 20 cm
-      translation_x_mm = 20;
-      rotation_rad = 0;
-      translation_y_mm = 0;
-      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
-      if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_ALLER_GRADINS_2_C;
-      }
-      break;
-
-    case STRAT_ALLER_GRADINS_2_C: // Cherche gradin
-      etat_action = gradin_approche();
-      if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
-        etat_strategie = STRAT_ALLER_GRADINS_2_D;
-      }
-      break;
-
-    case STRAT_ALLER_GRADINS_2_D:
-      etat_action = actionneur_action(ACTIONNEUR_EMPILE);
-      if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_FIN;
-      }
-      break;
-
 
     case STRAT_ALLER_PREPA_BACKSTAGE:
       if(couleur == COULEUR_JAUNE){
@@ -585,7 +541,7 @@ enum etat_action_t Strategie(int couleur){
       break;
 
     case STRAT_ALLER_BACKSTAGE:
-      etat_action = deplacement_relatif(-1140, 0, 0, 0);
+      etat_action = deplacement_relatif(-1140, 0, 0, 1);
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_FIN;
       }
@@ -714,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){
@@ -743,10 +700,18 @@ 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(-20, 0, 0, 0) != ACTION_TERMINEE);
+      // Prepa prise externe
+      while(actionneur_action(ACTIONNEUR_PRISE_EXTERNE)!=ACTION_TERMINEE);
+      // Avance de 4,4 cm
+      while(deplacement_relatif(45, 0, 0, 0) != ACTION_TERMINEE);
+      // On attrape les canettes internes
       while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE);
       statu_approche_gradin = GA_CHERCHE_GAUCHE;
       return ACTION_TERMINEE;

Cerveau/Com_actionneur.h

@@ -6,9 +6,11 @@
 #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
 
 struct com_actionneur_t{
   char demande_action, action_terminee;