Insertion du code de l'ascenseur. TODO: Vérifier que l'ascenseur s'initialise et répond aux commandes séries 'u' et 'd'

This commit is contained in:
Samuel 2025-05-23 06:43:35 +02:00
parent 18ff92e0e6
commit ecad2cc7a2
2 changed files with 155 additions and 68 deletions

View File

@ -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:");

145
Actionneurs/Ascenseur.ino Normal file
View File

@ -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;
}
}