diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 1242b72..aef7ea3 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -6,12 +6,6 @@ - - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/messages.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/tasks.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/commonitor.h - + diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp new file mode 100644 index 0000000..e5cda8a --- /dev/null +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -0,0 +1,674 @@ +/* + * 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_allow_StartReceive, 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_allow_StartReceive); + + 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_allow_StartReceive, 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_allow_StartReceive); +} + +/** + * @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 new file mode 100644 index 0000000..d50d818 --- /dev/null +++ b/software/raspberry/superviseur-robot/tasks.h @@ -0,0 +1,186 @@ +/* + * 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_allow_StartReceive; + + /**********************************************************************/ + /* 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__ +