From c81b6379fe98950e9af066b0995d67ba6b05f374 Mon Sep 17 00:00:00 2001 From: Lacroix Raphael Date: Tue, 11 Apr 2023 12:39:50 +0200 Subject: [PATCH] finished --- .../nbproject/configurations.xml | 2 +- .../nbproject/private/configurations.xml | 2 +- .../nbproject/private/private.xml | 3 + .../private/timestamps-10.105.1.10-xenomai-22 | 77 +++++++--- .../raspberry/superviseur-robot/tasks.cpp | 132 ++++++++++++------ 5 files changed, 150 insertions(+), 66 deletions(-) diff --git a/software/raspberry/superviseur-robot/nbproject/configurations.xml b/software/raspberry/superviseur-robot/nbproject/configurations.xml index f78aea7..48dba36 100644 --- a/software/raspberry/superviseur-robot/nbproject/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/configurations.xml @@ -119,7 +119,7 @@ - default + GNU|GNU true false diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index ffe1427..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.8: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 d6f1567..9f10d5a 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -7,9 +7,12 @@ + file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.h file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp + file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.h file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/main.cpp file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h + file:/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.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 df10862..5332383 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,39 +1,76 @@ -#Fri Mar 31 12:07:07 CEST 2023 -/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421 +#Tue Apr 11 12:36:14 CEST 2023 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.cpp=c1680524819149 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.cpp=c1680524985794 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1679401432930 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/Makefile=c1680524818973 /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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/README.md=c1680524819097 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1681203554260 /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=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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1680524819176 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1680524819187 /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=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 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.h=c1680524985797 /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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/README.md=c1680524818978 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.cpp=c1680524985804 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/.gitignore=c1680524819087 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1680524819092 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1681203554238 /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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.h=c1680524819103 /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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1680524819116 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.h=c1680524819138 /home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h=c1679401457331 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1680524819168 +/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 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1681209371364 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/wrapper.c=c1680524985856 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.gitignore=c1680524818971 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1680524819108 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1679401432832 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/superviseur.doxygen=c1680524819223 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680257220969 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/main.cpp=c1680524819156 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.dep.inc=c1679401432701 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1680524819095 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1680524819101 +/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/README.md=c1679401432714 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.dep.inc=c1680524818965 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1680524985812 +/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/main.cpp=c1679404095224 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1680524985801 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.properties=c1680524819214 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.h=c1680524985807 +/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/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.h=c1680524819130 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/gdbsudo.sh=c1680524819082 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1680524819127 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1681203554249 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.xml=c1680524985847 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.h=c1680524819153 /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 +/home/rlacroix/Bureau/4ir/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h=c1680862388302 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 8128cf4..5fae393 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -94,43 +94,54 @@ void Tasks::RobotLossCounter(void* arg){ Message* Tasks::parseMessage(Message* msg){ int state; - if (msg->CompareID(MESSAGE_ANSWER_NACK) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_TIMEOUT) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_ERROR) ||msg->CompareID(MESSAGE_ANSWER_COM_ERROR)){ - // increment - rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE); - compteurRobotLoss ++; - rt_mutex_release(&mutex_compteurRobotLoss); - } else { + + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + int tempRobotStarted = robotStarted ; + rt_mutex_release(&mutex_robotStarted); + + if (tempRobotStarted != 0) { - // reset - rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE); - compteurRobotLoss = 0; + if (msg->CompareID(MESSAGE_ANSWER_NACK) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_TIMEOUT) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_ERROR) ||msg->CompareID(MESSAGE_ANSWER_COM_ERROR)){ + // increment + rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE); + compteurRobotLoss ++; + cout << "Current loss value " << compteurRobotLoss << endl << flush; + rt_mutex_release(&mutex_compteurRobotLoss); + } else { + + // reset + rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE); + if (compteurRobotLoss > 0){ + cout << "Loss value reset" << endl << flush; + } + compteurRobotLoss = 0; + rt_mutex_release(&mutex_compteurRobotLoss); + } + + rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE); + if(compteurRobotLoss >= 3){ + // send info to monitor + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); // TODO : c'est probablement faux mais on a suivi la doc *sig* + cout << "Lost communication with robot " << endl << flush; + + // close robot + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + state = robot.Close(); + rt_mutex_release(&mutex_robot); + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 0; + rt_mutex_release(&mutex_robotStarted); + + + // send info to monitor + if (state < 0) { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); + } else { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); + } + } rt_mutex_release(&mutex_compteurRobotLoss); } - - rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE); - if(compteurRobotLoss >= 3){ - // send info to monitor - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); // TODO : c'est probablement faux mais on a suivi la doc *sig* - - // close robot - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - state = robot.Close(); - rt_mutex_release(&mutex_robot); - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 0; - rt_mutex_release(&mutex_robotStarted); - - - // send info to monitor - if (state < 0) { - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); - } else { - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); - } - } - rt_mutex_release(&mutex_compteurRobotLoss); - - return msg; } @@ -462,6 +473,23 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); robotPos = 0; rt_mutex_release(&mutex_robot_pos); + }else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){ + cout << "MONITOR LOST" << endl << flush; + + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + robot.Close(); + rt_mutex_release(&mutex_robot); + + + rt_mutex_acquire(&mutex_camera, TM_INFINITE); + camera.Close(); + rt_mutex_release(&mutex_camera); + + + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 0; + rt_mutex_release(&mutex_robotStarted); + } delete(msgRcv); // must be deleted manually, no consumer } @@ -497,6 +525,8 @@ void Tasks::OpenComRobot(void *arg) { rt_sem_v(&sem_connexionEstablished); msgSend = new Message(MESSAGE_ANSWER_ACK); } + + cout << "Open serial com ("; WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon } } @@ -580,21 +610,35 @@ void Tasks::RobotReloadMessage(void *arg) { int status; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + + + // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_reloadMessages, TM_INFINITE); + + + cout << "Starting to monitor packet losses " << endl << flush; rt_task_set_periodic(NULL, TM_NOW, 1000*1000*1000); while(1) { rt_task_wait_period(NULL); - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - status = parseMessage(robot.Write(robot.ReloadWD()))->GetID(); - rt_mutex_release(&mutex_robot); - // send info to monitor - if (status < 0) { - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); - } else { - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); + + + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + int tempRobotStarted = robotStarted ; + rt_mutex_release(&mutex_robotStarted); + + if (tempRobotStarted != 0) { + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + status = parseMessage(robot.Write(robot.ReloadWD()))->GetID(); + rt_mutex_release(&mutex_robot); + // send info to monitor + if (status < 0) { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); + } else { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); + } } } @@ -666,18 +710,18 @@ void Tasks::MoveTask(void *arg) { //cout << "Periodic movement update"; rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; + rt_mutex_release(&mutex_robotStarted); if (rs == 1) { rt_mutex_acquire(&mutex_move, TM_INFINITE); cpMove = move; rt_mutex_release(&mutex_move); - cout << " move: " << cpMove; + cout << " move: " << cpMove << endl << flush; rt_mutex_acquire(&mutex_robot, TM_INFINITE); parseMessage(robot.Write(new Message((MessageID)cpMove))); rt_mutex_release(&mutex_robot); } - rt_mutex_release(&mutex_robotStarted); //cout << endl << flush; } }