Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ascenseur, fonction step up & down. TODO: Vérifier que l'ascenseur s'initialise et répond aux commandes séries 'u' et 'd', que l'ascenseur monte avec 'm' et descend avec 'l'

Browse Source
This commit is contained in:
Samuel 2025-05-23 07:19:18 +02:00
parent cdbb340c6e
commit 9cc5c64a21
2 changed files with 160 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Actionneurs/Actionneurs.ino
View File

@ -1,4 +1,5 @@
#include <SCServo.h>
#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

220
Actionneurs/Ascenseur.ino
View File

@ -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;
}
}