diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp deleted file mode 100644 index f4acecf..0000000 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ /dev/null @@ -1,674 +0,0 @@ -/* - * Copyright (C) 2018 dimercur - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include "tasks.h" -#include - -// Déclaration des priorités des taches -#define PRIORITY_TSERVER 30 -#define PRIORITY_TOPENCOMROBOT 20 -#define PRIORITY_TMOVE 20 -#define PRIORITY_TSENDTOMON 22 -#define PRIORITY_TRECEIVEFROMMON 25 -#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22 -#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22 -#define PRIORITY_TCAMERA 21 - - -/* - * Some remarks: - * 1- This program is mostly a template. It shows you how to create tasks, semaphore - * message queues, mutex ... and how to use them - * - * 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore - * - * 3- Data flow is probably not optimal - * - * 4- Take into account that ComRobot::Write will block your task when serial buffer is full, - * time for internal buffer to flush - * - * 5- Same behavior existe for ComMonitor::Write ! - * - * 6- When you want to write something in terminal, use cout and terminate with endl and flush - * - * 7- Good luck ! - */ - -/** - * @brief Initialisation des structures de l'application (tâches, mutex, - * semaphore, etc.) - */ -void Tasks::Init() { - int status; - int err; - - /**************************************************************************************/ - /* Mutex creation */ - /**************************************************************************************/ - if (err = rt_mutex_create(&mutex_monitor, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_mutex_create(&mutex_robot, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_mutex_create(&mutex_robotStarted, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_mutex_create(&mutex_move, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_mutex_create(&mutex_killReceiveFromMon, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_mutex_create(&mutex_killVision, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_mutex_create(&mutex_searchArena, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_mutex_create(&mutex_drawArena, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_mutex_create(&mutex_getPosition, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_mutex_create(&mutex_killSendMon, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_mutex_create(&mutex_killDetectLostSupRob, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_mutex_create(&mutex_acquireImage, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - - - - /*if (err = rt_mutex_create(&mutex_robot_on, NULL)) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - }*/ - - - cout << "Mutexes created successfully" << endl << flush; - - /**************************************************************************************/ - /* Semaphors creation */ - /**************************************************************************************/ - if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_sem_create(&sem_openComRobot, NULL, 0, S_FIFO)) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_sem_create(&sem_startRobotWithoutWatchdog, NULL, 0, S_FIFO)) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - - //Initialization for some specific sem: Allow to run the first time - rt_sem_v(&sem_allowStartReceive); - - cout << "Semaphores created successfully" << endl << flush; - - /**************************************************************************************/ - /* Tasks creation */ - /**************************************************************************************/ - if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - cout << "Tasks created successfully" << endl << flush; - - /**************************************************************************************/ - /* Message queues creation */ - /**************************************************************************************/ - if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) { - cerr << "Error msg queue create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - cout << "Queues created successfully" << endl << flush; - -} - -/** - * @brief Démarrage des tâches - */ -void Tasks::Run() { - rt_task_set_priority(NULL, T_LOPRIO); - int err; - - if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - - cout << "Tasks launched" << endl << flush; -} - -/** - * @brief Arrêt des tâches - */ -void Tasks::Stop() { - monitor.Close(); - robot.Close(); -} - -/** - */ -void Tasks::Join() { - cout << "Tasks synchronized" << endl << flush; - rt_sem_broadcast(&sem_barrier); - pause(); -} - -/** - * @brief Thread handling server communication with the monitor. - */ -void Tasks::ServerTask(void *arg) { - int status; - - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are started) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /**************************************************************************************/ - /* The task server starts here */ - /**************************************************************************************/ - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - status = monitor.Open(SERVER_PORT); - rt_mutex_release(&mutex_monitor); - - cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; - - if (status < 0) throw std::runtime_error { - "Unable to start server on port " + std::to_string(SERVER_PORT) - }; - monitor.AcceptClient(); // Wait the monitor client - cout << "Rock'n'Roll baby, client accepted!" << endl << flush; - rt_sem_broadcast(&sem_serverOk); -} - -/** - * @brief Thread sending data to monitor. - */ -void Tasks::SendToMonTask(void* arg) { - Message *msg; - - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /**************************************************************************************/ - /* The task sendToMon starts here */ - /**************************************************************************************/ - rt_sem_p(&sem_serverOk, TM_INFINITE); - - while (1) { - cout << "wait msg to send" << endl << flush; - msg = ReadInQueue(&q_messageToMon); - cout << "Send msg to mon: " << msg->ToString() << endl << flush; - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Write(msg); // The message is deleted with the Write - rt_mutex_release(&mutex_monitor); - } -} - -/** - * @brief Thread receiving data from monitor. - */ -void Tasks::ReceiveFromMonTask(void *arg) { - Message *msgRcv; - bool killReceiveFromMonOk=0; - - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - //Wait twin task to die if not first round - rt_sem_p(&sem_allowStartReceive, TM_INFINITE); - - //Reinitialize control boolean - rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); - killReceiveFromMon=0; - rt_mutex_release(&mutex_killReceiveFromMon); - - /**************************************************************************************/ - /* The task receiveFromMon starts here */ - /**************************************************************************************/ - rt_sem_p(&sem_serverOk, TM_INFINITE); - cout << "Received message from monitor activated" << endl << flush; - while (!killReceiveFromMonOk) { - msgRcv = monitor.Read(); - cout << "Rcv <= " << msgRcv->ToString() << endl << flush; - - if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { - delete(msgRcv); - cout << "Connection to monitor lost" << endl; - monitor.Close(); - - rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); - killReceiveFromMon=1; - rt_mutex_release(&mutex_killReceiveFromMon); - - rt_mutex_acquire(&mutex_killVision, TM_INFINITE); - killVision=1; - rt_mutex_release(&mutex_killVision); - - rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); - killSendMon=1; - rt_mutex_release(&mutex_killSendMon); - - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=1; - rt_mutex_release(&mutex_killBattery); - - rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); - killDetectlostSupRob=1; - rt_mutex_release(&mutex_killDetectLostSupRob); - - rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); - acquireImage=0; - rt_mutex_release(&mutex_acquireImage); - - //exit(-1); - - - } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) { - cout << "Command Open Camera Received" << endl << flush; - //start task Vision - - } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { - cout << "Command Open Communication with Robot Received" << endl << flush; - rt_sem_v(&sem_openComRobot); - - } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { - cout << "Command Start Robot without Watchdog Received" << endl << flush; - rt_sem_v(&sem_startRobotWithoutWatchdog); - - } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { - cout << "Command Start Robot with Watchdog Received" << endl << flush; - //start task robot with watchdog - - } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) { - cout << "Command Search Arena Received" << endl << flush; - rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); - searchArena=1; - rt_mutex_release(&mutex_searchArena); - - } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) { - cout << "Command Get Root Position Received" << endl << flush; - rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); - getPosition=1; - rt_mutex_release(&mutex_getPosition); - - } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) { - cout << "Command Stop Getting Robot Position Received" << endl << flush; - rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); - getPosition=0; - rt_mutex_release(&mutex_getPosition); - - } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { - cout << "Command Confirm Arena Received" << endl << flush; - rt_mutex_acquire(&mutex_drawArena, TM_INFINITE); - drawArena=1; - rt_mutex_release(&mutex_drawArena); - - rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); - searchArena=0; - rt_mutex_release(&mutex_searchArena); - - rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); - acquireImage=1; - rt_mutex_release(&mutex_acquireImage); - - } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) { - cout << "Command Infirm Arena Received" << endl << flush; - rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); - searchArena=0; - rt_mutex_release(&mutex_searchArena); - - rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); - acquireImage=1; - rt_mutex_release(&mutex_acquireImage); - - } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || - msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || - msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || - msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) || - msgRcv->CompareID(MESSAGE_ROBOT_STOP)) { - - rt_mutex_acquire(&mutex_move, TM_INFINITE); - move = msgRcv->GetID(); - rt_mutex_release(&mutex_move); - } - delete(msgRcv); // must be deleted manually, no consumer - - //Update loop condition - rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); - killReceiveFromMonOk=killReceiveFromMon; - rt_mutex_release(&mutex_killReceiveFromMon); - - } - - rt_sem_v(&sem_allowStartReceive); -} - -/** - * @brief Thread opening communication with the robot. - */ -void Tasks::OpenComRobot(void *arg) { - int status; - int err; - - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /**************************************************************************************/ - /* The task openComRobot starts here */ - /**************************************************************************************/ - while (1) { - 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 << flush; - - 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. - *//** - * @brief Thread starting the communication with the robot. - */ - -void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - int killBattery=0; - /**************************************************************************************/ - /* The task startRobot starts here */ - /**************************************************************************************/ - Message* p_mess_answer_battery; - Message * msgSend; - rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE); - cout << "Start robot without watchdog ("; - - //Boolean to get the battery - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=0; - rt_mutex_release(&mutex_killBattery); - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(robot.StartWithoutWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; - cout << "Movement answer: " << msgSend->ToString() << endl << flush; - 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_task_set_periodic(NULL, TM_NOW, 500000000); - while (killBattery==0) { - rt_task_wait_period(NULL); - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - p_mess_answer_battery = robot.Write(robot.GetBattery()); - rt_mutex_release(&mutex_robot); - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Write(p_mess_answer_battery); - rt_mutex_release(&mutex_monitor); - //cout << endl << flush; - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=killBattery; - rt_mutex_release(&mutex_killBattery); - } - } -} - - -/** - * @brief Thread starting the communication with the robot. - *//** - * @brief Thread starting the communication with the robot. - */ - -void Tasks::StartRobotTaskWithWatchdog(void *arg) { - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /**************************************************************************************/ - /* The task startRobot starts here */ - /**************************************************************************************/ - Message* p_mess_answer_battery; - Message * msgSend; - int cpt=1; - int err; - rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); - cout << "Start robot with watchdog ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(robot.StartWithWD()); - rt_mutex_release(&mutex_robot); - - //boolean to ask battery - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=0; - rt_mutex_release(&mutex_killBattery); - - cout << msgSend->GetID(); - cout << ")" << endl; - - cout << "Movement answer: " << msgSend->ToString() << endl << flush; - 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_task_set_periodic(NULL, TM_NOW, 500000000); - while (1) { - cpt++; - if(cpt%2==0){ - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - robot.Write(robot.ReloadWD()); - rt_mutex_release(&mutex_robot); - } - rt_task_wait_period(NULL); - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - p_mess_answer_battery = robot.Write(robot.GetBattery()); - rt_mutex_release(&mutex_robot); - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Write(p_mess_answer_battery); - rt_mutex_release(&mutex_monitor); - cout << endl << flush; - } - } -} - - - - -/** - * @brief Thread handling control of the robot. - */ -void Tasks::MoveTask(void *arg) { - int rs; - int cpMove; - - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - /**************************************************************************************/ - /* The task starts here */ - /**************************************************************************************/ - rt_task_set_periodic(NULL, TM_NOW, 100000000); - - while (1) { - rt_task_wait_period(NULL); - 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; - - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - robot.Write(new Message((MessageID)cpMove)); - rt_mutex_release(&mutex_robot); - } - cout << endl << flush; - } -} - - -/** - * Write a message in a given queue - * @param queue Queue identifier - * @param msg Message to be stored - */ -void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) { - int err; - if ((err = rt_queue_write(queue, (const void *) &msg, sizeof ((const void *) &msg), Q_NORMAL)) < 0) { - cerr << "Write in queue failed: " << strerror(-err) << endl << flush; - throw std::runtime_error{"Error in write in queue"}; - } -} - -/** - * Read a message from a given queue, block if empty - * @param queue Queue identifier - * @return Message read - */ -Message *Tasks::ReadInQueue(RT_QUEUE *queue) { - int err; - Message *msg; - - if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) { - cout << "Read in queue failed: " << strerror(-err) << endl << flush; - throw std::runtime_error{"Error in read in queue"}; - }/** else { - cout << "@msg :" << msg << endl << flush; - } /**/ - - return msg; -} - diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h deleted file mode 100644 index 83c57c7..0000000 --- a/software/raspberry/superviseur-robot/tasks.h +++ /dev/null @@ -1,186 +0,0 @@ -/* - * Copyright (C) 2018 dimercur - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#ifndef __TASKS_H__ -#define __TASKS_H__ - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "messages.h" -#include "commonitor.h" -#include "comrobot.h" -#include "camera.h" -#include "img.h" - -using namespace std; - -class Tasks { -public: - /** - * @brief Initializes main structures (semaphores, tasks, mutex, etc.) - */ - void Init(); - - /** - * @brief Starts tasks - */ - void Run(); - - /** - * @brief Stops tasks - */ - void Stop(); - - /** - * @brief Suspends main thread - */ - void Join(); - -private: - /**********************************************************************/ - /* Shared data */ - /**********************************************************************/ - ComMonitor monitor; - ComRobot robot; - int robotStarted = 0; - int move = MESSAGE_ROBOT_STOP; - bool killBattery = 0; - bool killReceiveFromMon=0; - bool killVision=0; - bool searchArena=0; - bool killSendMon=0; - bool killDetectlostSupRob=0; - bool acquireImage=0; - bool getPosition=0; - bool drawArena=0; - - - - /* Tasks */ - /**********************************************************************/ - RT_TASK th_server; - RT_TASK th_sendToMon; - RT_TASK th_receiveFromMon; - RT_TASK th_openComRobot; - RT_TASK th_startRobotWithoutWatchdog; - RT_TASK th_startRobotWithWatchdog; - RT_TASK th_move; - - /**********************************************************************/ - /* Mutex */ - /**********************************************************************/ - RT_MUTEX mutex_monitor; - RT_MUTEX mutex_robot; - RT_MUTEX mutex_robotStarted; - RT_MUTEX mutex_move; - RT_MUTEX mutex_killBattery; - RT_MUTEX mutex_killReceiveFromMon; - RT_MUTEX mutex_killVision; - RT_MUTEX mutex_searchArena; - RT_MUTEX mutex_drawArena; - RT_MUTEX mutex_getPosition; - RT_MUTEX mutex_killSendMon; - RT_MUTEX mutex_killStartRobWD; - RT_MUTEX mutex_killDetectLostSupRob; - RT_MUTEX mutex_killStartRob; - RT_MUTEX mutex_acquireImage; - - /**********************************************************************/ - /* Semaphores */ - /**********************************************************************/ - RT_SEM sem_barrier; - RT_SEM sem_openComRobot; - RT_SEM sem_serverOk; - RT_SEM sem_startRobotWithoutWatchdog; - RT_SEM sem_startRobotWithWatchdog; - RT_SEM sem_allowStartReceive; - - /**********************************************************************/ - /* Message queues */ - /**********************************************************************/ - int MSG_QUEUE_SIZE; - RT_QUEUE q_messageToMon; - - /**********************************************************************/ - /* Tasks' functions */ - /**********************************************************************/ - /** - * @brief Thread handling server communication with the monitor. - */ - void ServerTask(void *arg); - - /** - * @brief Thread sending data to monitor. - */ - void SendToMonTask(void *arg); - - /** - * @brief Thread receiving data from monitor. - */ - void ReceiveFromMonTask(void *arg); - - /** - * @brief Thread opening communication with the robot. - */ - void OpenComRobot(void *arg); - - /** - * @brief Thread starting the communication with the robot without watchdog. - */ - void StartRobotTaskWithoutWatchdog(void *arg); - - /** - * @brief Thread starting the communication with the robot with watchdog. - */ - void StartRobotTaskWithWatchdog(void *arg); - - /** - * @brief Thread handling control of the robot. - */ - void MoveTask(void *arg); - - - - /**********************************************************************/ - /* Queue services */ - /**********************************************************************/ - /** - * Write a message in a given queue - * @param queue Queue identifier - * @param msg Message to be stored - */ - void WriteInQueue(RT_QUEUE *queue, Message *msg); - - /** - * Read a message from a given queue, block if empty - * @param queue Queue identifier - * @return Message read - */ - Message *ReadInQueue(RT_QUEUE *queue); - -}; - -#endif // __TASKS_H__ -