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 <SCServo.h>
+
+#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);
+ }
+}
+