Robot2025/Actionneurs/Ascenseur.ino

#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_DEPOSE,
  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) > 200 ){
        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) > 200 ){
        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 - (9400 * ASC_D_SIGNE);
      position_haute_gauche = position_basse_gauche - (9400 * 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){
    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_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){
    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_DESCENT;
    return 0;
  }
  return 1;
}
/// @brief Commande l'ascenseur en position de dépose
/// @return 1 si l'ascenseur n'est pas prêt
int Ascenseur_depose(void){
  if(etat_ascenseur == ASCENSEUR_ACTIF){
    tab_position[0] = position_haute_droit*0.8 + position_basse_droit * 0.2;
    tab_position[1] = position_haute_gauche * 0.8 + position_basse_gauche * 0.2;
    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_DEPOSE;
    return 0;
  }
  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();
}

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_cycle(){
  while(!Serial.available()){
    Serial.println("Ascenseur_monte");
    Ascenseur_monte();
    while(Ascenseur_get_etat() != ACTION_TERMINEE){
      Ascenseur_gestion();
    }
    Serial.println("Ascenseur_descend");
    Ascenseur_descend();
    while(Ascenseur_get_etat() != ACTION_TERMINEE){
      Ascenseur_gestion();
    }
  }
  while(Serial.available() > 0){
    Serial.read();
  }
}

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:
      delay(200);
      etat_ascenseur = ASCENSEUR_BUSY;
      break;

    case ASCENSEUR_DESCENT:
      delay(200);
      etat_ascenseur = ASCENSEUR_BUSY;
      break;

    case ASCENSEUR_DEPOSE:
      delay(200);
      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;

    

  }
}