From 9cc5c64a21abd466e68b935d866514f8381fe058 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 23 May 2025 07:19:18 +0200 Subject: [PATCH] =?UTF-8?q?Ascenseur,=20fonction=20step=20up=20&=20down.?= =?UTF-8?q?=20TODO:=20V=C3=A9rifier=20que=20l'ascenseur=20s'initialise=20e?= =?UTF-8?q?t=20r=C3=A9pond=20aux=20commandes=20s=C3=A9ries=20'u'=20et=20'd?= =?UTF-8?q?',=20que=20l'ascenseur=20monte=20avec=20'm'=20et=20descend=20av?= =?UTF-8?q?ec=20'l'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Actionneurs/Actionneurs.ino | 7 ++ Actionneurs/Ascenseur.ino | 220 +++++++++++++++++++++++++----------- 2 files changed, 160 insertions(+), 67 deletions(-) diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino index 4d26182..1c8a0f5 100644 --- a/Actionneurs/Actionneurs.ino +++ b/Actionneurs/Actionneurs.ino @@ -1,4 +1,5 @@ #include +#include "Ascenseur.h" #define SERVO_FOUCHE 5 #define SERVO_PINCE_GAUCHE 8 @@ -24,6 +25,12 @@ struct position_t{ uint position; } cible_haute, cible_basse, actuelle; +enum etat_action_t{ + ACTION_EN_COURS, + ACTION_TERMINEE, + ACTION_ECHEC +}; + /// @brief pilote la vitesse des moteurs /// @param vitesse vitesse signée, sera saturée à 255 diff --git a/Actionneurs/Ascenseur.ino b/Actionneurs/Ascenseur.ino index 9ecfad7..136cf6b 100644 --- a/Actionneurs/Ascenseur.ino +++ b/Actionneurs/Ascenseur.ino @@ -5,18 +5,35 @@ #define ASC_D_SIGNE -1 enum etat_ascenseur_t{ - INIT, - CHERCHE_BUTEE_INIT, - CHERCHE_BUTEE, - EA_POS_INIT, - ACTIF -}etat_ascenseur=CHERCHE_BUTEE_INIT; + 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; -static int step=0; 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){ @@ -33,6 +50,91 @@ void Ascenseur_config_servo(int id){ } +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(); @@ -43,7 +145,7 @@ void Ascenseur_step_down(void){ Ascenseur_update_step(); } -void Ascenseur_update_step(void); +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]); @@ -67,20 +169,12 @@ void Ascenseur_update_step(void); } void Ascenseur_gestion(void){ - 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]; - tab_acc[0] = 50; - tab_acc[1] = 50; + bool erreur; - static int position_basse_gauche, position_haute_gauche; - static int position_basse_droit, position_haute_droit; - static bool butee_g = 0, butee_d = 0; + int mov_g, mov_d; switch(etat_ascenseur){ - case INIT: + case ASCENSEUR_INIT: // On vérifie que les deux servomoteurs répondent do{ int ID = sms_sts.Ping(asc_ID[0]); @@ -88,56 +182,10 @@ void Ascenseur_gestion(void){ ID = sms_sts.Ping(asc_ID[1]); erreur += sms_sts.getLastError(); }while(erreur != 0); + etat_ascenseur = ASCENSEUR_ACTIF; break; - case CHERCHE_BUTEE_INIT: - 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_ascenseur = 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_ascenseur = EA_POS_INIT; - delay(500); - } - 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_ascenseur = ACTIF; - break; - - case ACTIF: + case ASCENSEUR_ACTIF: if (Serial.available() > 0) { // get incoming byte: int inByte = 0; @@ -148,11 +196,49 @@ void Ascenseur_gestion(void){ if(inByte == 'u'){ Ascenseur_step_up(); } + if(inByte == 'm'){ + Ascenseur_monte(); + } + if(inByte == 'l'){ + Ascenseur_descend(); + } while(Serial.available() > 0){ inByte = Serial.read(); } } 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; + + + } } \ No newline at end of file