From 96a3c8fe08b5855a30b157580a5be887c473ddb8 Mon Sep 17 00:00:00 2001 From: Newglear Date: Mon, 3 Apr 2023 14:41:39 +0200 Subject: [PATCH] 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 */