From 454a15e709bda4f94a771a03fbf821a439ad8e78 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Tue, 13 May 2025 22:30:22 +0200
Subject: [PATCH] =?UTF-8?q?R=C3=A9glage=20des=20but=C3=A9es=20de=20l'ascen?=
 =?UTF-8?q?sseur?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Actionneurs/Actionneurs.ino | 41 ++++++++++++++++++++++++++++++++-----
 1 file changed, 36 insertions(+), 5 deletions(-)

diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino
index 2db84e9..3053210 100644
--- a/Actionneurs/Actionneurs.ino
+++ b/Actionneurs/Actionneurs.ino
@@ -8,6 +8,13 @@
 #define FOURCHE_LEVEE     5, 227
 #define FOURCHE_PRISE     5, 345
 
+#define DOIGT_PINCE_GAUCHE_FERME 2080
+#define DOIGT_PINCE_GAUCHE_OUVRE 2367
+#define DOIGT_PINCE_GAUCHE_PARKING 1550
+
+#define ID_FEETECH_ASC_G 11
+#define ID_FEETECH_ASC_D 10
+
 SMS_STS sms_sts;
 
 int ID_Feetech = 4;
@@ -45,7 +52,7 @@ void setup()
   //ledcWrite(5, 227); // Position levée (butée haute)
   //ledcWrite(5, 345); // Position de prise
 
-
+/*
   while(1){
     ledcWrite(FOURCHE_PRISE);
     delay(1000);
@@ -56,7 +63,7 @@ void setup()
     ledcWrite(FOURCHE_TRANSPORT);
     delay(5000);
 
-  }
+  }*/
 
 
   // Servo pinces à aimant
@@ -72,6 +79,28 @@ void setup()
   ledcAttach(4, 2000, 8);
   ledcWrite(4, 0);
 
+  /*while(1){
+
+    ledcWrite(FOURCHE_PRISE);
+    delay(1000);
+    ledcWrite(FOURCHE_TRANSPORT);
+    delay(1000);
+    ledcWrite(FOURCHE_LEVEE);
+    delay(1000);
+    ledcWrite(FOURCHE_TRANSPORT);
+    delay(1000);
+
+    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_PARKING, 2400, 50);
+    delay(1000);
+    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_OUVRE, 2400, 50);
+    delay(3000);
+    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_FERME, 2400, 50);
+    delay(5000);
+    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_OUVRE, 2400, 50);
+    delay(5000);
+
+  }*/
+
 
   if(sms_sts.FeedBack(ID_Feetech)!=-1){
     actuelle.position = sms_sts.ReadPos(-1);
@@ -110,9 +139,9 @@ void goto_position(int servo_id, position_t position){
       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{
+        sms_sts.WriteSpe(servo_id, 2400, 50);
       }
     }
   }else{
@@ -127,6 +156,8 @@ void goto_position(int servo_id, position_t position){
   Serial.printf(">act_pos:%d\n",actuelle.position);
   Serial.printf(">act_nb_tour:%d\n",actuelle.nb_tour);
 
+
+
 }
 
 void loop()
@@ -146,7 +177,7 @@ void loop()
   */
   Serial.print(">millis:");
   Serial.println(millis());
-  if(millis() > myTime + 8000){
+  if(millis() > myTime + 4000){
     myTime = millis();
     if(m_pos){
       //sms_sts.WritePosEx(ID_Feetech, 4095, 2400, 50);