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