From 4f8dc2b99bfac8c69e83aab627165abfdcfa205b Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 10 May 2025 15:57:33 +0200 Subject: [PATCH] =?UTF-8?q?Actionneur:=20r=C3=A9glage=20des=20positions=20?= =?UTF-8?q?de=20la=20fourhce?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Actionneurs/Actionneurs.ino | 210 ++++++++++++++++++++++++++++++++++++ 1 file changed, 210 insertions(+) create mode 100644 Actionneurs/Actionneurs.ino diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino new file mode 100644 index 0000000..2db84e9 --- /dev/null +++ b/Actionneurs/Actionneurs.ino @@ -0,0 +1,210 @@ +#include + +#define SERVO_FOUCHE 5 +#define SERVO_PINCE_GAUCHE 8 +#define SERVO_PINCE_DROITE 9 + +#define FOURCHE_TRANSPORT 5, 307 +#define FOURCHE_LEVEE 5, 227 +#define FOURCHE_PRISE 5, 345 + +SMS_STS sms_sts; + +int ID_Feetech = 4; + +struct position_t{ + int nb_tour; + uint position; +} cible_haute, cible_basse, actuelle; + +void Moteur_set(int vitesse){ + ledcWrite(3, 0); + ledcWrite(4, 0); +} + +void setup() +{ + Serial.begin(115200);//sms舵机波特率115200 + 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; + + cible_basse.nb_tour = 2; + cible_basse.position = 1500; + + actuelle.nb_tour = 0; + + //Servo fourche (12V) + ledcAttach(5, 50, 12); + // ledcWrite(5, 307); // Neutre + ledcWrite(5, 307); // Position de transport + //ledcWrite(5, 227); // Position levée (butée haute) + //ledcWrite(5, 345); // Position de prise + + + while(1){ + ledcWrite(FOURCHE_PRISE); + delay(1000); + ledcWrite(FOURCHE_TRANSPORT); + delay(1000); + ledcWrite(FOURCHE_LEVEE); + delay(1000); + ledcWrite(FOURCHE_TRANSPORT); + delay(5000); + + } + + + // Servo pinces à aimant + ledcAttach(8, 50, 12); + ledcWrite(8, 307); + + ledcAttach(9, 50, 12); + ledcWrite(9, 307); + + // Moteur + ledcAttach(3, 2000, 8); + ledcWrite(3, 0); + ledcAttach(4, 2000, 8); + ledcWrite(4, 0); + + + if(sms_sts.FeedBack(ID_Feetech)!=-1){ + actuelle.position = sms_sts.ReadPos(-1); + }else{ + do{ + Serial.println("Erreur lecture"); + delay(1000); + }while(sms_sts.FeedBack(ID_Feetech) ==-1); + actuelle.position = sms_sts.ReadPos(-1); + } + + +} + +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() +{ + static int m_pos=0; + 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 + 8000){ + 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:"); + Serial.println(ID, DEC); + delay(100); + }else{ + Serial.println("Ping servo ID error!"); + delay(2000); + } + + int Pos; + int Speed; + int Load; + int Voltage; + int Temper; + int Move; + int Current; + if(sms_sts.FeedBack(ID)!=-1){ + Pos = sms_sts.ReadPos(-1); + Speed = sms_sts.ReadSpeed(-1); + Load = sms_sts.ReadLoad(-1); + Voltage = sms_sts.ReadVoltage(-1); + Temper = sms_sts.ReadTemper(-1); + Move = sms_sts.ReadMove(-1); + Current = sms_sts.ReadCurrent(-1); + Serial.print(">Position:"); + Serial.println(Pos); + Serial.print(">Speed:"); + Serial.println(Speed); + Serial.print(">Load:"); + Serial.println(Load); + Serial.print(">Voltage:"); + Serial.println(Voltage); + Serial.print(">Temper:"); + Serial.println(Temper); + Serial.print(">Move:"); + Serial.println(Move); + Serial.print(">Current:"); + Serial.println(Current); + delay(50); + }else{ + Serial.println("FeedBack err"); + delay(500); + } +} +