Translateur et Ascenseur : encapsulation et relecture des butées du translateur

Actionneurs/Actionneurs.ino

@@ -13,9 +13,6 @@
 #define DOIGT_PINCE_GAUCHE_OUVRE 2367
 #define DOIGT_PINCE_GAUCHE_PARKING 1550
 
-#define ID_FEETECH_ASC_G 11
-#define ID_FEETECH_ASC_D 10
-
 SMS_STS sms_sts;
 
 int ID_Feetech = 4;
@@ -31,6 +28,12 @@ enum etat_action_t{
   ACTION_ECHEC
 };
 
+enum translateur_action_t{
+  TRANSLATEUR_INIT,
+  TRANSLATEUR_AVANCE,
+  TRANSLATEUR_RECULE,
+}translateur_action = TRANSLATEUR_INIT;
+
 
 /// @brief pilote la vitesse des moteurs
 /// @param vitesse vitesse signée, sera saturée à 255
@@ -52,19 +55,22 @@ void Moteur_set(int vitesse){
 }
 
 void Translateur_avance(){
-  Moteur_set(128);
+  Moteur_set(-255);
+  translateur_action = TRANSLATEUR_AVANCE;
 }
 
 void Translateur_recule(){
-  Moteur_set(-128);
+  Moteur_set(255);
+  translateur_action = TRANSLATEUR_RECULE;
 }
 
 void setup()
 {
+  int liste_servo_id[4]= {3, 4, 10, 11};
   Serial.begin(115200);//sms舵机波特率115200
   Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
   sms_sts.pSerial = &Serial1;
-  delay(1000);
+  delay(5000);
   
   cible_haute.nb_tour = 0;
   cible_haute.position = 500;
@@ -93,6 +99,18 @@ void setup()
     delay(5000);
 
   }*/
+  int error = 0;
+  do{
+    error = 0;
+    for (int i =0; i<4; i++){
+      int ID = sms_sts.Ping(liste_servo_id[i]);
+      printf("Servo %d: %d\n", liste_servo_id[i], ID );
+      if( ID != liste_servo_id[i]){
+        error = 1;
+        delay(100);
+      }
+    }
+  }while(error);
 
 
   // Servo pinces à aimant
@@ -102,11 +120,8 @@ void setup()
   ledcAttach(9, 50, 12);
   ledcWrite(9, 307);
 
-  // Moteur
+  Translateur_init();
-  ledcAttach(3, 500, 8);
+  
-  ledcWrite(3, 0);
-  ledcAttach(4, 500, 8);
-  ledcWrite(4, 0);
 
   /*while(1){
 
@@ -129,6 +144,15 @@ void setup()
     delay(5000);
 
   }*/
+  /*
+  sms_sts.WheelMode(10);
+  sms_sts.WriteSpe(10, 2000, 50);
+  while(1){
+    printf(">load_d:%d\n", sms_sts.ReadLoad(10));
+    delay(25);
+  }*/
+
+
 
 
   if(sms_sts.FeedBack(ID_Feetech)!=-1){
@@ -145,6 +169,45 @@ void setup()
 }
 
 
+void Translateur_init(void){
+// Moteur
+  ledcAttach(3, 500, 8);
+  ledcWrite(3, 0);
+  ledcAttach(4, 500, 8);
+  ledcWrite(4, 0);
+  // Etat des contacteurs de fin de course
+  // relecture de 3 sur 2 (Avant)
+  // relecture de 4 sur 10 (Arrière)
+  pinMode(2, INPUT);
+  pinMode(10, INPUT);
+}
+
+enum etat_action_t Translateur_etat(void){
+  if(translateur_action == TRANSLATEUR_AVANCE){
+    if(digitalRead(2) == 0){
+      return ACTION_TERMINEE;
+    }
+  }
+  if(translateur_action == TRANSLATEUR_RECULE){
+    if(digitalRead(10) == 0){
+      return ACTION_TERMINEE;
+    }
+  }
+  return ACTION_EN_COURS;
+}
+
+void Translateur_cycle(void){
+  if(Translateur_etat() == ACTION_TERMINEE){
+    if(translateur_action == TRANSLATEUR_RECULE){
+      Translateur_avance();
+    }else{
+      Translateur_recule();
+    }
+  }
+}
+
+
+
 void loop()
 {
   static int m_pos=0;
@@ -152,22 +215,14 @@ void loop()
   static position_t cible;
 
   
+  
+  
+  if(millis() > myTime + 100){
+    myTime = millis();
     Serial.print(">millis:");
     Serial.println(millis());
-  if(millis() > myTime + 4000){
-    myTime = millis();
-    
   }
 
-  int ID = sms_sts.Ping(ID_Feetech);
-  if(ID!=-1){
-    Serial.print("Servo ID:");
-    Serial.println(ID, DEC);
-    delay(100);
-  }else{
-    Serial.println("Ping servo ID error!");
-    delay(2000);
-  }
 
     int Pos;
   int Speed;
@@ -194,13 +249,34 @@ void loop()
     if(inByte == 'l'){
       Ascenseur_descend();
     }
+    if(inByte == 'a'){
+      Translateur_avance();
+    }
+    if(inByte == 'z'){
+      Translateur_recule();
+    }
+    if(inByte == 'c'){
+      while(Serial.available() > 0){
+      inByte = Serial.read();
+      }
+      while(1){
+        Translateur_cycle();
+        if(Serial.available() > 0){
+          break;
+        }
+        printf(">GPIO2:%d\n",digitalRead(2));
+        printf(">GPIO10:%d\n",digitalRead(10));
+      }
+    }
+
+
     while(Serial.available() > 0){
       inByte = Serial.read();
     }
   }
 
   Ascenseur_gestion();
-
+  /*
   if(sms_sts.FeedBack(ID)!=-1){
     Pos = sms_sts.ReadPos(-1);
     Speed = sms_sts.ReadSpeed(-1);
@@ -227,6 +303,6 @@ void loop()
   }else{
     Serial.println("FeedBack err");
     delay(500);
-  }
+  }*/
 }
 

Actionneurs/Ascenseur.h

@@ -0,0 +1,6 @@
+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);

Actionneurs/Ascenseur.ino

@@ -52,6 +52,9 @@ void Ascenseur_config_servo(int id){
 
 enum etat_action_t Ascenseur_cherche_butees(void){
   static bool butee_g = 0, butee_d = 0;
+  static int old_pos_d, old_pos_g;
+  int pos_d, pos_g, my_load;
+
   static enum {
     CHERCHE_BUTEE_INIT,
     CHERCHE_BUTEE,
@@ -66,33 +69,49 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       tab_vitesses[1] = 500 * ASC_G_SIGNE;
       sms_sts.WheelMode(ID_FEETECH_ASC_D);
       sms_sts.WheelMode(ID_FEETECH_ASC_G);
+      old_pos_d = sms_sts.ReadPos(ID_FEETECH_ASC_D); 
+      old_pos_g = sms_sts.ReadPos(ID_FEETECH_ASC_G);
       sms_sts.SyncWriteSpe(asc_ID, 2, tab_vitesses, tab_acc);
       delay(200);
       etat_cherche_butees = CHERCHE_BUTEE;
+      Serial.printf("CHERCHE_BUTEE_INIT\n");
       break;
 
     case CHERCHE_BUTEE:
       {
-      int my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
+      
-      if(abs(my_load) > 150 ){
+      /*pos_d = sms_sts.ReadPos(ID_FEETECH_ASC_D);
+      printf(">delta_pos_d:%d\n",pos_d - old_pos_d);
+      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 ){
         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);
         butee_d = true;
       }
+      //old_pos_d = pos_d;
       
+      /*
+      pos_g = sms_sts.ReadPos(ID_FEETECH_ASC_G);
+      printf(">delta_pos_g:%d\n",pos_g - old_pos_g);*/
       my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
-      if(abs(my_load) > 150 ){
+      printf(">load_g:%d\n", my_load);
+      if(abs(my_load) > 250 ){
         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);
         butee_g = true;
       }
+      //old_pos_g = pos_g;
 
       if(butee_d && butee_g){
         
         etat_cherche_butees = EA_POS_INIT;
       }
+      Serial.printf("CHERCHE_BUTEE\n");
+      delay(100);
       break;}
 
     case EA_POS_INIT:
@@ -106,6 +125,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       tab_vitesses_u[1] = 2000;
       sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
       etat_cherche_butees = CHERCHE_BUTEE_INIT;
+      Serial.printf("EA_POS_INIT\n");
       return ACTION_TERMINEE;
       break;
   }
@@ -177,10 +197,12 @@ void Ascenseur_gestion(void){
     case ASCENSEUR_INIT:
       // On vérifie que les deux servomoteurs répondent
       do{
+        erreur = 0;
         int ID = sms_sts.Ping(asc_ID[0]);
         erreur = sms_sts.getLastError();
         ID = sms_sts.Ping(asc_ID[1]);
         erreur +=  sms_sts.getLastError();
+        Serial.printf("ASCENSEUR_INIT erreur:%d\n",erreur);
       }while(erreur != 0);
       etat_ascenseur = ASCENSEUR_ACTIF;
       break;