diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.4-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.4-xenomai-22 index 1851cb5..7f2f1b9 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.4-xenomai-22 +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.4-xenomai-22 @@ -1,4 +1,4 @@ -#Thu Apr 13 16:52:14 CEST 2023 +#Thu Apr 13 17:09:29 CEST 2023 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/README.md=c1681393207260 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1681393207255 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1681393207258 @@ -22,7 +22,7 @@ /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.dep.inc=c1681393207120 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1681393207321 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1681393207272 -/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1681397524403 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1681398565600 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.cpp=c1681393226950 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1681393226960 /home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/superviseur.doxygen=c1681393207355 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 5b07e81..9581a51 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -666,45 +666,55 @@ void Tasks::StartCameraTask(void *arg) { /**************************************************************************************/ while (1) { - Message * msgSend; Message * msg; bool status; rt_sem_p(&sem_startCamera, TM_INFINITE); - cout << "Start Camera "; + cout << "Camera changed state " << endl << flush; + rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); rt_mutex_acquire(&mutex_camera, TM_INFINITE); if(!cameraStarted){ + status = camera->Open(); - cout << "Camera Opened" << endl << flush; // Toggle a modifier ? + cout << "Camera Opened" << endl << flush; + rt_mutex_acquire(&mutex_image, TM_INFINITE); getImage = 1; rt_mutex_release(&mutex_image); + }else{ + camera->Close(); - status = true; + status = true; // camera->Close() does not return a status + cout << "Camera Closed" << endl << flush; + + rt_mutex_acquire(&mutex_image, TM_INFINITE); getImage = 0; rt_mutex_release(&mutex_image); - cout << "Camera Closed" << endl << flush; + + } - - if(!status){ - msgSend = new Message(MESSAGE_ANSWER_NACK); - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon - cout << msgSend->ToString() << endl << flush; - rt_mutex_release(&mutex_camera); - rt_mutex_release(&mutex_cameraStarted); - continue; - } - msg = new Message(MESSAGE_ANSWER_ACK); - cout << msg->ToString() << endl << flush; - cameraStarted = !cameraStarted; - cout << cameraStarted << endl << flush; rt_mutex_release(&mutex_camera); + + if(!status){ + + msg = new Message(MESSAGE_ANSWER_NACK); + cout << msg->ToString() << endl << flush; + cout << "Camera failed to start" << endl << flush; + + }else{ + + msg = new Message(MESSAGE_ANSWER_ACK); + cout << msg->ToString() << endl << flush; + cameraStarted = !cameraStarted; + cout << "Camera started successfully !" << endl <ToString(); rt_mutex_release(&mutex_robot); WriteInQueue(&q_messageToMon, batteryReply); - } - - + } } } @@ -785,7 +794,6 @@ void Tasks::BatteryTask(void *arg) { */ void Tasks::CameraTask(void *arg){ int cam; - int grab; Img* image; // Image qui sera envoyée au moniteur Arena arenaSaved,arena ; // arenaSaved : Arène sauvegardée par le moniteur, arena: Arène temporaire pour le traitement std::list robotPosition; // Liste des positions de robot à envoyer @@ -807,10 +815,9 @@ void Tasks::CameraTask(void *arg){ } rt_mutex_acquire(&mutex_image, TM_INFINITE); if(getImage){ - cout << "Statut du traitement " << getImage << endl; + // Lancer le traitement périodique //cout << "Traitement de l'image" << endl << flush ; rt_mutex_acquire(&mutex_camera, TM_INFINITE); - // Lancer le traitement périodique Img img = camera->Grab(); image = img.Copy(); @@ -821,8 +828,6 @@ void Tasks::CameraTask(void *arg){ cout << "Arret du traitment !!!!" << endl << flush; getImage = 0; - - // chopper l'arene if(arena.IsEmpty()){ cout << "Arena is empty" << endl << flush; // envoi d'un no ack au moniteur @@ -859,16 +864,13 @@ void Tasks::CameraTask(void *arg){ WriteInQueue(&q_messageToMon, imgToSend); } - cout << "Statut du traitement " << getImage << endl; - cout << "Statut du search " << searchArena << endl; rt_mutex_release(&mutex_image); - cout << "Avant Réponse ! " << endl << flush; - rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE); + if(searchArena){ - //rt_sem_p(&sem_answerSync, TM_INFINITE); - rt_mutex_acquire(&mutex_answerSync, TM_INFINITE); + rt_mutex_acquire(&mutex_answerSync, TM_INFINITE); // Possibilité de réduire les conditions inutiles avec des sémaphores if(answerSync){ + rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE); if(arenaConfirm){ // Arena OK cout << "Arena Saved" << endl << flush; @@ -879,15 +881,16 @@ void Tasks::CameraTask(void *arg){ // Rien ? } rt_mutex_acquire(&mutex_image, TM_INFINITE); - getImage = 1; // changer en booleen pour synchroniser plutot que ça + getImage = 1; rt_mutex_release(&mutex_image); searchArena = 0; answerSync = 0; - + rt_mutex_release(&mutex_answer_arena); } + rt_mutex_release(&mutex_answerSync); } - rt_mutex_release(&mutex_answer_arena); + rt_mutex_release(&mutex_search_arena); } }