From 96a3c8fe08b5855a30b157580a5be887c473ddb8 Mon Sep 17 00:00:00 2001 From: Newglear Date: Mon, 3 Apr 2023 14:41:39 +0200 Subject: [PATCH 1/4] draft Gwen camera jusqu'aux robot --- .idea/.gitignore | 8 ++ .idea/misc.xml | 20 +++++ .idea/vcs.xml | 6 ++ .../raspberry/superviseur-robot/tasks.cpp | 84 ++++++++++++++++--- software/raspberry/superviseur-robot/tasks.h | 3 + 5 files changed, 108 insertions(+), 13 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/misc.xml create mode 100644 .idea/vcs.xml diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..6a19798 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,20 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 1e0fb4f..783a624 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -285,6 +285,11 @@ 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_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){ + rt_mutex_acquire(&mutex_answer_arena); + msg_arena = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM? 1:0; // A vérifier que le ternaire est utile + rt_mutex_release(&mutex_answer_arena); + rt_sem_v(&sem_answerSync); } delete(msgRcv); // must be deleted manually, no consumer } @@ -365,7 +370,7 @@ void Tasks::StartCameraTask(void *arg) { rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ - /* The task startRobot starts here */ + /* The task Camera Task starts here */ /**************************************************************************************/ while (1) { @@ -466,28 +471,81 @@ void Tasks::BatteryTask(void *arg) { * @brief task in charge of managing the camera. */ void CameraTask(void *arg){ - int cam; + int cam; + std::list robotPosition; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000); while(1){ rt_task_wait_period(NULL); - rt_mutex_acquire(&mutex_camera, TM_INFINITE); - cam = cameraStarted; + rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); + cam = cameraStarted; + rt_mutex_release(&mutex_cameraStarted); - if(cam){ - // 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); + if(!cam){ + continue; } + rt_mutex_acquire(&mutex_camera, TM_INFINITE); + // Lancer le traitement périodique + Img img = camera.Grab(); + Arena arena= img.SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true + rt_mutex_release(&mutex_camera); + rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); + if(searchArena){ + // chopper l'arene + cout << "Search arena pls ! " << endl << flush; + 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; + } + } + rt_mutex_release(&mutex_search_arena); + rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); - } + if(robotPos){ + // chopper le robot + cout << "Pos du robot pls !" ; + robotPosition = img.SearchRobot(arena); + } + rt_mutex_release(&mutex_robot_pos); + + //Traitement du robot + if(){ // Robot position != null + img.DrawAllRobots(robotPosition); + } + + for (auto position : robotPosition) + { + WriteInQueue(&q_messageToMon, position); // Envoi de toutes les positions + } + + + + // Envoi de l'image + WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); + + rt_sem_p(&sem_answerSync, TM_INFINITE); + rt_mutex_acquire(&mutex_anwer_arena, TM_INFINITE); + + if(arenaConfirm){ + // Arena OK + img.DrawArena(arena); // Comment save ou rejeter? + }else{ + // Arena not OK + // Rien ? + } + rt_mutex_release(&mutex_answer_arena); + // Je sais pas si cette partie sert a qqc + } } /** diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index ec35145..36a57ae 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -67,6 +67,7 @@ private: Camera camera; int robotStarted = 0; int cameraStarted = 0; + int arenaConfirm = 0; int move = MESSAGE_ROBOT_STOP; /**********************************************************************/ @@ -90,6 +91,7 @@ private: RT_MUTEX mutex_cameraStarted; RT_MUTEX mutex_move; RT_MUTEX mutex_camera; + RT_MUTEX mutex_answer_arena; /**********************************************************************/ /* Semaphores */ @@ -99,6 +101,7 @@ private: RT_SEM sem_serverOk; RT_SEM sem_startRobot; RT_SEM sem_startCamera; + RT_SEM sem_answerSync; /**********************************************************************/ /* Message queues */ From a9194dee2d4f6fac23fb37ab7beb1dd80e2c166c Mon Sep 17 00:00:00 2001 From: alejeune Date: Mon, 3 Apr 2023 15:21:01 +0200 Subject: [PATCH 2/4] Camera presque finie --- .../nbproject/private/configurations.xml | 2 +- .../private/downloads-10.105.1.8-xenomai-22 | 0 .../nbproject/private/private.xml | 9 +- .../private/timestamps-10.105.1.8-xenomai-22 | 39 +++++ .../raspberry/superviseur-robot/tasks.cpp | 147 +++++++++++++----- software/raspberry/superviseur-robot/tasks.h | 4 + 6 files changed, 160 insertions(+), 41 deletions(-) create mode 100644 software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.8-xenomai-22 create mode 100644 software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index 59431f3..ffe1427 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml @@ -41,7 +41,7 @@ - xenomai@10.105.1.5:22 + xenomai@10.105.1.8:22 2 diff --git a/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.8-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.8-xenomai-22 new file mode 100644 index 0000000..e69de29 diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index ab84910..307d1c7 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -6,6 +6,13 @@ - + + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h + diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 new file mode 100644 index 0000000..2f029b3 --- /dev/null +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22 @@ -0,0 +1,39 @@ +#Mon Apr 03 15:19:02 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=c1680527409352 +/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=c1680518630237 +/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/wrapper.c=c1679401457381 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680519967821 +/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 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1679401432851 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1679401432844 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1679401457346 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1679401432827 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1679401432944 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1679401432840 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.h=c1679401432877 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h=c1679401457331 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.gitignore=c1679401432706 +VERSION=1.3 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.h=c1679401432868 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1679401432836 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1679401432848 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 783a624..1223fb8 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -27,6 +27,8 @@ #define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TCAMERA 21 #define PRIORITY_TBATTERY 25 +#define PRIORITY_TSTARTCAMERA 25 + /* * Some remarks: @@ -74,6 +76,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 +113,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 +142,7 @@ void Tasks::Init() { 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 +151,14 @@ 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); + } + if (err = rt_task_create(&th_startCamera, "th_startCamera", 0, PRIORITY_TSTARTCAMERA, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Tasks created successfully" << endl << flush; /**************************************************************************************/ @@ -176,7 +207,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; } @@ -240,7 +279,7 @@ void Tasks::SendToMonTask(void* arg) { while (1) { cout << "wait msg to send" << endl << flush; msg = ReadInQueue(&q_messageToMon); - cout << "Send msg to mon: " << msg->ToString() << endl << flush; + //cout << "Send msg to mon: " << msg->ToString() << endl << flush; rt_mutex_acquire(&mutex_monitor, TM_INFINITE); monitor.Write(msg); // The message is deleted with the Write rt_mutex_release(&mutex_monitor); @@ -285,12 +324,31 @@ 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_CLOSE)){ + rt_sem_v(&sem_startCamera); } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){ - rt_mutex_acquire(&mutex_answer_arena); - msg_arena = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM? 1:0; // A vérifier que le ternaire est utile + rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE); + arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0; // A vérifier que le ternaire est utile + cout << "Staut de l'arene" << arenaConfirm << endl << flush; rt_mutex_release(&mutex_answer_arena); rt_sem_v(&sem_answerSync); + }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){ + rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); + searchArena = 1; + rt_mutex_release(&mutex_search_arena); + cout << "Search arena true " << endl << flush; + }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); + cout << "Robot Pos true " << 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 } } @@ -365,35 +423,47 @@ void Tasks::StartRobotTask(void *arg) { * @brief Thread starting the communication with the robot. */ void Tasks::StartCameraTask(void *arg) { - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ - /* The task Camera Task starts here */ + /* The task startRobot starts here */ /**************************************************************************************/ while (1) { Message * msgSend; + Message * msg; + bool status; rt_sem_p(&sem_startCamera, TM_INFINITE); - cout << "Start Camera "; + rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); rt_mutex_acquire(&mutex_camera, TM_INFINITE); - if(!camera.Open()){ - msgSend = new Message(MESSAGE_ANSWER_NACK) + if(!cameraStarted){ + status = camera.Open(); + cout << "Camera Opened" << endl << flush; + }else{ + camera.Close(); + status = true; + 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->GetID << endl << flush; + cout << msgSend->ToString() << endl << flush; rt_mutex_release(&mutex_camera); + rt_mutex_release(&mutex_cameraStarted); continue; } - msg = new Message(MESSAGE_ANSWER_ACK); - cout << msgSend->GetID << endl << flush; + msg = new Message(MESSAGE_ANSWER_ACK); + cout << msg->ToString() << endl << flush; + cameraStarted = !cameraStarted; + cout << cameraStarted << endl << flush; rt_mutex_release(&mutex_camera); + rt_mutex_release(&mutex_cameraStarted); - if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { - rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); - cameraStarted = 1; - rt_mutex_release(&mutex_cameraStarted); - } + cout << "Cam success! " << endl < robotPosition; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; @@ -495,7 +565,6 @@ void CameraTask(void *arg){ rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); if(searchArena){ // chopper l'arene - cout << "Search arena pls ! " << endl << flush; if(arena.IsEmpty()){ cout << "Arena empty" << endl << flush; // envoi d'un no ack au moniteur @@ -505,47 +574,47 @@ void CameraTask(void *arg){ img.DrawArena(arena); // envoi de l'arene pour vérif - cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush; } } - rt_mutex_release(&mutex_search_arena); - rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); + rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); if(robotPos){ // chopper le robot cout << "Pos du robot pls !" ; robotPosition = img.SearchRobot(arena); + + + + //Traitement du robot + if(!robotPosition.empty()){ // Robot position != null + img.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); - - //Traitement du robot - if(){ // Robot position != null - img.DrawAllRobots(robotPosition); - } - - for (auto position : robotPosition) - { - WriteInQueue(&q_messageToMon, position); // Envoi de toutes les positions - } - - - // Envoi de l'image WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); rt_sem_p(&sem_answerSync, TM_INFINITE); - rt_mutex_acquire(&mutex_anwer_arena, TM_INFINITE); + rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE); if(arenaConfirm){ // Arena OK img.DrawArena(arena); // Comment save ou rejeter? + //searchArena = 0; }else{ // Arena not OK + searchArena = 0; // Rien ? } rt_mutex_release(&mutex_answer_arena); + rt_mutex_release(&mutex_search_arena); // Je sais pas si cette partie sert a qqc - } + } } /** diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 36a57ae..a2216f2 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -66,8 +66,10 @@ private: ComRobot robot; Camera camera; int robotStarted = 0; + int robotPos = 0; int cameraStarted = 0; int arenaConfirm = 0; + int searchArena = 0; int move = MESSAGE_ROBOT_STOP; /**********************************************************************/ @@ -91,7 +93,9 @@ private: RT_MUTEX mutex_cameraStarted; RT_MUTEX mutex_move; RT_MUTEX mutex_camera; + RT_MUTEX mutex_search_arena; RT_MUTEX mutex_answer_arena; + RT_MUTEX mutex_robot_pos; /**********************************************************************/ /* Semaphores */ From 670fff6a0aef6ce3a9869cca50dc30506fb3171c Mon Sep 17 00:00:00 2001 From: alejeune Date: Fri, 7 Apr 2023 12:10:04 +0200 Subject: [PATCH 3/4] Camera almost finished: todo suppress tooggle, semaphore battery, fps count --- .../raspberry/superviseur-robot/lib/img.h | 2 +- .../nbproject/private/configurations.xml | 2 +- .../private/downloads-10.105.1.3-xenomai-22 | 0 .../nbproject/private/private.xml | 2 + .../private/timestamps-10.105.1.3-xenomai-22 | 39 +++++++++++ .../raspberry/superviseur-robot/tasks.cpp | 68 ++++++++++++------- software/raspberry/superviseur-robot/tasks.h | 2 +- 7 files changed, 88 insertions(+), 27 deletions(-) create mode 100644 software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.3-xenomai-22 create mode 100644 software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 diff --git a/software/raspberry/superviseur-robot/lib/img.h b/software/raspberry/superviseur-robot/lib/img.h index 11822e0..191d5c6 100644 --- a/software/raspberry/superviseur-robot/lib/img.h +++ b/software/raspberry/superviseur-robot/lib/img.h @@ -39,7 +39,7 @@ using namespace std; /** * Redefinition of cv::Mat type */ -typedef cv::Mat ImageMat; + typedef cv::Mat ImageMat; /** * Declaration of Jpg type diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index ffe1427..a20af05 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml @@ -41,7 +41,7 @@ - xenomai@10.105.1.8:22 + xenomai@10.105.1.3:22 2 diff --git a/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.3-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.3-xenomai-22 new file mode 100644 index 0000000..e69de29 diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 307d1c7..56849c0 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -9,8 +9,10 @@ file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 new file mode 100644 index 0000000..536a776 --- /dev/null +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 @@ -0,0 +1,39 @@ +#Fri Apr 07 11:52:43 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/.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=c1680518630237 +/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/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/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 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1679401432851 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1679401432844 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1679401457346 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1679401432827 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1679401432944 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1679401432840 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.h=c1679401432877 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h=c1680855564574 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.gitignore=c1679401432706 +VERSION=1.3 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.h=c1679401432868 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1679401432836 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1679401432848 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 1223fb8..dab0eea 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -22,12 +22,12 @@ #define PRIORITY_TSERVER 30 #define PRIORITY_TOPENCOMROBOT 20 #define PRIORITY_TMOVE 20 -#define PRIORITY_TSENDTOMON 22 +#define PRIORITY_TSENDTOMON 23 #define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TCAMERA 21 -#define PRIORITY_TBATTERY 25 -#define PRIORITY_TSTARTCAMERA 25 +#define PRIORITY_TBATTERY 19 +#define PRIORITY_TSTARTCAMERA 22 /* @@ -46,7 +46,19 @@ * * 6- When you want to write something in terminal, use cout and terminate with endl and flush * - * 7- Good luck ! + * 7- Good luck !#include + +// Déclaration des priorités des taches +#define PRIORITY_TSERVER 30 +#define PRIORITY_TOPENCOMROBOT 20 +#define PRIORITY_TMOVE 20 +#define PRIORITY_TSENDTOMON 23 +#define PRIORITY_TRECEIVEFROMMON 25 +#define PRIORITY_TSTARTROBOT 20 +#define PRIORITY_TCAMERA 21 +#define PRIORITY_TBATTERY 19 +#define PRIORITY_TSTARTCAMERA 22 + */ /** @@ -164,12 +176,14 @@ void Tasks::Init() { /**************************************************************************************/ /* Message queues creation */ /**************************************************************************************/ - if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) { + if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*100, Q_UNLIMITED, Q_FIFO)) < 0) { cerr << "Error msg queue create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } cout << "Queues created successfully" << endl << flush; - + + camera = new Camera(sm,5); + cout << "New camera ! " << endl << flush; } /** @@ -329,7 +343,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){ rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE); arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0; // A vérifier que le ternaire est utile - cout << "Staut de l'arene" << arenaConfirm << endl << flush; + cout << "Statut de l'arene" << arenaConfirm << endl << flush; rt_mutex_release(&mutex_answer_arena); rt_sem_v(&sem_answerSync); }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){ @@ -439,10 +453,10 @@ void Tasks::StartCameraTask(void *arg) { rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); rt_mutex_acquire(&mutex_camera, TM_INFINITE); if(!cameraStarted){ - status = camera.Open(); + status = camera->Open(); cout << "Camera Opened" << endl << flush; }else{ - camera.Close(); + camera->Close(); status = true; cout << "Camera Closed" << endl << flush; } @@ -520,7 +534,7 @@ 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; @@ -534,7 +548,7 @@ void Tasks::BatteryTask(void *arg) { } rt_mutex_release(&mutex_robotStarted); - } + }*/ } /** @@ -542,7 +556,11 @@ void Tasks::BatteryTask(void *arg) { */ void Tasks::CameraTask(void *arg){ int cam; + Img* image; + Arena arenaSaved ; std::list robotPosition; + MessageImg* imgToSend; + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -557,12 +575,15 @@ void Tasks::CameraTask(void *arg){ continue; } + cout << "Traitement de l'image" << endl << flush ; rt_mutex_acquire(&mutex_camera, TM_INFINITE); // Lancer le traitement périodique - Img img = camera.Grab(); - Arena arena= img.SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true + + Img img = camera->Grab(); + 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 if(searchArena){ // chopper l'arene if(arena.IsEmpty()){ @@ -571,23 +592,22 @@ void Tasks::CameraTask(void *arg){ }else{ cout << "Draw Arena" << endl << flush; - img.DrawArena(arena); - + 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 << "Pos du robot pls !" ; - robotPosition = img.SearchRobot(arena); - - + robotPosition = image->SearchRobot(arena); //Traitement du robot if(!robotPosition.empty()){ // Robot position != null - img.DrawAllRobots(robotPosition); + image->DrawAllRobots(robotPosition); } for (auto position : robotPosition) { @@ -597,23 +617,23 @@ void Tasks::CameraTask(void *arg){ } rt_mutex_release(&mutex_robot_pos); // Envoi de l'image - WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); + 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(arenaConfirm){ // Arena OK - img.DrawArena(arena); // Comment save ou rejeter? - //searchArena = 0; + arenaSaved = arena; + searchArena = 0; }else{ // Arena not OK - searchArena = 0; // Rien ? } rt_mutex_release(&mutex_answer_arena); rt_mutex_release(&mutex_search_arena); - // Je sais pas si cette partie sert a qqc } } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index a2216f2..b89b7d8 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -64,7 +64,7 @@ private: /**********************************************************************/ ComMonitor monitor; ComRobot robot; - Camera camera; + Camera* camera; int robotStarted = 0; int robotPos = 0; int cameraStarted = 0; From fa2322f0bde4d9ee4ae0de3fe72b2cbc669627fa Mon Sep 17 00:00:00 2001 From: alejeune Date: Tue, 11 Apr 2023 12:43:11 +0200 Subject: [PATCH 4/4] Camera bancale --- .../nbproject/private/private.xml | 1 + .../private/timestamps-10.105.1.3-xenomai-22 | 6 +- .../raspberry/superviseur-robot/tasks.cpp | 153 +++++++++++------- software/raspberry/superviseur-robot/tasks.h | 4 + 4 files changed, 102 insertions(+), 62 deletions(-) diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 56849c0..f7493fc 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -14,6 +14,7 @@ file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 index 536a776..631787a 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 +++ b/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 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index dab0eea..0ff2de1 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/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 <ToString(); + MessageBattery* batteryReply = (MessageBattery*)robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET)); 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 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 ; - rt_mutex_acquire(&mutex_camera, TM_INFINITE); - // Lancer le traitement périodique + Img img = camera->Grab(); + image = img.Copy(); + 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(); - 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 - if(searchArena){ - // chopper l'arene - 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{ + cout << "Draw Arena" << endl << flush; + image->DrawArena(arena); + // envoi de l'arene pour vérif + } + }else { + image->DrawArena(arenaSaved); } - }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(arenaConfirm){ - // Arena OK - arenaSaved = arena; + if(searchArena){ + rt_sem_p(&sem_answerSync, TM_INFINITE); + if(arenaConfirm){ + // 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); diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index b89b7d8..f163e45 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/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 */