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 <SCServo.h>
#include "Ascenseur.h"
#define SERVO_FOUCHE 5 #define SERVO_FOUCHE 5
#define SERVO_PINCE_GAUCHE 8 #define SERVO_PINCE_GAUCHE 8
@ -24,6 +25,12 @@ struct position_t{
uint position; uint position;
} cible_haute, cible_basse, actuelle; } cible_haute, cible_basse, actuelle;
enum etat_action_t{
ACTION_EN_COURS,
ACTION_TERMINEE,
ACTION_ECHEC
};
/// @brief pilote la vitesse des moteurs /// @brief pilote la vitesse des moteurs
/// @param vitesse vitesse signée, sera saturée à 255 /// @param vitesse vitesse signée, sera saturée à 255

218
Actionneurs/Ascenseur.ino
View File

@ -5,18 +5,35 @@
#define ASC_D_SIGNE -1 #define ASC_D_SIGNE -1
enum etat_ascenseur_t{ enum etat_ascenseur_t{
INIT, ASCENSEUR_INIT,
CHERCHE_BUTEE_INIT, ASCENSEUR_ACTIF,
CHERCHE_BUTEE, ASCENSEUR_MONTE,
EA_POS_INIT, ASCENSEUR_DESCENT,
ACTIF ASCENSEUR_BUSY,
}etat_ascenseur=CHERCHE_BUTEE_INIT; }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){ 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_D);
Ascenseur_config_servo(ID_FEETECH_ASC_G); Ascenseur_config_servo(ID_FEETECH_ASC_G);
while(Ascenseur_cherche_butees() != ACTION_TERMINEE);
} }
void Ascenseur_config_servo(int id){ 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){ void Ascenseur_step_up(void){
step--; step--;
Ascenseur_update_step(); Ascenseur_update_step();
@ -43,7 +145,7 @@ void Ascenseur_step_down(void){
Ascenseur_update_step(); 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[0] = position_basse_droit + (250 + step * 500)* (ASC_D_SIGNE); // Droit
tab_position[1] = position_basse_gauche + (250 + step * 500)* (ASC_G_SIGNE); // Gauche 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_droit:%d\n", tab_position[0]);
@ -67,20 +169,12 @@ void Ascenseur_update_step(void);
} }
void Ascenseur_gestion(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; bool erreur;
static int position_basse_gauche, position_haute_gauche; int mov_g, mov_d;
static int position_basse_droit, position_haute_droit;
static bool butee_g = 0, butee_d = 0;
switch(etat_ascenseur){ switch(etat_ascenseur){
case INIT: case ASCENSEUR_INIT:
// On vérifie que les deux servomoteurs répondent // On vérifie que les deux servomoteurs répondent
do{ do{
int ID = sms_sts.Ping(asc_ID[0]); int ID = sms_sts.Ping(asc_ID[0]);
@ -88,56 +182,10 @@ void Ascenseur_gestion(void){
ID = sms_sts.Ping(asc_ID[1]); ID = sms_sts.Ping(asc_ID[1]);
erreur += sms_sts.getLastError(); erreur += sms_sts.getLastError();
}while(erreur != 0); }while(erreur != 0);
etat_ascenseur = ASCENSEUR_ACTIF;
break; break;
case CHERCHE_BUTEE_INIT: case ASCENSEUR_ACTIF:
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:
if (Serial.available() > 0) { if (Serial.available() > 0) {
// get incoming byte: // get incoming byte:
int inByte = 0; int inByte = 0;
@ -148,11 +196,49 @@ void Ascenseur_gestion(void){
if(inByte == 'u'){ if(inByte == 'u'){
Ascenseur_step_up(); Ascenseur_step_up();
} }
if(inByte == 'm'){
Ascenseur_monte();
}
if(inByte == 'l'){
Ascenseur_descend();
}
while(Serial.available() > 0){ while(Serial.available() > 0){
inByte = Serial.read(); inByte = Serial.read();
} }
} }
break; 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;
} }
} }