diff --git a/Actionneurs/Ascenseur.ino b/Actionneurs/Ascenseur.ino index 9843a9b..9ecfad7 100644 --- a/Actionneurs/Ascenseur.ino +++ b/Actionneurs/Ascenseur.ino @@ -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;