From be868ed6f49ad0c180e2b664355dcf166d8f4192 Mon Sep 17 00:00:00 2001
From: vitrat <vitrat@etud.insa-toulouse.fr>
Date: Sun, 29 Mar 2020 11:19:22 +0200
Subject: [PATCH] vision funciton not fully working but almost

---
 .../raspberry/superviseur-robot/tasks.cpp     | 63 +++++++++++++------
 software/raspberry/superviseur-robot/tasks.h  |  1 +
 2 files changed, 44 insertions(+), 20 deletions(-)

diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index 4c8b238..770ed3c 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -154,7 +154,10 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }   
-    
+    if (err = rt_sem_create(&sem_allowStartCapture, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }  
     
     //Initialization for some specific sem: Allow to run the first time
     rt_sem_v(&sem_allowStartReceive);
@@ -668,76 +671,96 @@ void Tasks::Vision(void *arg){
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
-    
-    rt_sem_p(&sem_askBattery, TM_INFINITE);
     int killVisionOk=0;
     Camera camera;
-    int acquireImageOk=1;
+    int acquireImageOk=0;
     int searchArenaOk=0;
     int drawArenaOk=0;
     int getPositionOk=0;
     Arena arena;
+    //Img* img;
     list<Position> positionList;
     list<Position>::iterator it;
     string strPos;
-    MessagePosition* msgPos;
+    //MessagePosition* msgPos;
     Message* msgPosMsg;
-    MessageImg* msgImg;
+    MessageImg msgImg;
     Message* msgImgMsg;
-    msgPos->SetID(MESSAGE_CAM_POSITION);
-    msgImg->SetID(MESSAGE_CAM_IMAGE);
+    msgImg.SetID(MESSAGE_CAM_IMAGE);
+    //msgPos->SetID(MESSAGE_CAM_POSITION);
     rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
     killVision=0;
     rt_mutex_release(&mutex_killVision);
-    camera.Open();
+    rt_task_set_periodic(NULL, TM_NOW, 100000000);
+    camera.Open();  
     while(killVisionOk==0){
+        rt_task_wait_period(NULL);
         while(acquireImageOk==1){
+            rt_task_wait_period(NULL);
+            cout << "open cam" << endl;
             Img img=camera.Grab();
+            cout << "after camera grab" << endl << endl << endl;
+            msgImg.SetImage(&img);
+            cout << "after set image in msgImg" << endl << endl << endl;
             if(searchArenaOk==1){
                 rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
                 acquireImage=0;
                 rt_mutex_release(&mutex_acquireImage);
-                acquireImageOk=acquireImage;
                 arena=img.SearchArena();
                 img.DrawArena(arena);
             }
             if(drawArenaOk==1){
-              //  img.DrawArena(arena);
+                img.DrawArena(arena);
                 rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
                 acquireImage=1;
                 rt_mutex_release(&mutex_acquireImage);
-                acquireImageOk=acquireImage;
             }
             if(getPositionOk==1){
+                cout << "in GetPos=1" << endl << endl << endl;
                 //On démarre la recherche du robot dans la zone définie par l'arène
                 positionList=img.SearchRobot(arena);
                 //Définitition et assignation de l'itérateur de parcrous de la liste positionList
                 it=positionList.begin();
                 //Définition d'un messagePosition qui va contenir l'information (x,y)
-                msgPos->SetPosition(*it);
+                //msgPos->SetPosition(*it);
+                MessagePosition msgPos(MESSAGE_CAM_POSITION,*it);
                 //Transformation en message classique
-                msgPosMsg=msgPos->Copy();
+                msgPosMsg=msgPos.Copy();
                 //Envoi
-                WriteInQueue(&q_messageToMon,msgPos);
+                cout << "envoi au mon" << endl;
+                WriteInQueue(&q_messageToMon,&msgPos);
                 //Dessis du robot sur l'image
                 img.DrawRobot(*it);
             }
+            
             //Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas)
-            msgImg->SetImage(&img);
+            msgImg.SetImage(&img);
             //Transformation en message classique
-            msgImgMsg=msgImg->Copy();
+            msgImgMsg=msgImg.Copy();
             //Envoi
-            WriteInQueue(&q_messageToMon,msgImg);
+            WriteInQueue(&q_messageToMon,&msgImg);
         }
+        rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+            acquireImageOk=acquireImage;
+        rt_mutex_release(&mutex_acquireImage);
+        rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+            getPositionOk=getPosition;
+        rt_mutex_release(&mutex_getPosition);
+        rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+            searchArenaOk=searchArena;
+        rt_mutex_release(&mutex_searchArena);
+        rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
+            drawArenaOk=drawArena;
+        rt_mutex_release(&mutex_drawArena);
     }
     rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
         searchArena=0;
     rt_mutex_release(&mutex_searchArena);
-    searchArenaOk=1;
+    searchArenaOk=0;
     rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
         getPosition=0;
     rt_mutex_release(&mutex_getPosition);
-    getPositionOk=1;
+    getPositionOk=0;
 }
 
 
diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h
index 857006c..c01e5c2 100644
--- a/software/raspberry/superviseur-robot/tasks.h
+++ b/software/raspberry/superviseur-robot/tasks.h
@@ -117,6 +117,7 @@ private:
     RT_SEM sem_startRobotWithWatchdog;
     RT_SEM sem_allowStartReceive;
     RT_SEM sem_askBattery;
+    RT_SEM sem_allowStartCapture;
     
     /**********************************************************************/
     /* Message queues                                                     */