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

separate com and robot management

Browse source
This commit is contained in:
Arnaud Vergnet 2021-03-17 10:30:02 +01:00
parent 636f009274
commit f78ec42f7b
2 changed files with 85 additions and 49 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -101,11 +101,15 @@ void Tasks::Init() {
cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl; cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if ((err = rt_sem_create(&sem_closeComRobot, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_closeComRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) { if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl; cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if ((err = rt_sem_create(&sem_stopRobot, nullptr, 0, S_FIFO))) { if ((err = rt_sem_create(&sem_stopRobot, nullptr, 0, S_PULSE))) {
cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl; cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
@ -139,6 +143,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_manageRobotCom, "th_manageRobotCom", 0, PRIORITY_TSTARTROBOT, 0))) {
cerr << "Error task create th_manageRobotCom: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if ((err = rt_task_create(&th_manageRobot, "th_manageRobot", 0, PRIORITY_TSTARTROBOT, 0))) { if ((err = rt_task_create(&th_manageRobot, "th_manageRobot", 0, PRIORITY_TSTARTROBOT, 0))) {
cerr << "Error task create th_manageRobot: " << strerror(-err) << endl; cerr << "Error task create th_manageRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -176,6 +184,10 @@ 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_manageRobotCom, reinterpret_cast<void (*)(void *)>(&Tasks::ManageRobotComTask), this)) {
cerr << "Error task start ManageRobotComTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_manageRobot, reinterpret_cast<void (*)(void *)>(&Tasks::ManageRobotTask), this)) { if (err = rt_task_start(&th_manageRobot, reinterpret_cast<void (*)(void *)>(&Tasks::ManageRobotTask), this)) {
cerr << "Error task start ManageRobotTask: " << strerror(-err) << endl; cerr << "Error task start ManageRobotTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -241,6 +253,7 @@ void Tasks::Join() {
// 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_sem_v(&sem_stopRobot);
rt_sem_v(&sem_closeComRobot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE); rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE);
monitorConnected = false; monitorConnected = false;
@ -351,6 +364,39 @@ void Tasks::Join() {
} }
} }
/**
* @brief Thread managing the communication with the robot.
*/
[[noreturn]] void Tasks::ManageRobotComTask(void *arg) {
int status;
cout << "Start " << __PRETTY_FUNCTION__ << endl;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
/* *************************************************************************************
* The task startRobot 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;
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_sem_p(&sem_closeComRobot, TM_INFINITE);
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Close();
rt_mutex_release(&mutex_robot);
}
}
}
/** /**
* @brief Thread starting the communication with the robot. * @brief Thread starting the communication with the robot.
*/ */
@ -365,56 +411,44 @@ void Tasks::Join() {
* *************************************************************************************/ * *************************************************************************************/
while (true) { while (true) {
Message *msgSend; Message *msgSend;
rt_sem_p(&sem_openComRobot, TM_INFINITE); rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Open serial com ("; rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
rt_mutex_acquire(&mutex_robot, TM_INFINITE); const WatchdogMode mode = watchdogMode;
status = robot.Open(); rt_mutex_release(&mutex_watchdogMode);
rt_mutex_release(&mutex_robot);
cout << status; if (mode == WITH_WATCHDOG) {
cout << ")" << endl; cout << "Start robot with watchdog (";
if (status < 0) { rt_mutex_acquire(&mutex_robot, TM_INFINITE);
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); // msgSend will be deleted by sendToMon msgSend = robot.Write(ComRobot::StartWithWD());
rt_mutex_release(&mutex_robot);
cout << msgSend->GetID();
cout << ")" << endl;
} else { } else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); // msgSend will be deleted by sendToMon cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
rt_sem_p(&sem_startRobot, TM_INFINITE); msgSend = robot.Write(ComRobot::StartWithoutWD());
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); rt_mutex_release(&mutex_robot);
const WatchdogMode mode = watchdogMode; cout << msgSend->GetID();
rt_mutex_release(&mutex_watchdogMode); cout << ")" << endl;
}
if (mode == WITH_WATCHDOG) {
cout << "Start robot with watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(ComRobot::StartWithWD());
rt_mutex_release(&mutex_robot);
cout << msgSend->GetID();
cout << ")" << endl;
} else {
cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(ComRobot::StartWithoutWD());
rt_mutex_release(&mutex_robot);
cout << msgSend->GetID();
cout << ")" << endl;
}
cout << "Movement answer: " << msgSend->ToString() << endl; cout << "Start answer: " << msgSend->ToString() << endl;
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1; robotStarted = 1;
rt_mutex_release(&mutex_robotStarted); cout << "robot started" << endl;
rt_sem_p(&sem_stopRobot, TM_INFINITE); rt_mutex_release(&mutex_robotStarted);
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_sem_p(&sem_stopRobot, TM_INFINITE);
robot.Write(new Message(MESSAGE_ROBOT_STOP)); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Close(); robot.Write(new Message(MESSAGE_ROBOT_STOP));
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 0; robotStarted = 0;
rt_mutex_release(&mutex_robotStarted); cout << "robot stopped" << endl;
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robotStarted);
} rt_mutex_release(&mutex_robot);
} }
} }
} }

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

@ -81,6 +81,7 @@ private:
RT_TASK th_receiveFromMon; RT_TASK th_receiveFromMon;
RT_TASK th_move; RT_TASK th_move;
RT_TASK th_sendBatteryLevel; RT_TASK th_sendBatteryLevel;
RT_TASK th_manageRobotCom;
RT_TASK th_manageRobot; RT_TASK th_manageRobot;
/* ********************************************************************* /* *********************************************************************
@ -99,6 +100,7 @@ private:
RT_SEM sem_barrier; RT_SEM sem_barrier;
RT_SEM sem_serverOk; RT_SEM sem_serverOk;
RT_SEM sem_openComRobot; RT_SEM sem_openComRobot;
RT_SEM sem_closeComRobot;
RT_SEM sem_startRobot; RT_SEM sem_startRobot;
RT_SEM sem_stopRobot; RT_SEM sem_stopRobot;
RT_SEM sem_stopServer; RT_SEM sem_stopServer;
@ -129,9 +131,9 @@ private:
[[noreturn]] void ReceiveFromMonTask(void *arg); [[noreturn]] void ReceiveFromMonTask(void *arg);
/** /**
* @brief Thread opening communication with the robot. * @brief Thread managing communication with the robot.
*/ */
[[noreturn]] void OpenComRobot(void *arg); [[noreturn]] void ManageRobotComTask(void *arg);
/** /**
* @brief Thread starting the communication with the robot. * @brief Thread starting the communication with the robot.