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'

Browse Source
This commit is contained in:
Samuel 2025-05-23 06:48:02 +02:00
parent ecad2cc7a2
commit cdbb340c6e
1 changed files with 38 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
Actionneurs/Ascenseur.ino
View File

@ -12,6 +12,8 @@ enum etat_ascenseur_t{
ACTIF
}etat_ascenseur=CHERCHE_BUTEE_INIT;
static int step=0;
void Ascenseur_init(void){
Ascenseur_config_servo(ID_FEETECH_ASC_D);
Ascenseur_config_servo(ID_FEETECH_ASC_G);
@ -31,9 +33,42 @@ void Ascenseur_config_servo(int id){
}
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){
u8 asc_ID[2] = {ID_FEETECH_ASC_D, ID_FEETECH_ASC_G};
static int step=0;
s16 tab_vitesses[2], tab_position[2];
u16 tab_vitesses_u[2];
u8 tab_acc[2];
@ -103,41 +138,19 @@ void Ascenseur_gestion(void){
break;
case ACTIF:
etat_ascenseur = ACTIF;
if (Serial.available() > 0) {
// get incoming byte:
int inByte = 0;
inByte = Serial.read();
if(inByte == 'd'){
step++;
Ascenseur_step_down();
}
if(inByte == 'u'){
step--;
Ascenseur_step_up();
}
while(Serial.available() > 0){
inByte = Serial.read();
}
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);
}
break;