From ecad2cc7a266c449cde62625c415b25e015c3c15 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 23 May 2025 06:43:35 +0200 Subject: [PATCH] =?UTF-8?q?Insertion=20du=20code=20de=20l'ascenseur.=20TOD?= =?UTF-8?q?O:=20V=C3=A9rifier=20que=20l'ascenseur=20s'initialise=20et=20r?= =?UTF-8?q?=C3=A9pond=20aux=20commandes=20s=C3=A9ries=20'u'=20et=20'd'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Actionneurs/Actionneurs.ino | 78 +++---------------- Actionneurs/Ascenseur.ino | 145 ++++++++++++++++++++++++++++++++++++ 2 files changed, 155 insertions(+), 68 deletions(-) create mode 100644 Actionneurs/Ascenseur.ino diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino index 4197d64..4d26182 100644 --- a/Actionneurs/Actionneurs.ino +++ b/Actionneurs/Actionneurs.ino @@ -58,8 +58,7 @@ void setup() Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000 sms_sts.pSerial = &Serial1; delay(1000); - sms_sts.WheelMode(ID_Feetech);//舵机ID1切换至电机恒速模式 - //sms_sts.WriteSpe(ID_Feetech, 2400, 100); + cible_haute.nb_tour = 0; cible_haute.position = 500; @@ -135,53 +134,15 @@ void setup() actuelle.position = sms_sts.ReadPos(-1); } + Ascenseur_init(); + + while(1){ + Ascenseur_gestion(); + } + } -void goto_position(int servo_id, position_t position){ - // Actualise la position - struct position_t precedente; - static int loin = 1; - precedente = actuelle; - if(sms_sts.FeedBack(servo_id)!=-1){ - actuelle.position = sms_sts.ReadPos(-1); - } - if(abs((int)actuelle.position - (int)precedente.position) > 2000){ - // Rebouclage compteur - if(actuelle.position < precedente.position){ - // Sens positif - actuelle.nb_tour++; - }else{ - actuelle.nb_tour--; - } - } - - // Sens de déplacement - if(actuelle.nb_tour != position.nb_tour){ - if(loin != 1){ - loin = 1; - sms_sts.WheelMode(servo_id); - if(actuelle.nb_tour < position.nb_tour){ - sms_sts.WriteSpe(servo_id, -2400, 50); - }else{ - sms_sts.WriteSpe(servo_id, 2400, 50); - } - } - }else{ - if(loin != 0){ - loin = 0; - sms_sts.ServoMode(servo_id); - sms_sts.WritePosEx(ID_Feetech, position.position, 2400, 50); - } - } - Serial.printf(">cible_pos:%d\n",position.position); - Serial.printf(">cible_nb_tour:%d\n",position.nb_tour); - Serial.printf(">act_pos:%d\n",actuelle.position); - Serial.printf(">act_nb_tour:%d\n",actuelle.nb_tour); - - - -} void loop() { @@ -189,33 +150,14 @@ void loop() static unsigned long myTime=0; static position_t cible; - /* - //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置 - sms_sts.WritePosEx(1, 4095, 2400, 50); - delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差) - - //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置 - sms_sts.WritePosEx(1, 0, 2400, 50); - delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差) - */ + Serial.print(">millis:"); Serial.println(millis()); if(millis() > myTime + 4000){ myTime = millis(); - if(m_pos){ - //sms_sts.WritePosEx(ID_Feetech, 4095, 2400, 50); - //sms_sts.WriteSpe(ID_Feetech, 2400, 50); - cible = cible_haute; - m_pos = 0; - }else{ - m_pos ++; - //sms_sts.WriteSpe(ID_Feetech, -2400, 50); - //sms_sts.WritePosEx(ID_Feetech, 0, 2400, 50); - //sms_sts.WriteSpe(ID_Feetech, -80, 100); - cible = cible_basse; - } + } - goto_position(ID_Feetech, cible); + int ID = sms_sts.Ping(ID_Feetech); if(ID!=-1){ Serial.print("Servo ID:"); diff --git a/Actionneurs/Ascenseur.ino b/Actionneurs/Ascenseur.ino new file mode 100644 index 0000000..9843a9b --- /dev/null +++ b/Actionneurs/Ascenseur.ino @@ -0,0 +1,145 @@ +#define ID_FEETECH_ASC_G 11 +#define ID_FEETECH_ASC_D 10 + +#define ASC_G_SIGNE 1 +#define ASC_D_SIGNE -1 + +enum etat_ascenseur_t{ + INIT, + CHERCHE_BUTEE_INIT, + CHERCHE_BUTEE, + EA_POS_INIT, + ACTIF +}etat_ascenseur=CHERCHE_BUTEE_INIT; + +void Ascenseur_init(void){ + Ascenseur_config_servo(ID_FEETECH_ASC_D); + Ascenseur_config_servo(ID_FEETECH_ASC_G); +} + +void Ascenseur_config_servo(int id){ + sms_sts.unLockEprom(id); // unlock EPROM-SAFE + sms_sts.writeByte(id, 30, 1); // Resolution à 1 + sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_L, 0); // Multitour + sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_H, 0); // Multitour + + sms_sts.LockEprom(id); // unlock EPROM-SAFE + Serial.printf("Servo ID : %d\n",id); + for(int i=0; i < 40; i++){ + Serial.printf("Memoire %d : %d\n", i, sms_sts.readByte(ID_Feetech, i)); + } + +} + +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]; + 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; + + switch(etat_ascenseur){ + case INIT: + // On vérifie que les deux servomoteurs répondent + do{ + int ID = sms_sts.Ping(asc_ID[0]); + erreur = sms_sts.getLastError(); + ID = sms_sts.Ping(asc_ID[1]); + erreur += sms_sts.getLastError(); + }while(erreur != 0); + 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: + etat_ascenseur = ACTIF; + if (Serial.available() > 0) { + // get incoming byte: + int inByte = 0; + inByte = Serial.read(); + if(inByte == 'd'){ + step++; + } + if(inByte == 'u'){ + step--; + } + 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; + + } +} \ No newline at end of file