diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index a20af05..31c7391 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.3:22 + xenomai@10.105.1.4:22 2 diff --git a/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.4-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.4-xenomai-22 new file mode 100644 index 0000000..e69de29 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 new file mode 100644 index 0000000..1851cb5 --- /dev/null +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.4-xenomai-22 @@ -0,0 +1,39 @@ +#Thu Apr 13 16:52:14 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 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.h=c1681393207285 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/.gitignore=c1681393207250 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.h=c1681393672548 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.h=c1681393226953 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1681393207325 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1681393207318 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1681393207323 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.gitignore=c1681393207123 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1681393207281 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1681393207269 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.properties=c1681393207350 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1681393226957 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1681393207264 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1681393226947 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h=c1681396618984 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/gdbsudo.sh=c1681393207246 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.cpp=c1681393226941 +/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/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 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.xml=c1681393226999 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.h=c1681393207266 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.cpp=c1681393207300 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/main.cpp=c1681393207306 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.h=c1681393207303 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/Makefile=c1681393207126 +VERSION=1.3 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1681393207332 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/wrapper.c=c1681393227008 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.h=c1681393207291 +/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/README.md=c1681393207129 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index b3b1f6a..5b07e81 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -160,6 +160,10 @@ void Tasks::Init() { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_mutex_create(&mutex_answerSync, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Mutexes created successfully" << endl << flush; /**************************************************************************************/ @@ -196,6 +200,9 @@ void Tasks::Init() { if (err = rt_sem_create(&sem_reloadMessages, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); + }if (err = rt_sem_create(&sem_answerSync, NULL, 0, S_FIFO)) { + cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); } cout << "Semaphores created successfully" << endl << flush; @@ -408,11 +415,17 @@ void Tasks::ReceiveFromMonTask(void *arg) { delete(msgRcv); exit(-1); } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { + rt_sem_v(&sem_openComRobot); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { + rt_sem_v(&sem_startRobot); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { + rt_sem_v(&sem_startRobotWithWatchdog); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || @@ -422,36 +435,54 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_move, TM_INFINITE); move = msgRcv->GetID(); rt_mutex_release(&mutex_move); - } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){ + + } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){ // Ouverture de la caméra + rt_sem_v(&sem_startCamera); - } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){ + + } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){ // Fermeture de la caméra + rt_sem_v(&sem_startCamera); - }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){ + + }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){ // Demande de recherche de l'arène + cout << "Enabling search arena" << endl << flush; + rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); - searchArena = 1; + searchArena = 1; // Activation du flag de recherche de l'arène 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_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){ + + cout << "Search arena started " << endl << flush; + + } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || // Réponse du moniteur sur la confirmation de l'arène + 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 << "Statut de l'arene" << arenaConfirm << endl << flush; + arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0; + //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_POSITION_COMPUTE_START)){ + + //rt_sem_v(&sem_answerSync); + rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE); + answerSync = 1; + rt_mutex_release(&mutex_answer_arena); + + }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){ // Recherche de la position du robot + 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)){ + + cout << "Searching robot position " << endl << flush; + + }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){ // Arret de la recherche du robot + rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); - robotPos = 0; + robotPos = 0; // rt_mutex_release(&mutex_robot_pos); cout << "Robot Pos false" << endl << flush; - }else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){ + + }else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){ // Perte de connexion avec le robot cout << "MONITOR LOST" << endl << flush; rt_mutex_acquire(&mutex_robot, TM_INFINITE); @@ -755,27 +786,29 @@ void Tasks::BatteryTask(void *arg) { void Tasks::CameraTask(void *arg){ int cam; int grab; - Img* image; - Arena arenaSaved,arena ; - std::list robotPosition; + 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 MessageImg* imgToSend; 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); + rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000); // Periode de 100 ms + while(1){ rt_task_wait_period(NULL); rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); cam = cameraStarted; rt_mutex_release(&mutex_cameraStarted); - + if(!cam){ continue; } - + rt_mutex_acquire(&mutex_image, TM_INFINITE); if(getImage){ - cout << "Traitement de l'image" << endl << flush ; + cout << "Statut du traitement " << getImage << endl; + //cout << "Traitement de l'image" << endl << flush ; rt_mutex_acquire(&mutex_camera, TM_INFINITE); // Lancer le traitement périodique @@ -785,14 +818,16 @@ void Tasks::CameraTask(void *arg){ 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); + + cout << "Arret du traitment !!!!" << endl << flush; getImage = 0; - rt_mutex_release(&mutex_image); + // chopper l'arene if(arena.IsEmpty()){ - cout << "Arena empty" << endl << flush; + cout << "Arena is empty" << endl << flush; // envoi d'un no ack au moniteur - + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); + }else{ cout << "Draw Arena" << endl << flush; image->DrawArena(arena); @@ -824,24 +859,33 @@ 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); - if(arenaConfirm){ - // Arena OK - cout << "Arena Saved" << endl << flush; - arenaSaved = arena; - }else{ - // Arena not OK - cout << "Arena Dumped" << endl << flush; - // Rien ? + //rt_sem_p(&sem_answerSync, TM_INFINITE); + rt_mutex_acquire(&mutex_answerSync, TM_INFINITE); + if(answerSync){ + + 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; + answerSync = 0; + } - rt_mutex_acquire(&mutex_image, TM_INFINITE); - getImage = 1; // changer en booleen pour synchroniser plutot que ça - rt_mutex_release(&mutex_image); - searchArena = 0; + rt_mutex_release(&mutex_answerSync); } 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 046c14b..5a57ca6 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -72,6 +72,7 @@ private: int arenaConfirm = 0; int searchArena = 0; int getImage = 0; + int answerSync = 0; int move = MESSAGE_ROBOT_STOP; /**********************************************************************/ @@ -105,6 +106,7 @@ private: RT_MUTEX mutex_robot_pos; RT_MUTEX mutex_battery; RT_MUTEX mutex_image; + RT_MUTEX mutex_answerSync; /**********************************************************************/