From 289898a76344ede362fd428a273de1cd524327d3 Mon Sep 17 00:00:00 2001
From: Geoffrey Frogeye <geoffrey@frogeye.fr>
Date: Wed, 9 May 2018 01:00:16 +0200
Subject: [PATCH] Attente fin de consigne

---
 chef/src/dimensions.h |  2 ++
 chef/src/movement.c   | 40 ++++++++++++++++++++++++++++++++++++----
 2 files changed, 38 insertions(+), 4 deletions(-)

diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h
index b3fcfbb..4ff3a82 100644
--- a/chef/src/dimensions.h
+++ b/chef/src/dimensions.h
@@ -37,6 +37,8 @@
 #define D_DIR_ECART_MAX 5.0 // mm
 #define O_DIR_ECART_MIN (6.0 / 360.0 * 2.0 * M_PI) // rad
 #define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad
+#define D_CONS_THRESOLD 1.0 // mm
+#define O_CONS_THRESOLD (6.0 / 360.0 * 2.0 * M_PI) // rad
 #define O_GAIN 3.0
 #define P 3.0
 #define I 0.0
diff --git a/chef/src/movement.c b/chef/src/movement.c
index 1c8c09f..26037f0 100644
--- a/chef/src/movement.c
+++ b/chef/src/movement.c
@@ -1,7 +1,7 @@
 #include <math.h>
 #include <pthread.h>
-#include <stdlib.h>
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
 
@@ -15,6 +15,9 @@ pthread_mutex_t movCons;
 pthread_mutex_t movEnableMutex;
 pthread_cond_t movEnableCond;
 bool movEnableBool;
+pthread_mutex_t movDoneMutex;
+pthread_cond_t movDoneCond;
+bool movDoneBool;
 
 float xDiff;
 float yDiff;
@@ -47,6 +50,9 @@ void configureMovement()
     pthread_mutex_init(&movEnableMutex, NULL);
     pthread_cond_init(&movEnableCond, NULL);
     movEnableBool = false;
+    pthread_mutex_init(&movDoneMutex, NULL);
+    pthread_cond_init(&movDoneCond, NULL);
+    movDoneBool = false;
 
     pthread_create(&tMovement, NULL, TaskMovement, NULL);
 
@@ -78,6 +84,25 @@ void setDestination(struct position* pos)
     pthread_mutex_unlock(&movCons);
 }
 
+void waitDestination()
+{
+    pthread_mutex_lock(&movDoneMutex);
+    while (!movDoneBool) {
+        pthread_cond_wait(&movDoneCond, &movDoneMutex);
+    }
+    pthread_mutex_unlock(&movDoneMutex);
+}
+
+void sendTo(float x, float y, float o)
+{
+    enableConsigne();
+    struct position pos = {x, y, o};
+    setDestination(&pos);
+    waitDestination();
+    brake();
+    disableConsigne();
+}
+
 float angleGap(float target, float actual)
 {
     return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
@@ -135,7 +160,7 @@ void* TaskMovement(void* pData)
         // Si on est loin de la consigne, l'angle cible devient celui orienté vers la consigne
         if (dDirEcart > D_DIR_ECART_MAX) {
             oRetenu = true;
-        // Si on est proche de la consigne, l'angle cible devient celui voulu par la consigne
+            // Si on est proche de la consigne, l'angle cible devient celui voulu par la consigne
         } else if (dDirEcart < D_DIR_ECART_MIN) {
             oRetenu = false;
         }
@@ -146,13 +171,20 @@ void* TaskMovement(void* pData)
         // pour se réorienter vers l'angle de la consigne
         if (oDirEcartAbs > O_DIR_ECART_MAX) {
             dRetenu = true;
-        // Si l'écart avec l'angle orienté vers la consigne est petit, la distance cible devient
-        // la distance entre la position actuelle et la consigne
+            // Si l'écart avec l'angle orienté vers la consigne est petit, la distance cible devient
+            // la distance entre la position actuelle et la consigne
         } else if (oDirEcartAbs < O_DIR_ECART_MIN) {
             dRetenu = false;
         }
         dErr = dRetenu ? 0 : dDirEcart;
 
+        // Calcul si on est arrivé
+        pthread_mutex_lock(&movDoneMutex);
+        clock_gettime(CLOCK_REALTIME, &pidNow);
+        movDoneBool = !dRetenu && !oRetenu && dDirEcart < D_CONS_THRESOLD && oEcart < O_CONS_THRESOLD;
+        pthread_cond_signal(&movDoneCond);
+        pthread_mutex_unlock(&movDoneMutex);
+
         // Ordre → Volt
         // Nombre de rotation nécessaire sur les deux roues dans le même sens pour arriver à la distance voulue
         float dErrRev = dErr / WHEEL_PERIMETER;