Camera bancale

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

@@ -14,6 +14,7 @@
             <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
             <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</file>
             <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp</file>
+            <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h</file>
             <file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
         </group>
     </open-files>

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

@@ -1,11 +1,11 @@
-#Fri Apr 07 11:52:43 CEST 2023
+#Tue Apr 11 12:37:03 CEST 2023
 /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/messages.cpp=c1680248859421
 /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=c1680861150700
+/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1681209407979
 /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
@@ -15,7 +15,7 @@
 /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/wrapper.c=c1679401457381
-/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680860665596
+/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1681208571544
 /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
 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp=c1679401457304

software/raspberry/superviseur-robot/tasks.cpp

@@ -104,6 +104,14 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_battery, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_mutex_create(&mutex_image, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Mutexes created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -347,9 +355,13 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             rt_mutex_release(&mutex_answer_arena);
             rt_sem_v(&sem_answerSync); 
         }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
+            cout << "Enabling search arena" << endl << flush;
             rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
             searchArena = 1; 
             rt_mutex_release(&mutex_search_arena); 
+            rt_mutex_acquire(&mutex_image, TM_INFINITE); 
+            getImage = 0; 
+            rt_mutex_release(&mutex_image); 
             cout << "Search arena true " << endl << flush;
         }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
             rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
@@ -361,6 +373,11 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             robotPos = 0; 
             rt_mutex_release(&mutex_robot_pos); 
             cout << "Robot Pos false" << endl << flush;
+        }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); 
+            cout << "Robot Pos false" << endl << flush;
         }
 
         delete(msgRcv); // must be deleted manually, no consumer
@@ -454,10 +471,16 @@ void Tasks::StartCameraTask(void *arg) {
         rt_mutex_acquire(&mutex_camera, TM_INFINITE);
         if(!cameraStarted){
             status = camera->Open(); 
-            cout << "Camera Opened" << endl << flush;
+            cout << "Camera Opened" << endl << flush; // Toggle a modifier ? 
+            rt_mutex_acquire(&mutex_image, TM_INFINITE); 
+            getImage = 1; 
+            rt_mutex_release(&mutex_image);
         }else{
             camera->Close(); 
             status = true;
+            rt_mutex_acquire(&mutex_image, TM_INFINITE); 
+            getImage = 0; 
+            rt_mutex_release(&mutex_image);
             cout << "Camera Closed" << endl << flush;
         }
         
@@ -475,6 +498,7 @@ void Tasks::StartCameraTask(void *arg) {
         cout << cameraStarted << endl << flush; 
         rt_mutex_release(&mutex_camera);
         rt_mutex_release(&mutex_cameraStarted);
+
         
         cout << "Cam success! " << endl <<flush; 
         cout << "Fin de start cam" << endl << flush;
@@ -507,7 +531,7 @@ void Tasks::MoveTask(void *arg) {
             cpMove = move;
             rt_mutex_release(&mutex_move);
             
-            cout << " move: " << cpMove;
+            cout << " move: " << cpMove << endl << flush;
             
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
             robot.Write(new Message((MessageID)cpMove));
@@ -534,30 +558,29 @@ void Tasks::BatteryTask(void *arg) {
     /**************************************************************************************/
     rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000);
 
-    /*while (1) {
+    while (1) {
         rt_task_wait_period(NULL);
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
         rs = robotStarted;
         if (rs == 1) {
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-            Message* batteryReply = robot.Write(robot.GetBattery());
+            MessageBattery* batteryReply = (MessageBattery*)robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
-            string batteryLevel = batteryReply->ToString();
             rt_mutex_release(&mutex_robot);
             WriteInQueue(&q_messageToMon, batteryReply); 
-            
         }
         rt_mutex_release(&mutex_robotStarted);
         
-    }*/
+    }
 }
 
 /**
 * @brief task in charge of managing the camera.
 */
 void Tasks::CameraTask(void *arg){
-    int cam;  
+    int cam; 
+    int grab; 
     Img* image; 
-    Arena arenaSaved ;
+    Arena arenaSaved,arena ;
     std::list<Position> robotPosition;
     MessageImg* imgToSend;
     
@@ -574,63 +597,75 @@ void Tasks::CameraTask(void *arg){
         if(!cam){
             continue;
         }
+       
+        if(getImage){
+            cout << "Traitement de l'image" << endl << flush ;
+            rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+            // Lancer le traitement périodique 
 
-        cout << "Traitement de l'image" << endl << flush ;
+            Img img = camera->Grab(); 
-        rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+            image = img.Copy();
-        // Lancer le traitement périodique 
+            rt_mutex_release(&mutex_camera);
+            rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
+            arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true 
+            if(searchArena){
+                rt_mutex_acquire(&mutex_image, TM_INFINITE); 
+                getImage = 0; 
+                rt_mutex_release(&mutex_image);
+                // chopper l'arene 
+                if(arena.IsEmpty()){
+                    cout << "Arena empty" << endl << flush;
+                    // envoi d'un no ack au moniteur 
 
-        Img img = camera->Grab(); 
+                }else{
-        image = img.Copy();
+                    cout << "Draw Arena" << endl << flush;
-        rt_mutex_release(&mutex_camera);
+                    image->DrawArena(arena);
-        rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
+                    // envoi de l'arene pour vérif
-        Arena arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true 
+                }
-        if(searchArena){
+            }else {
-            // chopper l'arene 
+                image->DrawArena(arenaSaved);
-            if(arena.IsEmpty()){
-                cout << "Arena empty" << endl << flush;
-                // envoi d'un no ack au moniteur 
-
-            }else{
-                cout << "Draw Arena" << endl << flush;
-                image->DrawArena(arena);
-                // envoi de l'arene pour vérif
             }
-        }else {
+
-            image->DrawArena(arenaSaved);
+            rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
-        }
+            if(robotPos){
+                // chopper le robot
+                cout << "Searching robot positions" << endl << flush ;
+                robotPosition =  image->SearchRobot(arena);
+
+                //Traitement du robot
+                if(!robotPosition.empty()){ // Robot position != null
+                    image->DrawAllRobots(robotPosition);
+                } 
+                for (auto position : robotPosition) 
+                {
+                    WriteInQueue(&q_messageToMon, new MessagePosition(MESSAGE_CAM_POSITION,position)); // Envoi de toutes les positions 
+                }       
+            }
+            rt_mutex_release(&mutex_robot_pos); 
+            // Envoi de l'image
+            cout << "Envoi de l'image" << endl << flush ; 
+            imgToSend = new MessageImg(MESSAGE_CAM_IMAGE,image);
+            WriteInQueue(&q_messageToMon, imgToSend);
         
-        rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+        }   
-        if(robotPos){
-            // chopper le robot
-            cout << "Pos du robot pls !" ;
-            robotPosition =  image->SearchRobot(arena);
 
-            //Traitement du robot
+        
-            if(!robotPosition.empty()){ // Robot position != null
-                image->DrawAllRobots(robotPosition);
-            } 
-            for (auto position : robotPosition) 
-            {
-                WriteInQueue(&q_messageToMon, new MessagePosition(MESSAGE_CAM_POSITION,position)); // Envoi de toutes les positions 
-            }
-                
-        }
-        rt_mutex_release(&mutex_robot_pos); 
-        // Envoi de l'image
-        cout << "Envoi de l'image" << endl << flush ; 
-        imgToSend = new MessageImg(MESSAGE_CAM_IMAGE,image);
-        WriteInQueue(&q_messageToMon, imgToSend);
-
-        rt_sem_p(&sem_answerSync, TM_INFINITE);
         rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE);
-
+        if(searchArena){
-        if(arenaConfirm){
+            rt_sem_p(&sem_answerSync, TM_INFINITE);
-            // Arena OK
+            if(arenaConfirm){
-            arenaSaved = arena; 
+                // Arena OK
+                cout << "Arena Saved" << endl << flush;
+                arenaSaved = arena; 
+            }else{
+                // Arena not OK
+                cout << "Arena Dumped" << endl << flush;
+                // Rien ? 
+            }
+            rt_mutex_acquire(&mutex_image, TM_INFINITE); 
+            getImage = 1; // changer en booleen pour synchroniser plutot que ça 
+            rt_mutex_release(&mutex_image);
             searchArena = 0;
-        }else{
-            // Arena not OK
-            // Rien ? 
         }
         rt_mutex_release(&mutex_answer_arena);
         rt_mutex_release(&mutex_search_arena); 

software/raspberry/superviseur-robot/tasks.h

@@ -70,6 +70,7 @@ private:
     int cameraStarted = 0;
     int arenaConfirm = 0; 
     int searchArena = 0; 
+    int getImage = 0;
     int move = MESSAGE_ROBOT_STOP;
     
     /**********************************************************************/
@@ -96,6 +97,9 @@ private:
     RT_MUTEX mutex_search_arena;
     RT_MUTEX mutex_answer_arena;
     RT_MUTEX mutex_robot_pos;
+    RT_MUTEX mutex_battery;
+    RT_MUTEX mutex_image;
+
 
     /**********************************************************************/
     /* Semaphores                                                         */