Camera ouverte traitement en cours

software/raspberry/superviseur-robot/lib/camera.h

@@ -52,7 +52,7 @@ public:
 
     /**
      * Open camera
-     * @return True if camera is open, false otherwise
+     * @return True if camera is open, fallocalhostse otherwise
      */
     bool Open();
     
@@ -62,7 +62,7 @@ public:
     void Close();
     
     /**
-     * Get width of sampled image
+     * Get width of sampled imagelocalhost
      * @return Width of sampled picture
      */
     int GetWidth() const;

software/raspberry/superviseur-robot/nbproject/private/configurations.xml

@@ -41,7 +41,7 @@
     </conf>
     <conf name="Debug__RPI_" type="1">
       <toolsSet>
-        <developmentServer>xenomai@10.105.1.5:22</developmentServer>
+        <developmentServer>xenomai@10.105.1.10:22</developmentServer>
         <platform>2</platform>
       </toolsSet>
       <dbx_gdbdebugger version="1">

software/raspberry/superviseur-robot/nbproject/private/private.xml

@@ -6,6 +6,11 @@
     </data>
     <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
     <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
-        <group/>
+        <group>
+            <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
+            <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h</file>
+            <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
+            <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp</file>
+        </group>
     </open-files>
 </project-private>

software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.10-xenomai-22

@@ -1,20 +1,20 @@
-#Tue Mar 21 15:11:16 CET 2023
+#Fri Mar 31 12:07:07 CEST 2023
-/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1679401432892
+/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1679401432930
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1679401432864
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/gdbsudo.sh=c1679401432813
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1679401432832
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1679401457314
-/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1679407831774
+/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680257220969
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.dep.inc=c1679401432701
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1679401432922
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp=c1679401457320
-/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1679401457309
+/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1680256228274
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/README.md=c1679401432714
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1679401457337
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/Makefile=c1679401432709
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1679401432820
-/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1679406345632
+/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680253954166
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/wrapper.c=c1679401457381
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp=c1679404095224
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.properties=c1679401432983

software/raspberry/superviseur-robot/tasks.cpp

@@ -27,6 +27,7 @@
 #define PRIORITY_TSTARTROBOT 20
 #define PRIORITY_TCAMERA 21
 #define PRIORITY_TBATTERY 25
+#define PRIORITY_TSTARTCAMERA 25
 
 /*
  * Some remarks:
@@ -74,6 +75,22 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_cameraStarted, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_mutex_create(&mutex_robot_pos, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_mutex_create(&mutex_search_arena, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_mutex_create(&mutex_camera, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Mutexes created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -95,6 +112,10 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_sem_create(&sem_startCamera, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Semaphores created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -120,6 +141,10 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_create(&th_startCamera, "th_startCamera", 0, PRIORITY_TSTARTCAMERA, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -128,6 +153,10 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -176,7 +205,15 @@ void Tasks::Run() {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-
+    if (err = rt_task_start(&th_camera, (void(*)(void*)) & Tasks::CameraTask, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_task_start(&th_startCamera, (void(*)(void*)) & Tasks::StartCameraTask, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
     cout << "Tasks launched" << endl << flush;
 }
 
@@ -285,6 +322,18 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             rt_mutex_release(&mutex_move);
         } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
             rt_sem_v(&sem_startCamera);
+        }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
+            rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
+            searchArena = 1; 
+            rt_mutex_release(&mutex_search_arena); 
+        }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
+            rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+            robotPos = 1; 
+            rt_mutex_release(&mutex_robot_pos); 
+        }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
+            rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+            robotPos = 0; 
+            rt_mutex_release(&mutex_robot_pos); 
         }
         delete(msgRcv); // must be deleted manually, no consumer
     }
@@ -370,25 +419,34 @@ void Tasks::StartCameraTask(void *arg) {
     while (1) {
 
         Message * msgSend;
+        Message * msg;
         rt_sem_p(&sem_startCamera, TM_INFINITE);
         cout << "Start Camera ";
         rt_mutex_acquire(&mutex_camera, TM_INFINITE);
-        if(!camera.Open()){
+        bool status = camera.Open(); 
-            msgSend = new Message(MESSAGE_ANSWER_NACK)
+        if(!status){
+            msgSend = new Message(MESSAGE_ANSWER_NACK);
             WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
-            cout << msgSend->GetID << endl << flush; 
+            cout << "NACK !" << endl << flush ;
+            cout << msgSend->ToString() << endl << flush; 
+            cout << "NACK1 ! " << endl << flush ;
             rt_mutex_release(&mutex_camera);
+            cout << "NACK2 ! " << endl << flush ;
             continue;
         }
-        msg = new Message(MESSAGE_ANSWER_ACK); 
+        msg = new Message(MESSAGE_ANSWER_ACK);
-        cout << msgSend->GetID << endl << flush;
+        cout << "ACK3 ! " << endl << flush ;
+        cout << msg->ToString() << endl << flush;
+        cout << "ACK4 ! " << endl << flush ;
         rt_mutex_release(&mutex_camera);
-        
+        cout << "ACK ! " << endl << flush ;
-        if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
+        rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
-            rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
+        cout << "ff" << endl << flush ;
-            cameraStarted = 1;
+        cameraStarted = 1;
-            rt_mutex_release(&mutex_cameraStarted);
+        cout << "Cam success! " << endl <<flush; 
-        }
+        rt_mutex_release(&mutex_cameraStarted);
+  
+        cout << "Fin de start cam" << endl << flush;
     }
 }
 
@@ -403,14 +461,8 @@ void Tasks::MoveTask(void *arg) {
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
     
-    /**************************************************************************************/
+    /*********************************************************/
-    /* The task starts here                                                               */
+        //cout << "Periodic movement update";
-    /**************************************************************************************/
-    rt_task_set_periodic(NULL, TM_NOW, 100000000);
-
-    while (1) {
-        rt_task_wait_period(NULL);
-        cout << "Periodic movement update";
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
         rs = robotStarted;
         if (rs == 1) {
@@ -425,9 +477,8 @@ void Tasks::MoveTask(void *arg) {
             rt_mutex_release(&mutex_robot);
         }
         rt_mutex_release(&mutex_robotStarted);
-        cout << endl << flush;
+        //cout << endl << flush;
     }
-}
 
 /**
  * @brief task in charge of the battery status.
@@ -462,10 +513,12 @@ void Tasks::BatteryTask(void *arg) {
     }
 }
 
+
 /**
 * @brief task in charge of managing the camera.
 */
-void CameraTask(void *arg){
+void Tasks::CameraTask(void *arg){
+    
     int cam; 
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
@@ -473,19 +526,46 @@ void CameraTask(void *arg){
     rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000);
     while(1){
         rt_task_wait_period(NULL);
-        rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+        rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
         cam = cameraStarted; 
+        rt_mutex_release(&mutex_cameraStarted);
+        if(!cam){
         
-        if(cam){
+            continue;
-            // Lancer le traitement périodique 
-            rt_mutex_release(&mutex_camera);
-        }else{
-            rt_mutex_release(&mutex_camera);
-            
-            rt_mutex_acquire(&mutex, TM_INFINITE);
-            rt_mutex_release(&mutex_camera);
         }
 
+        rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+        // Lancer le traitement périodique 
+        Img img = camera.Grab(); 
+        Arena arena; 
+        rt_mutex_release(&mutex_camera);
+        rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
+        if(searchArena){
+            // chopper l'arene 
+            cout << "Search arena pls ! " << endl << flush;
+            arena = img.SearchArena();
+            // searchArena = 0; // toggle? 
+        }
+        rt_mutex_release(&mutex_search_arena); 
+
+        rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+        if(robotPos){
+            // chopper le robot
+            cout << "Pos du robot pls !" ;
+            
+        }
+        rt_mutex_release(&mutex_robot_pos); 
+        if(arena.IsEmpty()){
+                cout << "Arena empty" << endl << flush;
+                // envoi d'un no ack au moniteur 
+            }else{
+                cout << "Draw Arena" << endl << flush;
+                img.DrawArena(arena);
+
+                // envoi de l'arene pour vérif
+        }
+        cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
+        WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); 
         
     }
 }

software/raspberry/superviseur-robot/tasks.h

@@ -67,6 +67,8 @@ private:
     Camera camera;
     int robotStarted = 0;
     int cameraStarted = 0;
+    int searchArena = 0;
+    int robotPos = 0; 
     int move = MESSAGE_ROBOT_STOP;
     
     /**********************************************************************/
@@ -90,6 +92,8 @@ private:
     RT_MUTEX mutex_cameraStarted;
     RT_MUTEX mutex_move;
     RT_MUTEX mutex_camera;
+    RT_MUTEX mutex_search_arena;
+    RT_MUTEX mutex_robot_pos; 
 
     /**********************************************************************/
     /* Semaphores                                                         */