Reglage des servos + fonctions de réglage

Actionneurs/Actionneurs.ino

@@ -1,19 +1,29 @@
 #include <SCServo.h>
+#include "Ascenseur.h"
 
 #define SERVO_FOUCHE 5
 #define SERVO_PINCE_GAUCHE 8
 #define SERVO_PINCE_DROITE 9
 
-#define FOURCHE_TRANSPORT 5, 307
-#define FOURCHE_LEVEE     5, 227
-#define FOURCHE_PRISE     5, 345
+#define FOURCHE_TRANSPORT 5, 287
+#define FOURCHE_LEVEE     5, 217
+#define FOURCHE_PRISE     5, 327
 
-#define DOIGT_PINCE_GAUCHE_FERME 2080
-#define DOIGT_PINCE_GAUCHE_OUVRE 2367
-#define DOIGT_PINCE_GAUCHE_PARKING 1550
+#define DOIGT_PINCE_GAUCHE_FERME 3, 1000
+#define DOIGT_PINCE_GAUCHE_OUVRE 3, 800
+#define DOIGT_PINCE_GAUCHE_SEUIL 815
 
-#define ID_FEETECH_ASC_G 11
-#define ID_FEETECH_ASC_D 10
+/// TODO renseigner de vraies valeurs
+#define DOIGT_PINCE_DROIT_FERME 4, 1000
+#define DOIGT_PINCE_DROIT_OUVRE 4, 800
+#define DOIGT_PINCE_DROIT_SEUIL 815
+/// FIN TODO
+
+#define AIMANT_PINCE_DROIT_TIENT 8, 147
+#define AIMANT_PINCE_DROIT_LACHE 8, 337
+
+#define AIMANT_PINCE_GAUCHE_TIENT 9, 500
+#define AIMANT_PINCE_GAUCHE_LACHE 9, 400
 
 SMS_STS sms_sts;
 
@@ -24,6 +34,32 @@ 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,
+  TRANSLATEUR_AVANCE,
+  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
+}
+
 
 /// @brief pilote la vitesse des moteurs
 /// @param vitesse vitesse signée, sera saturée à 255
@@ -45,21 +81,23 @@ 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);
-  sms_sts.WheelMode(ID_Feetech);//舵机ID1切换至电机恒速模式
-  //sms_sts.WriteSpe(ID_Feetech, 2400, 100);
+  delay(5000);
+  
   cible_haute.nb_tour = 0;
   cible_haute.position = 500;
 
@@ -87,6 +125,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
@@ -96,11 +146,8 @@ void setup()
   ledcAttach(9, 50, 12);
   ledcWrite(9, 307);
 
-  // Moteur
-  ledcAttach(3, 500, 8);
-  ledcWrite(3, 0);
-  ledcAttach(4, 500, 8);
-  ledcWrite(4, 0);
+  Translateur_init();
+  
 
   /*while(1){
 
@@ -123,6 +170,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){
@@ -135,97 +191,104 @@ void setup()
     actuelle.position = sms_sts.ReadPos(-1);
   }
 
-  
+  Ascenseur_init();  
 }
 
-void goto_position(int servo_id, position_t position){
-  // Actualise la position
-  struct position_t precedente;
-  static int loin = 1;
-  precedente = actuelle;
-  if(sms_sts.FeedBack(servo_id)!=-1){
-    actuelle.position = sms_sts.ReadPos(-1);
+
+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(abs((int)actuelle.position - (int)precedente.position) > 2000){
-    // Rebouclage compteur
-    if(actuelle.position < precedente.position){
-      // Sens positif
-      actuelle.nb_tour++;
+  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{
-      actuelle.nb_tour--;
+      Translateur_recule();
     }
   }
+}
 
-  // Sens de déplacement
-  if(actuelle.nb_tour != position.nb_tour){
-    if(loin != 1){
-      loin = 1;
-      sms_sts.WheelMode(servo_id);
-      if(actuelle.nb_tour < position.nb_tour){
-        sms_sts.WriteSpe(servo_id, -2400, 50);
-      }else{
-        sms_sts.WriteSpe(servo_id, 2400, 50);
+void reglage_servo(int pin_servo){
+  static int valeur=307; // Neutre
+  printf("reglage_servo %d\n", pin_servo);
+  while(1){
+    if (Serial.available() > 0) {
+      // get incoming byte:
+      int inByte = 0;
+      inByte = Serial.read();
+      if(inByte == 'e'){
+        valeur++;
       }
+      if(inByte == 'z'){
+        valeur+=10;
+      }
+      if(inByte == 'a'){
+        valeur+=100;
+      }
+      if(inByte == 'd'){
+        valeur--;
+      }
+      if(inByte == 's'){
+        valeur-=10;
+      }
+      if(inByte == 'f'){
+        valeur-=100;
+      }
+      if(inByte == 'q'){
+        break;
+      }
+      printf("Valeur %d\n", valeur);
+      ledcWrite(pin_servo, valeur);
     }
-  }else{
-    if(loin != 0){
-      loin = 0;
-      sms_sts.ServoMode(servo_id);
-      sms_sts.WritePosEx(ID_Feetech, position.position, 2400, 50);
-    }
+    Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
+    Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
+    delay(50);
   }
-  Serial.printf(">cible_pos:%d\n",position.position);
-  Serial.printf(">cible_nb_tour:%d\n",position.nb_tour);
-  Serial.printf(">act_pos:%d\n",actuelle.position);
-  Serial.printf(">act_nb_tour:%d\n",actuelle.nb_tour);
-
-
 
 }
 
+
+
 void loop()
 {
   static int m_pos=0;
   static unsigned long myTime=0;
   static position_t cible;
 
-  /*
-  //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
-  sms_sts.WritePosEx(1, 4095, 2400, 50);
-  delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
-
-  //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
-  sms_sts.WritePosEx(1, 0, 2400, 50);
-  delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
-  */
-  Serial.print(">millis:");
-  Serial.println(millis());
-  if(millis() > myTime + 4000){
+  
+  
+  
+  if(millis() > myTime + 100){
     myTime = millis();
-    if(m_pos){
-      //sms_sts.WritePosEx(ID_Feetech, 4095, 2400, 50);
-      //sms_sts.WriteSpe(ID_Feetech, 2400, 50);
-      cible = cible_haute;
-      m_pos = 0;
-    }else{
-      m_pos ++;
-      //sms_sts.WriteSpe(ID_Feetech, -2400, 50);
-      //sms_sts.WritePosEx(ID_Feetech, 0, 2400, 50);
-      //sms_sts.WriteSpe(ID_Feetech, -80, 100);
-      cible = cible_basse;
-    }
-  }
-  goto_position(ID_Feetech, cible);
-  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);
+    Serial.print(">millis:");
+    Serial.println(millis());
   }
 
+
     int Pos;
   int Speed;
   int Load;
@@ -233,6 +296,61 @@ void loop()
   int Temper;
   int Move;
   int Current;
+
+
+  if (Serial.available() > 0) {
+    // get incoming byte:
+    int inByte = 0;
+    inByte = Serial.read();
+    if(inByte == 'd'){
+      Ascenseur_step_down();
+    }
+    if(inByte == 'u'){
+      Ascenseur_step_up();
+    }
+    if(inByte == 'm'){
+      Ascenseur_monte();
+    }
+    if(inByte == 'l'){
+      Ascenseur_descend();
+    }
+    if(inByte == 'a'){
+      Translateur_avance();
+    }
+    if(inByte == 'z'){
+      Translateur_recule();
+    }
+    if(inByte == '1'){
+      reglage_servo(8);
+    }
+    if(inByte == '2'){
+      reglage_servo(9);
+    }
+    if(inByte == '3'){
+      reglage_servo(5);
+    }
+    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);
@@ -259,6 +377,8 @@ 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

@@ -0,0 +1,246 @@
+#define ID_FEETECH_ASC_G 11
+#define ID_FEETECH_ASC_D 10
+
+#define ASC_G_SIGNE 1
+#define ASC_D_SIGNE -1
+
+enum etat_ascenseur_t{
+  ASCENSEUR_INIT,
+  ASCENSEUR_ACTIF,
+  ASCENSEUR_MONTE,
+  ASCENSEUR_DESCENT,
+  ASCENSEUR_BUSY,
+}etat_ascenseur=ASCENSEUR_INIT;
+
+// Fonction privée
+void Ascenseur_update_step(void);
+
+int step=0;
+
+u8 asc_ID[2] = {ID_FEETECH_ASC_D, ID_FEETECH_ASC_G};
+s16 tab_vitesses[2], tab_position[2];
+u16 tab_vitesses_u[2];
+u8 tab_acc[2];
+
+int position_basse_gauche, position_haute_gauche;
+int position_basse_droit, position_haute_droit;
+
+
+void Ascenseur_init(void){
+  tab_acc[0] = 50;
+  tab_acc[1] = 50;
+  Ascenseur_config_servo(ID_FEETECH_ASC_D);
+  Ascenseur_config_servo(ID_FEETECH_ASC_G);
+
+  while(Ascenseur_cherche_butees() != ACTION_TERMINEE);
+
+}
+
+void Ascenseur_config_servo(int id){
+  sms_sts.unLockEprom(id); // unlock EPROM-SAFE
+  sms_sts.writeByte(id, 30, 1); // Resolution à 1
+  sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_L, 0); // Multitour 
+  sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_H, 0); // Multitour 
+
+  sms_sts.LockEprom(id); // unlock EPROM-SAFE
+  Serial.printf("Servo ID : %d\n",id);
+  for(int i=0; i < 40; i++){
+    Serial.printf("Memoire %d : %d\n", i, sms_sts.readByte(ID_Feetech, i));
+  }
+
+}
+
+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,
+    EA_POS_INIT,
+  }etat_cherche_butees=CHERCHE_BUTEE_INIT;
+
+  switch(etat_cherche_butees){
+    case CHERCHE_BUTEE_INIT:
+      butee_g = 0;
+      butee_d = 0;
+      tab_vitesses[0] = 500 * ASC_D_SIGNE;
+      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:
+      {
+      
+      /*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);
+      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:
+      sms_sts.ServoMode(ID_FEETECH_ASC_D);
+      sms_sts.ServoMode(ID_FEETECH_ASC_G);
+      tab_position[0] = position_basse_droit + (-250 * ASC_D_SIGNE); // Droit
+      tab_position[1] = position_basse_gauche + (-250 * ASC_G_SIGNE); // Gauche
+      position_haute_droit = position_basse_droit - (8100 * ASC_D_SIGNE);
+      position_haute_gauche = position_basse_gauche - (8100 * ASC_G_SIGNE);
+      tab_vitesses_u[0] = 2000;
+      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;
+  }
+
+  return ACTION_EN_COURS;
+
+}
+
+
+/// @brief Commande l'ascenseur en position haute
+/// @return 1 si l'ascenseur n'est pas prêt
+int Ascenseur_monte(void){
+  if(etat_ascenseur == ASCENSEUR_ACTIF){
+    etat_ascenseur = ASCENSEUR_MONTE;
+    return 0;
+  }
+  return 1;
+}
+
+/// @brief Commande l'ascenseur en position basse
+/// @return 1 si l'ascenseur n'est pas prêt
+int Ascenseur_descend(void){
+  if(etat_ascenseur == ASCENSEUR_ACTIF){
+    etat_ascenseur = ASCENSEUR_DESCENT;
+    return 0;
+  }
+  return 1;
+}
+
+void Ascenseur_step_up(void){
+  step--;
+  Ascenseur_update_step();
+}
+
+void Ascenseur_step_down(void){
+  step++;
+  Ascenseur_update_step();
+}
+
+void Ascenseur_update_step(void){
+  tab_position[0] = position_basse_droit + (250 + step * 500)* (ASC_D_SIGNE); // Droit
+  tab_position[1] = position_basse_gauche + (250 + step * 500)* (ASC_G_SIGNE); // Gauche
+  Serial.printf("position_droit:%d\n", tab_position[0]);
+  Serial.printf("position_gauche:%d\n", tab_position[1]);
+  if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) > 1){
+    tab_position[0] = position_haute_droit;
+  }
+  if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) < 0){
+    tab_position[0] = position_basse_droit;
+  }
+  if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) > 1){
+    tab_position[1] = position_haute_gauche;
+  }
+  if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) < 0){
+    tab_position[1] = position_basse_gauche;
+  }
+  tab_vitesses_u[0] = 2000;
+  tab_vitesses_u[1] = 2000;
+  sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+
+}
+
+void Ascenseur_gestion(void){
+
+  bool erreur;
+  int mov_g, mov_d;
+
+  switch(etat_ascenseur){
+    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;
+
+    case ASCENSEUR_ACTIF:
+      break;
+
+    case ASCENSEUR_MONTE:
+      tab_position[0] = position_haute_droit;
+      tab_position[1] = position_haute_gauche;
+      tab_vitesses_u[0] = 2000;
+      tab_vitesses_u[1] = 2000;
+      sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+      etat_ascenseur = ASCENSEUR_BUSY;
+      break;
+
+    case ASCENSEUR_DESCENT:
+      tab_position[0] = position_basse_droit;
+      tab_position[1] = position_basse_gauche;
+      tab_vitesses_u[0] = 2000;
+      tab_vitesses_u[1] = 2000;
+      sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+      etat_ascenseur = ASCENSEUR_BUSY;
+      break;
+
+    case ASCENSEUR_BUSY:
+      mov_g = sms_sts.ReadMove(ID_FEETECH_ASC_G);
+      mov_d = sms_sts.ReadMove(ID_FEETECH_ASC_D);
+      if(!mov_g && !mov_d){
+        // Fin du mouvement
+        etat_ascenseur = ASCENSEUR_ACTIF;
+
+      }
+      
+
+      break;
+
+    
+
+  }
+}