From ff949302d2406ebf21d9d063e89cb01c18288669 Mon Sep 17 00:00:00 2001
From: bap <baptiste.taillandier@isen-ouest.yncrea.fr>
Date: Sat, 20 Apr 2024 14:52:53 +0200
Subject: [PATCH] Ajout angle middle pinces

---
 MyTCPClient.cpp | 113 ++++++++++++++++++++++++++++++++++--------------
 MyTCPClient.h   |  29 +++++++++----
 2 files changed, 102 insertions(+), 40 deletions(-)

diff --git a/MyTCPClient.cpp b/MyTCPClient.cpp
index 18add6d..511c13d 100644
--- a/MyTCPClient.cpp
+++ b/MyTCPClient.cpp
@@ -161,63 +161,112 @@ void MyTCPClient::lever_bras() {
 }
 
 void MyTCPClient::fermer_pince(int pince, bool force) {
-    if (!pinceOuverte[pince] && !force){
-        return;
-    }
-    int angle, old_angle;
-    if (pince < 0 || pince > 2){
+    anglePince pinceChoisie;
+    int angleDebut;
+    if ((etatPince[pince] == PINCE_FERMER && !force) || pince < 0 || pince > 2){
         return;
     }
     switch(pince){
         case 0:
-            angle = 142;
-                old_angle = 115;
-        break;
+            pinceChoisie = anglePince0;
+            break;
         case 1:
-            angle = 42;
-                old_angle = 22;
-        break;
+            pinceChoisie = anglePince1;
+            break;
         case 2:
-            angle = 152;
-            old_angle = 130;
-        break;
+            pinceChoisie = anglePince2;
+            break;
+    }
+    switch (etatPince[pince]) {
+        case PINCE_FERMER:
+            this->pwm_setServoPosition(pince, pinceChoisie.fermer)
+            return;
+        case PINCE_MIDDLE:
+            angleDebut = pinceChoisie.middle;
+            break;
+        case PINCE_OUVERTE:
+            angleDebut = pinceChoisie.ouverte;
+            break;
     }
     std::cout << "Fermer pince : " << pince << std::endl;
-    for(int i = old_angle; i <= angle;i++){
+    for(int i = angleDebut; i <= pinceChoisie.fermer;i++){
         this->pwm_setServoPosition(pince, i);
         usleep(5'000);
     }
-    pinceOuverte[pince] = false;
+    etatPince[pince] = PINCE_FERMER;
+}
+
+void MyTCPClient::middle_pince(int pince, bool force){
+    anglePince pinceChoisie;
+    if ((etatPince[pince] == PINCE_MIDDLE && !force) || pince < 0 || pince > 2){
+        return;
+    }
+    switch(pince){
+        case 0:
+            pinceChoisie = anglePince0;
+            break;
+        case 1:
+            pinceChoisie = anglePince1;
+            break;
+        case 2:
+            pinceChoisie = anglePince2;
+            break;
+    }
+    std::cout << "Middle pince : " << pince << std::endl;
+    switch (etatPince[pince]) {
+        case PINCE_OUVERTE:
+            for(int i = pinceChoisie.ouverte; i <= pinceChoisie.middle;i++){
+                this->pwm_setServoPosition(pince, i);
+                usleep(5'000);
+            }
+            break;
+        case PINCE_MIDDLE:
+            this->pwm_setServoPosition(pince, pinceChoisie.middle);
+            return;
+        case PINCE_FERMER:
+            for (int i = pinceChoisie.fermer; i >= pinceChoisie.middle;i--){
+                this->pwm_setServoPosition(pince, i);
+                usleep(5'000);
+            }
+            break;
+    }
+    etatPince[pince] = PINCE_OUVERTE;
 }
 
 void MyTCPClient::ouvrir_pince(int pince, bool force) {
-    if (pinceOuverte[pince] && !force){
-        return;
-    }
-    int angle, old_angle;
-    if (pince < 0 || pince > 2){
+    anglePince pinceChoisie;
+    int angleDebut;
+    if ((etatPince[pince] == PINCE_OUVERTE && !force) || pince < 0 || pince > 2){
         return;
     }
     switch(pince){
         case 0:
-            angle = 115;
-                old_angle = 142;
-        break;
+            pinceChoisie = anglePince0;
+            break;
         case 1:
-            angle = 22;
-                old_angle = 42;
-        break;
+            pinceChoisie = anglePince1;
+            break;
         case 2:
-            angle = 130;
-                old_angle = 152;
-        break;
+            pinceChoisie = anglePince2;
+            break;
+    }
+    switch (etatPince[pince]) {
+        case PINCE_FERMER:
+            angleDebut = pinceChoisie.fermer;
+            break;
+        case PINCE_MIDDLE:
+            angleDebut = pinceChoisie.middle;
+            break;
+        case PINCE_OUVERTE:
+            this->pwm_setServoPosition(pince, pinceChoisie.ouverte)
+            return;
     }
     std::cout << "Ouvrir pince : " << pince << std::endl;
-    for (int i = old_angle; i >= angle;i--){
+    for (int i = angleDebut; i >= pinceChoisie.ouverte;i--){
         this->pwm_setServoPosition(pince, i);
         usleep(5'000);
     }
-    pinceOuverte[pince] = true;
+    etatPince[pince] = PINCE_OUVERTE;
 }
 
 
diff --git a/MyTCPClient.h b/MyTCPClient.h
index bfc693f..06c21fd 100644
--- a/MyTCPClient.h
+++ b/MyTCPClient.h
@@ -19,11 +19,23 @@ enum BrasState {
     BRAS_TRANSPORT
 };
 
+enum PinceState {
+    PINCE_OUVERTE,
+    PINCE_MIDDLE,
+    PINCE_FERMER
+};
+
 typedef struct {
     int servo4;
     int servo5;
 }angleBras;
 
+typedef struct {
+    int fermer;
+    int middle;
+    int ouverte;
+}anglePince;
+
 class MyTCPClient : public TCPClient {
 public:
     explicit MyTCPClient(const char* serverIP = "127.0.0.1", int port = 8080);
@@ -36,14 +48,10 @@ public:
 
     void pwm_setServoPosition(int servo, int position);
 
- //   void baisser_bras(bool force = false);
-    
     void baisser_bras();
 
     void transport_bras();
 
-//    void lever_bras(bool force = false);
-    
     void lever_bras();
 
     void fermer_pince(int pince, bool force = false);
@@ -59,8 +67,13 @@ public:
 private:
     PiPCA9685::PCA9685 pca;
     BrasState positionBras = BRAS_BAS;
-    bool pinceOuverte[3] = {true, true, true};
-    angleBras angleBrasBas = {180, 0};
-    angleBras angleBrasTransport = {168, 12};
-    angleBras angleBrasHaut = {53, 127};//78, 102
+    int etatPince[3] = {PINCE_OUVERTE, PINCE_OUVERTE, PINCE_OUVERTE};
+
+    const angleBras angleBrasBas = {180, 0};
+    const angleBras angleBrasTransport = {168, 12};
+    const angleBras angleBrasHaut = {53, 127};//78, 102
+
+    const anglePince anglePince0 = {142, 129, 115};//Ecart -27
+    const anglePince anglePince1 = {42, 32, 22};//Ecart -20
+    const anglePince anglePince2 = {152, 141, 130};//Ecart -22
 };