vergnet/very-dumb
1
1
Fork 0
Code Issues Pull requests Releases Wiki Activity

Add manage robot task

Browse source
This commit is contained in:
Arnaud Vergnet 2021-03-15 11:54:44 +01:00
parent 3a6ae4db19
commit 8f4c8ef661
2 changed files with 78 additions and 81 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

149
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -22,7 +22,6 @@ using namespace std;
// Déclaration des priorités des taches // Déclaration des priorités des taches
constexpr int PRIORITY_TSERVER = 30; constexpr int PRIORITY_TSERVER = 30;
constexpr int PRIORITY_TOPENCOMROBOT = 20;
constexpr int PRIORITY_TMOVE = 20; constexpr int PRIORITY_TMOVE = 20;
constexpr int PRIORITY_TSENDTOMON = 22; constexpr int PRIORITY_TSENDTOMON = 22;
constexpr int PRIORITY_TRECEIVEFROMMON = 25; constexpr int PRIORITY_TRECEIVEFROMMON = 25;
@ -65,6 +64,10 @@ void Tasks::Init() {
cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl; cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl;
exit(EXIT_FAILURE); 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))) { if ((err = rt_mutex_create(&mutex_robot, nullptr))) {
cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl; cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -90,10 +93,6 @@ void Tasks::Init() {
cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl; cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl;
exit(EXIT_FAILURE); 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))) { if ((err = rt_sem_create(&sem_serverOk, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl; cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -106,10 +105,6 @@ void Tasks::Init() {
cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl; cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); 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))) { if ((err = rt_sem_create(&sem_stopServer, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_stopServer: " << strerror(-err) << endl; cerr << "Error semaphore create sem_stopServer: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -131,14 +126,6 @@ void Tasks::Init() {
cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl; cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl;
exit(EXIT_FAILURE); 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))) { if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) {
cerr << "Error task create th_move: " << strerror(-err) << endl; cerr << "Error task create th_move: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -148,6 +135,10 @@ void Tasks::Init() {
cerr << "Error task create th_sendBatteryLevel: " << strerror(-err) << endl; cerr << "Error task create th_sendBatteryLevel: " << strerror(-err) << endl;
exit(EXIT_FAILURE); 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; cout << "Tasks created successfully" << endl;
@ -181,12 +172,8 @@ void Tasks::Run() {
cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl; cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_openComRobot, reinterpret_cast<void (*)(void *)>(&Tasks::OpenComRobot), this)) { if (err = rt_task_start(&th_manageRobot, reinterpret_cast<void (*)(void *)>(&Tasks::ManageRobotTask), this)) {
cerr << "Error task start OpenComRobot: " << strerror(-err) << endl; cerr << "Error task start ManageRobotTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobot, reinterpret_cast<void (*)(void *)>(&Tasks::StartRobotTask), this)) {
cerr << "Error task start StartRobotTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_move, reinterpret_cast<void (*)(void *)>(&Tasks::MoveTask), this)) { if (err = rt_task_start(&th_move, reinterpret_cast<void (*)(void *)>(&Tasks::MoveTask), this)) {
@ -242,12 +229,19 @@ void Tasks::Join() {
monitor.AcceptClient(); // Wait the monitor client monitor.AcceptClient(); // Wait the monitor client
cout << "Rock'n'Roll baby, client accepted!" << endl; 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); rt_sem_broadcast(&sem_serverOk);
// Wait for stopServer signal // Wait for stopServer signal
rt_sem_p(&sem_stopServer, TM_INFINITE); rt_sem_p(&sem_stopServer, TM_INFINITE);
rt_sem_v(&sem_stopRobot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE);
monitorConnected = false;
monitor.Close(); monitor.Close();
rt_mutex_release(&mutex_monitorConnected);
rt_mutex_release(&mutex_monitor); rt_mutex_release(&mutex_monitor);
} }
} }
@ -272,7 +266,11 @@ void Tasks::Join() {
msg = ReadInQueue(&q_messageToMon); msg = ReadInQueue(&q_messageToMon);
cout << "Send msg to mon: " << msg->ToString() << endl; cout << "Send msg to mon: " << msg->ToString() << endl;
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); 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); rt_mutex_release(&mutex_monitor);
} }
} }
@ -302,13 +300,11 @@ void Tasks::Join() {
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
cout << "Monitor connection lost! Stopping robot..." << endl; cout << "Monitor connection lost! Stopping robot..." << endl;
// rt_sem_v(&sem_stopCamera); // TODO // rt_sem_v(&sem_stopCamera); // TODO
rt_sem_v(&sem_stopRobot);
rt_sem_v(&sem_stopComRobot);
rt_sem_v(&sem_stopServer); rt_sem_v(&sem_stopServer);
lostConnection = true; lostConnection = true;
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { } 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)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITHOUT_WATCHDOG; watchdogMode = WITHOUT_WATCHDOG;
@ -320,6 +316,8 @@ void Tasks::Join() {
watchdogMode = WITH_WATCHDOG; watchdogMode = WITH_WATCHDOG;
rt_mutex_release(&mutex_watchdogMode); rt_mutex_release(&mutex_watchdogMode);
rt_sem_v(&sem_startRobot); 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) || } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || 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. * @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; cout << "Start " << __PRETTY_FUNCTION__ << endl;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
@ -396,25 +362,60 @@ void Tasks::Join() {
while (true) { while (true) {
Message *msgSend; Message *msgSend;
rt_sem_p(&sem_startRobot, TM_INFINITE); rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Start robot without watchdog ("; cout << "Open serial com (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(ComRobot::StartWithoutWD()); status = robot.Open();
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
cout << msgSend->GetID(); cout << status;
cout << ")" << endl; 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; if (mode == WITH_WATCHDOG) {
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon 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); cout << "Movement answer: " << msgSend->ToString() << endl;
robotStarted = 1; WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
rt_mutex_release(&mutex_robotStarted);
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) { [[noreturn]] void Tasks::SendBatteryLevel(void *arg) {
int rs; int rs;
cout << "Start " << __PRETTY_FUNCTION__ << endl; cout << "Start " << __PRETTY_FUNCTION__ << endl;
@ -487,9 +488,7 @@ void Tasks::Join() {
} }
if (counter == 3) { if (counter == 3) {
rt_sem_v(&sem_stopRobot); rt_sem_v(&sem_stopRobot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR));
monitor.Write(new Message(MESSAGE_ANSWER_COM_ERROR));
rt_mutex_release(&mutex_monitor);
} }
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
} }

10
software/raspberry/superviseur-robot/tasks.h
View file

@ -70,6 +70,7 @@ private:
ComRobot robot; ComRobot robot;
int robotStarted = 0; int robotStarted = 0;
int move = MESSAGE_ROBOT_STOP; int move = MESSAGE_ROBOT_STOP;
bool monitorConnected = false;
WatchdogMode watchdogMode = WITHOUT_WATCHDOG; WatchdogMode watchdogMode = WITHOUT_WATCHDOG;
/* ********************************************************************* /* *********************************************************************
@ -78,15 +79,15 @@ private:
RT_TASK th_server; RT_TASK th_server;
RT_TASK th_sendToMon; RT_TASK th_sendToMon;
RT_TASK th_receiveFromMon; RT_TASK th_receiveFromMon;
RT_TASK th_openComRobot;
RT_TASK th_startRobot;
RT_TASK th_move; RT_TASK th_move;
RT_TASK th_sendBatteryLevel; RT_TASK th_sendBatteryLevel;
RT_TASK th_manageRobot;
/* ********************************************************************* /* *********************************************************************
* Mutex * Mutex
* ********************************************************************/ * ********************************************************************/
RT_MUTEX mutex_monitor; RT_MUTEX mutex_monitor;
RT_MUTEX mutex_monitorConnected;
RT_MUTEX mutex_robot; RT_MUTEX mutex_robot;
RT_MUTEX mutex_robotStarted; RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move; RT_MUTEX mutex_move;
@ -96,11 +97,9 @@ private:
* Semaphores * Semaphores
* ********************************************************************/ * ********************************************************************/
RT_SEM sem_barrier; RT_SEM sem_barrier;
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk; RT_SEM sem_serverOk;
RT_SEM sem_startRobot; RT_SEM sem_startRobot;
RT_SEM sem_stopRobot; RT_SEM sem_stopRobot;
RT_SEM sem_stopComRobot;
RT_SEM sem_stopServer; RT_SEM sem_stopServer;
/* ********************************************************************* /* *********************************************************************
@ -136,7 +135,7 @@ private:
/** /**
* @brief Thread starting the communication with the robot. * @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. * @brief Thread handling control of the robot.
@ -166,7 +165,6 @@ private:
* @return Message read * @return Message read
*/ */
static Message *ReadInQueue(RT_QUEUE *queue); static Message *ReadInQueue(RT_QUEUE *queue);
}; };