From c67814a39b630e52e4a9186a2f62a23b047f05ff Mon Sep 17 00:00:00 2001 From: Robin Marin-Muller Date: Thu, 4 Apr 2024 17:55:02 +0200 Subject: [PATCH] Added acknowledge, created SendToRobot() --- .../timestamps-10.105.0.140-xenomai-22 | 6 ++--- .../raspberry/superviseur-robot/tasks.cpp | 23 +++++++++++-------- software/raspberry/superviseur-robot/tasks.h | 3 +++ 3 files changed, 19 insertions(+), 13 deletions(-) diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 index ce021a0..8f61dbb 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 @@ -1,4 +1,4 @@ -#Thu Apr 04 17:21:41 CEST 2024 +#Thu Apr 04 17:49:31 CEST 2024 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/main.cpp=c1712238648769 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1712238648773 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1712238648790 @@ -8,13 +8,13 @@ /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1712238648775 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1712238648721 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1712238648782 -/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1712243914675 +/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1712244813410 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/README.md=c1712238648637 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1712238648826 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1712238648778 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1712238648829 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1712238648730 -/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1712244064711 +/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1712245729354 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1712238648753 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/.dep.inc=c1712238648629 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/img.h=c1712238648760 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index f904a7c..9fcfe77 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -487,6 +487,13 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) { /* OUR CODE */ +void Tasks::SendToRobot(MessageID message) +{ + rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + WriteInQueue(&q_messageToMon, new Message(message)); + rt_mutex_release(&mutex_monitor); +} + void Tasks::BatteryStatusTask(void * arg) { // Variables int rs(0); @@ -512,16 +519,9 @@ void Tasks::BatteryStatusTask(void * arg) { getbatterytask = robotBatteryGet; if ((rs != 0) && getbatterytask) { - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - Message* msg = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET)); - rt_mutex_release(&mutex_robot); - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Write(msg); - rt_mutex_release(&mutex_monitor); - + SendToRobot(MESSAGE_ROBOT_BATTERY_GET); robotBatteryGet = false; getbatterytask = false; - } rt_mutex_release(&mutex_batteryGet); } @@ -563,7 +563,10 @@ void Tasks::OpenCamera(void * arg) if(is_opened) { cameraStatus = CameraStatusEnum::OPENED; - std::cout << "Camera Opened" << std::endl; + SendToRobot(MESSAGE_ANSWER_ACK); + } + else { + SendToRobot(MESSAGE_ANSWER_NACK); } rt_mutex_release(&mutex_cameraStatus); } @@ -626,8 +629,8 @@ void Tasks::CloseCamera(void * arg) rt_mutex_release(&mutex_camera); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); cameraStatus = CameraStatusEnum::CLOSED; - std::cout << "Camera Closed" << std::endl; rt_mutex_release(&mutex_cameraStatus); + SendToRobot(MESSAGE_ANSWER_ACK); } } } \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index dd4b700..8092d38 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -156,6 +156,9 @@ private: void OpenCamera(void * arg); void ImageCamera(void * arg); void CloseCamera(void * arg); + + // Utility functions + void SendToRobot(MessageID message); /**********************************************************************/