diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp new file mode 100644 index 0000000..20cf3ab --- /dev/null +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -0,0 +1,655 @@ +/* + * 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 24 +#define PRIORITY_TSTARTROBOTWITHWATCHDOG 23 +#define PRIORITY_TCAMERA 21 +#define PRIORITY_TVISION 26 + +/* + * 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_killBattery, 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_acquireImage, 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); + } + 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_askBattery, NULL, 0, S_FIFO)) { + cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_sem_create(&sem_allowStartCaptureImage, NULL, 0, S_FIFO)) { + cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + 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); + } + if (err = rt_task_create(&th_askBattery, "th_askBattery", 0, PRIORITY_TMOVE, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_create(&th_vision, "th_vision", 0, PRIORITY_TVISION, 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); + } + if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + /* if (err = rt_task_start(&th_vision, (void(*)(void*)) & Tasks::Vision, 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; + + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + + /**************************************************************************************/ + /* The task receiveFromMon starts here */ + /**************************************************************************************/ + rt_sem_p(&sem_serverOk, TM_INFINITE); + cout << "Received message from monitor activated" << endl << flush; + + while (1) { + msgRcv = monitor.Read(); + cout << "Rcv <= " << msgRcv->ToString() << endl << flush; + + if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { + delete(msgRcv); + exit(-1); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { + rt_sem_v(&sem_openComRobot); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { + rt_sem_v(&sem_startRobotWithoutWatchdog); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { + rt_sem_v(&sem_startRobotWithWatchdog); + } 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); // mus be deleted manually, no consumer + } +} + + + + +/** + * @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. + */ +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 killBatteryOk=0; + Message * msgSend; + /**************************************************************************************/ + /* The task startRobot starts here */ + /**************************************************************************************/ + //Boolean to get the battery + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=0; + rt_mutex_release(&mutex_killBattery); + rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE); + cout << "Start robot without watchdog ("; + 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 (killBatteryOk==0) { + rt_task_wait_period(NULL); + rt_sem_v(&sem_askBattery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=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); + int killBatteryOk=0; + int cpt=1; + Message * msgSend; + /**************************************************************************************/ + /* The task startRobot starts here */ + /**************************************************************************************/ + //Boolean to get the battery + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=0; + rt_mutex_release(&mutex_killBattery); + 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); + 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 (killBatteryOk==0) { + cpt++; + if(cpt==2){ + robot.Write(robot.ReloadWD()); + cpt=0; + } + rt_task_wait_period(NULL); + rt_sem_v(&sem_askBattery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=killBattery; + rt_mutex_release(&mutex_killBattery); + } + } +} + + +/** + * @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; + } +} + + + +void Tasks::AskBattery(void *arg){ + Message* p_mess_answer_battery; + 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 */ + /**************************************************************************************/ + while(1){ + rt_sem_p(&sem_askBattery, TM_INFINITE); + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + p_mess_answer_battery = robot.Write(robot.GetBattery()); + rt_mutex_release(&mutex_robot); + WriteInQueue(&q_messageToMon, p_mess_answer_battery); + cout << endl << flush; + } +} + + +//This task works on a difficult principle that fully make it controllable through +// monitor, accessing booleans variables to set getPosition or searchArena + + + +void Tasks::Vision(void *arg){ + 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_askBattery, TM_INFINITE); + int killVisionOk=0; + Camera camera; + int acquireImageOk=1; + int searchArenaOk=0; + int drawArenaOk=0; + int getPositionOk=0; + Arena arena; + list positionList; + list::iterator it; + string strPos; + MessagePosition* msgPos; + Message* msgPosMsg; + MessageImg* msgImg; + Message* msgImgMsg; + msgPos->SetID(MESSAGE_CAM_POSITION); + msgImg->SetID(MESSAGE_CAM_IMAGE); + rt_mutex_acquire(&mutex_killVision, TM_INFINITE); + killVision=0; + rt_mutex_release(&mutex_killVision); + camera.Open(); + while(killVisionOk==0){ + while(acquireImageOk==1){ + Img img=camera.Grab(); + if(searchArenaOk==1){ + rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); + acquireImage=0; + rt_mutex_release(&mutex_acquireImage); + acquireImageOk=acquireImage; + arena=img.SearchArena(); + img.DrawArena(arena); + } + if(drawArenaOk==1){ + // img.DrawArena(arena); + rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); + acquireImage=1; + rt_mutex_release(&mutex_acquireImage); + acquireImageOk=acquireImage; + } + if(getPositionOk==1){ + //On démarre la recherche du robot dans la zone définie par l'arène + positionList=img.SearchRobot(arena); + //Définitition et assignation de l'itérateur de parcrous de la liste positionList + it=positionList.begin(); + //Définition d'un messagePosition qui va contenir l'information (x,y) + msgPos->SetPosition(*it); + //Transformation en message classique + msgPosMsg=msgPos->Copy(); + //Envoi + WriteInQueue(&q_messageToMon,msgPos); + //Dessis du robot sur l'image + img.DrawRobot(*it); + } + //Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas) + msgImg->SetImage(&img); + //Transformation en message classique + msgImgMsg=msgImg->Copy(); + //Envoi + WriteInQueue(&q_messageToMon,msgImg); + } + } + rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); + searchArena=0; + rt_mutex_release(&mutex_searchArena); + searchArenaOk=1; + rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); + getPosition=0; + rt_mutex_release(&mutex_getPosition); + getPositionOk=1; +} + + + + + +/** + * 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..32de3c9 --- /dev/null +++ b/software/raspberry/superviseur-robot/tasks.h @@ -0,0 +1,188 @@ +/* + * 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; + RT_TASK th_vision; + RT_TASK th_askBattery; + + /**********************************************************************/ + /* Mutex */ + /**********************************************************************/ + RT_MUTEX mutex_monitor; + RT_MUTEX mutex_robot; + RT_MUTEX mutex_robotStarted; + RT_MUTEX mutex_move; + RT_MUTEX mutex_killBattery; + RT_MUTEX mutex_killVision; + RT_MUTEX mutex_acquireImage; + RT_MUTEX mutex_searchArena; + RT_MUTEX mutex_drawArena; + RT_MUTEX mutex_getPosition; + + /**********************************************************************/ + /* Semaphores */ + /**********************************************************************/ + RT_SEM sem_barrier; + RT_SEM sem_openComRobot; + RT_SEM sem_serverOk; + RT_SEM sem_startRobotWithoutWatchdog; + RT_SEM sem_startRobotWithWatchdog; + RT_SEM sem_askBattery; + RT_SEM sem_allowStartCaptureImage; + + /**********************************************************************/ + /* 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. + */ + 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); + + + void AskBattery(void *arg); + + + void Vision(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__ +