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 */