From 694c6314a36981e5ce92dbc45e98e258d5d4ad70 Mon Sep 17 00:00:00 2001 From: alejeune Date: Fri, 31 Mar 2023 12:13:03 +0200 Subject: [PATCH] Camera ouverte traitement en cours --- .../raspberry/superviseur-robot/lib/camera.h | 4 +- .../nbproject/private/configurations.xml | 2 +- .../nbproject/private/private.xml | 7 +- .../private/timestamps-10.105.1.10-xenomai-22 | 10 +- .../raspberry/superviseur-robot/tasks.cpp | 144 ++++++++++++++---- software/raspberry/superviseur-robot/tasks.h | 4 + 6 files changed, 130 insertions(+), 41 deletions(-) diff --git a/software/raspberry/superviseur-robot/lib/camera.h b/software/raspberry/superviseur-robot/lib/camera.h index 125d50f..68b361d 100644 --- a/software/raspberry/superviseur-robot/lib/camera.h +++ b/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; diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index 59431f3..7d18332 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.10:22 2 diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index ab84910..4096a95 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -6,6 +6,11 @@ - + + 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/main.cpp + diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.10-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.10-xenomai-22 index 0e13424..df10862 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.10-xenomai-22 +++ b/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 -/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1679401432892 +#Fri Mar 31 12:07:07 CEST 2023 +/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 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 1e0fb4f..186e3f2 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/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()){ - msgSend = new Message(MESSAGE_ANSWER_NACK) + bool status = camera.Open(); + 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); - cout << msgSend->GetID << endl << flush; + msg = new Message(MESSAGE_ANSWER_ACK); + cout << "ACK3 ! " << endl << flush ; + cout << msg->ToString() << endl << flush; + cout << "ACK4 ! " << endl << flush ; rt_mutex_release(&mutex_camera); - - if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { - rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); - cameraStarted = 1; - rt_mutex_release(&mutex_cameraStarted); - } + cout << "ACK ! " << endl << flush ; + rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); + cout << "ff" << endl << flush ; + cameraStarted = 1; + cout << "Cam success! " << endl <