From 8f4c8ef6616315332f575f7e60b8a8a7e109f2b6 Mon Sep 17 00:00:00 2001 From: Arnaud Vergnet Date: Mon, 15 Mar 2021 11:54:44 +0100 Subject: [PATCH] Add manage robot task --- .../raspberry/superviseur-robot/tasks.cpp | 149 +++++++++--------- software/raspberry/superviseur-robot/tasks.h | 10 +- 2 files changed, 78 insertions(+), 81 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 6826c34..9f1b756 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -22,7 +22,6 @@ using namespace std; // Déclaration des priorités des taches constexpr int PRIORITY_TSERVER = 30; -constexpr int PRIORITY_TOPENCOMROBOT = 20; constexpr int PRIORITY_TMOVE = 20; constexpr int PRIORITY_TSENDTOMON = 22; constexpr int PRIORITY_TRECEIVEFROMMON = 25; @@ -65,6 +64,10 @@ void Tasks::Init() { cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl; exit(EXIT_FAILURE); } + if ((err = rt_mutex_create(&mutex_monitorConnected, nullptr))) { + cerr << "Error mutex create mutex_monitorConnected: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } if ((err = rt_mutex_create(&mutex_robot, nullptr))) { cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -90,10 +93,6 @@ void Tasks::Init() { cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - if ((err = rt_sem_create(&sem_openComRobot, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } if ((err = rt_sem_create(&sem_serverOk, nullptr, 0, S_FIFO))) { cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -106,10 +105,6 @@ void Tasks::Init() { cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - if ((err = rt_sem_create(&sem_stopComRobot, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create sem_stopComRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } if ((err = rt_sem_create(&sem_stopServer, nullptr, 0, S_FIFO))) { cerr << "Error semaphore create sem_stopServer: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -131,14 +126,6 @@ void Tasks::Init() { cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) { - cerr << "Error task create th_openComRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) { - cerr << "Error task create th_startRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) { cerr << "Error task create th_move: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -148,6 +135,10 @@ void Tasks::Init() { cerr << "Error task create th_sendBatteryLevel: " << strerror(-err) << endl; exit(EXIT_FAILURE); } + if ((err = rt_task_create(&th_manageRobot, "th_manageRobot", 0, PRIORITY_TSTARTROBOT, 0))) { + cerr << "Error task create th_manageRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } cout << "Tasks created successfully" << endl; @@ -181,12 +172,8 @@ void Tasks::Run() { cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - if (err = rt_task_start(&th_openComRobot, reinterpret_cast(&Tasks::OpenComRobot), this)) { - cerr << "Error task start OpenComRobot: " << strerror(-err) << endl; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_startRobot, reinterpret_cast(&Tasks::StartRobotTask), this)) { - cerr << "Error task start StartRobotTask: " << strerror(-err) << endl; + if (err = rt_task_start(&th_manageRobot, reinterpret_cast(&Tasks::ManageRobotTask), this)) { + cerr << "Error task start ManageRobotTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_move, reinterpret_cast(&Tasks::MoveTask), this)) { @@ -242,12 +229,19 @@ void Tasks::Join() { monitor.AcceptClient(); // Wait the monitor client cout << "Rock'n'Roll baby, client accepted!" << endl; + rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE); + monitorConnected = true; + rt_mutex_release(&mutex_monitorConnected); rt_sem_broadcast(&sem_serverOk); // Wait for stopServer signal rt_sem_p(&sem_stopServer, TM_INFINITE); + rt_sem_v(&sem_stopRobot); rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE); + monitorConnected = false; monitor.Close(); + rt_mutex_release(&mutex_monitorConnected); rt_mutex_release(&mutex_monitor); } } @@ -272,7 +266,11 @@ void Tasks::Join() { msg = ReadInQueue(&q_messageToMon); cout << "Send msg to mon: " << msg->ToString() << endl; rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Write(msg); // The message is deleted with the Write + rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE); + if (monitorConnected) { + monitor.Write(msg); // The message is deleted with the Write + } + rt_mutex_release(&mutex_monitorConnected); rt_mutex_release(&mutex_monitor); } } @@ -302,13 +300,11 @@ void Tasks::Join() { if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { cout << "Monitor connection lost! Stopping robot..." << endl; -// rt_sem_v(&sem_stopCamera); // TODO - rt_sem_v(&sem_stopRobot); - rt_sem_v(&sem_stopComRobot); +// rt_sem_v(&sem_stopCamera); // TODO rt_sem_v(&sem_stopServer); lostConnection = true; } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { - rt_sem_v(&sem_openComRobot); + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); watchdogMode = WITHOUT_WATCHDOG; @@ -320,6 +316,8 @@ void Tasks::Join() { watchdogMode = WITH_WATCHDOG; rt_mutex_release(&mutex_watchdogMode); rt_sem_v(&sem_startRobot); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_RESET)) { + rt_sem_v(&sem_stopRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || @@ -349,43 +347,11 @@ void Tasks::Join() { } } -/** - * @brief Thread opening communication with the robot. - */ -[[noreturn]] void Tasks::OpenComRobot(void *arg) { - int status; - int err; - - cout << "Start " << __PRETTY_FUNCTION__ << endl; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /* ************************************************************************************* - * The task openComRobot starts here - * *************************************************************************************/ - while (true) { - rt_sem_p(&sem_openComRobot, TM_INFINITE); - cout << "Open serial com ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - status = robot.Open(); - rt_mutex_release(&mutex_robot); - cout << status; - cout << ")" << endl; - - Message *msgSend; - if (status < 0) { - msgSend = new Message(MESSAGE_ANSWER_NACK); - } else { - msgSend = new Message(MESSAGE_ANSWER_ACK); - } - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon - } -} - /** * @brief Thread starting the communication with the robot. */ -[[noreturn]] void Tasks::StartRobotTask(void *arg) { +[[noreturn]] void Tasks::ManageRobotTask(void *arg) { + int status; cout << "Start " << __PRETTY_FUNCTION__ << endl; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -396,25 +362,60 @@ void Tasks::Join() { while (true) { Message *msgSend; rt_sem_p(&sem_startRobot, TM_INFINITE); - cout << "Start robot without watchdog ("; + cout << "Open serial com ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(ComRobot::StartWithoutWD()); + status = robot.Open(); rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); + cout << status; cout << ")" << endl; + if (status < 0) { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); // msgSend will be deleted by sendToMon + } else { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); // msgSend will be deleted by sendToMon + rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); + const WatchdogMode mode = watchdogMode; + rt_mutex_release(&mutex_watchdogMode); - cout << "Movement answer: " << msgSend->ToString() << endl; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + if (mode == WITH_WATCHDOG) { + cout << "Start robot with watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(ComRobot::StartWithWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + } else { + cout << "Start robot without watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(ComRobot::StartWithoutWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + } - if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 1; - rt_mutex_release(&mutex_robotStarted); + + cout << "Movement answer: " << msgSend->ToString() << endl; + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + + if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + rt_mutex_release(&mutex_robotStarted); + rt_sem_p(&sem_stopRobot, TM_INFINITE); + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + robot.Write(new Message(MESSAGE_ROBOT_STOP)); + robot.Close(); + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 0; + rt_mutex_release(&mutex_robotStarted); + rt_mutex_release(&mutex_robot); + } } } } - +/** + * @brief Thread seding the battery level. + */ [[noreturn]] void Tasks::SendBatteryLevel(void *arg) { int rs; cout << "Start " << __PRETTY_FUNCTION__ << endl; @@ -487,9 +488,7 @@ void Tasks::Join() { } if (counter == 3) { rt_sem_v(&sem_stopRobot); - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Write(new Message(MESSAGE_ANSWER_COM_ERROR)); - rt_mutex_release(&mutex_monitor); + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); } rt_mutex_release(&mutex_robot); } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 886f451..929097b 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -70,6 +70,7 @@ private: ComRobot robot; int robotStarted = 0; int move = MESSAGE_ROBOT_STOP; + bool monitorConnected = false; WatchdogMode watchdogMode = WITHOUT_WATCHDOG; /* ********************************************************************* @@ -78,15 +79,15 @@ private: RT_TASK th_server; RT_TASK th_sendToMon; RT_TASK th_receiveFromMon; - RT_TASK th_openComRobot; - RT_TASK th_startRobot; RT_TASK th_move; RT_TASK th_sendBatteryLevel; + RT_TASK th_manageRobot; /* ********************************************************************* * Mutex * ********************************************************************/ RT_MUTEX mutex_monitor; + RT_MUTEX mutex_monitorConnected; RT_MUTEX mutex_robot; RT_MUTEX mutex_robotStarted; RT_MUTEX mutex_move; @@ -96,11 +97,9 @@ private: * Semaphores * ********************************************************************/ RT_SEM sem_barrier; - RT_SEM sem_openComRobot; RT_SEM sem_serverOk; RT_SEM sem_startRobot; RT_SEM sem_stopRobot; - RT_SEM sem_stopComRobot; RT_SEM sem_stopServer; /* ********************************************************************* @@ -136,7 +135,7 @@ private: /** * @brief Thread starting the communication with the robot. */ - [[noreturn]] void StartRobotTask(void *arg); + [[noreturn]] void ManageRobotTask(void *arg); /** * @brief Thread handling control of the robot. @@ -166,7 +165,6 @@ private: * @return Message read */ static Message *ReadInQueue(RT_QUEUE *queue); - };