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;
         }
         
@@ -476,6 +499,7 @@ void Tasks::StartCameraTask(void *arg) {
         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,21 +558,19 @@ 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);
         
-    }*/
+    }
 }
 
 /**
@@ -556,8 +578,9 @@ void Tasks::BatteryTask(void *arg) {
 */
 void Tasks::CameraTask(void *arg){
     int cam; 
+    int grab; 
     Img* image; 
-    Arena arenaSaved ;
+    Arena arenaSaved,arena ;
     std::list<Position> robotPosition;
     MessageImg* imgToSend;
     
@@ -575,6 +598,7 @@ void Tasks::CameraTask(void *arg){
             continue;
         }
        
+        if(getImage){
             cout << "Traitement de l'image" << endl << flush ;
             rt_mutex_acquire(&mutex_camera, TM_INFINITE);
             // Lancer le traitement périodique 
@@ -583,8 +607,11 @@ void Tasks::CameraTask(void *arg){
             image = img.Copy();
             rt_mutex_release(&mutex_camera);
             rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
-        Arena arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true 
+            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;
@@ -602,7 +629,7 @@ void Tasks::CameraTask(void *arg){
             rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
             if(robotPos){
                 // chopper le robot
-            cout << "Pos du robot pls !" ;
+                cout << "Searching robot positions" << endl << flush ;
                 robotPosition =  image->SearchRobot(arena);
 
                 //Traitement du robot
@@ -613,7 +640,6 @@ void Tasks::CameraTask(void *arg){
                 {
                     WriteInQueue(&q_messageToMon, new MessagePosition(MESSAGE_CAM_POSITION,position)); // Envoi de toutes les positions 
                 }       
-                
             }
             rt_mutex_release(&mutex_robot_pos); 
             // Envoi de l'image
@@ -621,17 +647,26 @@ void Tasks::CameraTask(void *arg){
             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);
 
+        
+        rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE);
+        if(searchArena){
+            rt_sem_p(&sem_answerSync, TM_INFINITE);
             if(arenaConfirm){
                 // Arena OK
+                cout << "Arena Saved" << endl << flush;
                 arenaSaved = arena; 
-            searchArena = 0;
             }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;
+        }
         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                                                         */