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'
This commit is contained in:
parent
cdbb340c6e
commit
9cc5c64a21
@ -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
|
||||
|
@ -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;
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user