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

Add move watchdog

Browse source
This commit is contained in:
Arnaud Vergnet 2021-03-12 15:09:42 +01:00
parent 7d3372a443
commit c3555e26ba
2 changed files with 20 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -98,6 +98,10 @@ void Tasks::Init() {
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))) {
cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl; cout << "Semaphores created successfully" << endl;
/* ************************************************************************************* /* *************************************************************************************
@ -400,6 +404,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
[[noreturn]] void Tasks::MoveTask(void *arg) { [[noreturn]] void Tasks::MoveTask(void *arg) {
int rs; int rs;
int cpMove; int cpMove;
int counter = 0;
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)
@ -412,7 +417,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
while (true) { while (true) {
rt_task_wait_period(nullptr); rt_task_wait_period(nullptr);
cout << "Periodic movement update"; cout << "Periodic movement update" << endl;
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
@ -421,13 +426,23 @@ void Tasks::ReceiveFromMonTask(void *arg) {
cpMove = move; cpMove = move;
rt_mutex_release(&mutex_move); rt_mutex_release(&mutex_move);
cout << " move: " << cpMove; cout << "move: " << cpMove << endl;
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(new Message((MessageID) cpMove)); auto msgSend = robot.Write(new Message((MessageID) cpMove));
if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
counter = 0;
} else {
counter ++;
}
if (counter == 3) {
rt_sem_v(&sem_stopRobot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(new Message(MESSAGE_ANSWER_COM_ERROR));
rt_mutex_release(&mutex_monitor);
}
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
} }
cout << endl;
} }
} }

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

@ -91,6 +91,7 @@ private:
RT_SEM sem_openComRobot; RT_SEM sem_openComRobot;
RT_SEM sem_serverOk; RT_SEM sem_serverOk;
RT_SEM sem_startRobot; RT_SEM sem_startRobot;
RT_SEM sem_stopRobot;
/* ********************************************************************* /* *********************************************************************
* Message queues * Message queues