#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 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); sms_sts.SyncWriteSpe(asc_ID, 2, tab_vitesses, tab_acc); delay(200); etat_cherche_butees = CHERCHE_BUTEE; break; case CHERCHE_BUTEE: { int my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D); if(abs(my_load) > 150 ){ 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; } my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G); if(abs(my_load) > 150 ){ 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; } if(butee_d && butee_g){ etat_cherche_butees = EA_POS_INIT; } 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; 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{ int ID = sms_sts.Ping(asc_ID[0]); erreur = sms_sts.getLastError(); ID = sms_sts.Ping(asc_ID[1]); erreur += sms_sts.getLastError(); }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; } }