From 9e5c6003fc53243d665b68d0978164fe0e8bc649 Mon Sep 17 00:00:00 2001
From: Raphael <benistan@etud.insa-toulouse.fr>
Date: Sun, 29 Mar 2020 15:29:55 +0200
Subject: [PATCH] Robot doesn't wait for client 2

---
 .../raspberry/superviseur-robot/tasks.cpp     | 34 +++++++++++--------
 1 file changed, 19 insertions(+), 15 deletions(-)

diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index 9702541..1422279 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -422,12 +422,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 
                 //Wait every task to die
                 sleep(1);
-                
-                //All closes
-                rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
-                    monitor.Close();
-                rt_mutex_release(&mutex_monitor);
-                
   
                 rt_mutex_acquire(&mutex_robot, TM_INFINITE);
                     status=robot.Close();
@@ -440,14 +434,13 @@ void Tasks::ReceiveFromMonTask(void *arg) {
                     cout << "Close Robot Success" << endl << flush;
                 }
 
-                //Tasks::Join();
+                //All closes
+                rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+                    monitor.Close();
+                rt_mutex_release(&mutex_monitor);
                 
                 //Release restarted tasks
-//                cout << "End sleep" << endl << flush;
                 rt_sem_broadcast(&sem_barrier);             
-//                cout << "Mes couilles" << endl << flush;
-
-                //exit(-1);
 
 
             } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
@@ -760,9 +753,12 @@ void Tasks::DetectLostSupRob(void *arg){
         //Wait the Communication with the Robot to be Set
         rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
         cout << "Start DetectLostSupRob" << endl << flush;
-
-        kill_detectLostSupRobOk = 0;
+        
+        //Initialize counter to detect loss
+        cpt = 0;
+        
         //Initialize the variable for the loop condition
+        kill_detectLostSupRobOk = 0;        
         rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
             killDetectLostSupRob = 0;
         rt_mutex_release(&mutex_killDetectLostSupRob);
@@ -784,15 +780,23 @@ void Tasks::DetectLostSupRob(void *arg){
                 cpt++;
 
                 if(cpt==3){
+                    
+                    //acknowledge loss communication with robot
+                    //WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
+                    
+                    cout << "Restart Communication with Robot" << endl << flush;
                     //Trigger Kill of DetectLostSupRob
                     rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
                         killDetectLostSupRob=1;
                     rt_mutex_release(&mutex_killDetectLostSupRob);
 
-                    //Trigger Kill of the Battery acquisition
+                    //Trigger Kill of the Battery acquisition and  therefore Robot with or without WD
                     rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
                         killBattery=1;
-                    rt_mutex_release(&mutex_killBattery);  
+                    rt_mutex_release(&mutex_killBattery);
+                    
+                    rt_sem_v(&sem_openComRobot);    
+
 
                 }
             }