WIP - sequence empillement

Actionneurs/Actionneurs.ino

@@ -9,9 +9,9 @@
 #define FOURCHE_LEVEE     5, 217
 #define FOURCHE_PRISE     5, 327
 
-#define DOIGT_PINCE_GAUCHE_FERME 3, 1000
-#define DOIGT_PINCE_GAUCHE_OUVRE 3, 800
-#define DOIGT_PINCE_GAUCHE_SEUIL 815
+#define DOIGT_PINCE_GAUCHE_FERME 3, 1500
+#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1842
+#define DOIGT_PINCE_GAUCHE_SEUIL 1515
 
 /// TODO renseigner de vraies valeurs
 #define DOIGT_PINCE_DROIT_FERME 4, 1000
@@ -34,11 +34,7 @@ struct position_t{
   uint position;
 } cible_haute, cible_basse, actuelle;
 
-enum etat_action_t{
-  ACTION_EN_COURS,
-  ACTION_TERMINEE,
-  ACTION_ECHEC
-};
+
 
 enum translateur_action_t{
   TRANSLATEUR_INIT,
@@ -46,19 +42,7 @@ enum translateur_action_t{
   TRANSLATEUR_RECULE,
 }translateur_action = TRANSLATEUR_INIT;
 
-
-enum etat_actionneur_t{
-  ACTIONNEUR_DEPLACEMENT,
-  ACTIONNEUR_PRISE_EXTERNE,
-  ACTIONNEUR_DEPILE,
-  ACTIONNEUR_PRISE_INTERNE,
-  ACTIONNEUR_RECULE_PRISE_INTERNE,
-  ACTIONNEUR_LEVE,
-  ACTIONNEUR_AVANCE_PRISE_INTERNE,
-  ACTIONNEUR_DEPOSE_PRISE_INTERNE,
-  ACTIONNEUR_RANGE_TRANSLATEUR,
-  ACTIONNEUR_DESCEND
-}
+enum etat_action_t Actionneur_empile();
 
 
 /// @brief pilote la vitesse des moteurs
@@ -145,6 +129,7 @@ void setup()
 
   ledcAttach(9, 50, 12);
   ledcWrite(9, 307);
+  
 
   Translateur_init();
   
@@ -211,7 +196,7 @@ void Translateur_init(void){
 enum etat_action_t Translateur_etat(void){
   if(translateur_action == TRANSLATEUR_AVANCE){
     if(digitalRead(2) == 0){
-      return ACTION_TERMINEE;
+        return ACTION_TERMINEE;
     }
   }
   if(translateur_action == TRANSLATEUR_RECULE){
@@ -317,6 +302,12 @@ void loop()
     if(inByte == 'a'){
       Translateur_avance();
     }
+    if(inByte == 'w'){
+      Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
+    }
+    if(inByte == 'x'){
+      Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+    }
     if(inByte == 'z'){
       Translateur_recule();
     }
@@ -329,6 +320,9 @@ void loop()
     if(inByte == '3'){
       reglage_servo(5);
     }
+    if(inByte == 'u'){
+      while(Actionneur_empile() == ACTION_EN_COURS);
+    }
     if(inByte == 'c'){
       while(Serial.available() > 0){
       inByte = Serial.read();
@@ -348,6 +342,10 @@ void loop()
       inByte = Serial.read();
     }
   }
+  Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
+  Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
+  Serial.printf(">GPIO2:%d\n",digitalRead(2));
+  Serial.printf(">GPIO10:%d\n",digitalRead(10));
 
   Ascenseur_gestion();
   /*
@@ -378,7 +376,143 @@ void loop()
     Serial.println("FeedBack err");
     delay(500);
   }*/
+
+  delay(25);
+}
+
+void Servo_set(int servo, int position){
+  switch(servo){
+    case 5:
+    case 8:
+    case 9:
+      ledcWrite(servo, position);
+      break;
+
+    case 3:
+    case 4:
+      sms_sts.WritePosEx(servo, position, 2000);
+      break;
+
+  }
 }
 
 
+enum etat_action_t Actionneur_empile(){
+  static enum etat_actionneur_t{
+    ACTIONNEUR_DEPLACEMENT,
+    ACTIONNEUR_PRISE_EXTERNE,
+    ACTIONNEUR_DEPILE,
+    ACTIONNEUR_PRISE_INTERNE_1,
+    ACTIONNEUR_PRISE_INTERNE_2,
+    ACTIONNEUR_RECULE_PRISE_INTERNE_1,
+    ACTIONNEUR_RECULE_PRISE_INTERNE_2,
+    ACTIONNEUR_LEVE,
+    ACTIONNEUR_AVANCE_PRISE_INTERNE,
+    ACTIONNEUR_DEPOSE_PRISE_INTERNE,
+    ACTIONNEUR_RANGE_TRANSLATEUR,
+    ACTIONNEUR_DESCEND
+  } etat_actionneur=ACTIONNEUR_DEPLACEMENT;
 
+  switch(etat_actionneur){
+    case ACTIONNEUR_DEPLACEMENT:
+      Servo_set(FOURCHE_TRANSPORT);
+      Translateur_recule();
+      Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+      Servo_set(DOIGT_PINCE_DROIT_FERME);
+      Servo_set(AIMANT_PINCE_DROIT_LACHE);
+      Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+      delay(5000);
+      etat_actionneur = ACTIONNEUR_PRISE_EXTERNE;
+      break;
+
+    case ACTIONNEUR_PRISE_EXTERNE:
+      Servo_set(FOURCHE_PRISE);
+      Servo_set(AIMANT_PINCE_DROIT_LACHE);
+      Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+      // Attente avance
+      delay(5000);
+      etat_actionneur = ACTIONNEUR_DEPILE;
+      break;
+
+    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");
+      Translateur_avance();
+      
+      Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
+      Servo_set(DOIGT_PINCE_DROIT_OUVRE);
+      etat_actionneur = ACTIONNEUR_PRISE_INTERNE_2;
+      break;
+
+    case ACTIONNEUR_PRISE_INTERNE_2:
+      Serial.println("ACTIONNEUR_PRISE_INTERNE_2");
+      if(Translateur_etat() == ACTION_TERMINEE){
+        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        Servo_set(DOIGT_PINCE_DROIT_FERME);
+        delay(500);
+        etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_1;
+      }
+      break;
+
+    case ACTIONNEUR_RECULE_PRISE_INTERNE_1:
+      Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_1");
+      Translateur_recule();
+      delay(1000);
+      etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_2;
+      break;
+
+    case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
+      Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
+      if(Translateur_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_LEVE;
+        Ascenseur_monte();
+      }
+      break;
+
+    case ACTIONNEUR_LEVE:
+      Serial.println("ACTIONNEUR_LEVE");
+      if(Ascenseur_get_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_AVANCE_PRISE_INTERNE;
+        Translateur_avance();
+      }
+      break;
+
+    case ACTIONNEUR_AVANCE_PRISE_INTERNE:
+      Serial.println("ACTIONNEUR_AVANCE_PRISE_INTERNE");
+      if(Translateur_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_DEPOSE_PRISE_INTERNE;
+      }
+      break;
+
+    case ACTIONNEUR_DEPOSE_PRISE_INTERNE:
+      Servo_set(DOIGT_PINCE_DROIT_OUVRE);
+      Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
+      delay(500);
+      Translateur_recule();
+      etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
+      break;
+
+    case ACTIONNEUR_RANGE_TRANSLATEUR:
+      if(Translateur_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_DESCEND;
+        Servo_set(DOIGT_PINCE_DROIT_FERME);
+        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        Ascenseur_monte();
+      }
+      break;
+
+    case ACTIONNEUR_DESCEND:
+      if(Ascenseur_get_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_DEPLACEMENT;
+        Servo_set(FOURCHE_TRANSPORT);
+        return ACTION_TERMINEE;
+      }
+      break;
+  }
+  Ascenseur_gestion(); 
+  return ACTION_EN_COURS;
+}

Actionneurs/Ascenseur.h

@@ -1,6 +1,13 @@
+enum etat_action_t{
+  ACTION_EN_COURS,
+  ACTION_TERMINEE,
+  ACTION_ECHEC
+};
+
 void Ascenseur_init(void);
 int Ascenseur_monte(void);
 int Ascenseur_descend(void);
 void Ascenseur_step_up(void);
 void Ascenseur_step_down(void);
-void Ascenseur_gestion(void);
+void Ascenseur_gestion(void);
+enum etat_action_t Ascenseur_get_etat(void);

Actionneurs/Ascenseur.ino

@@ -155,6 +155,13 @@ int Ascenseur_descend(void){
   return 1;
 }
 
+enum etat_action_t Ascenseur_get_etat(void){
+  if(etat_ascenseur == ASCENSEUR_ACTIF){
+    return ACTION_TERMINEE;
+  }
+  return ACTION_EN_COURS;
+}
+
 void Ascenseur_step_up(void){
   step--;
   Ascenseur_update_step();