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;
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;
/* *************************************************************************************
@ -400,6 +404,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
[[noreturn]] void Tasks::MoveTask(void *arg) {
int rs;
int cpMove;
int counter = 0;
cout << "Start " << __PRETTY_FUNCTION__ << endl;
// Synchronization barrier (waiting that all tasks are starting)
@ -412,7 +417,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
while (true) {
rt_task_wait_period(nullptr);
cout << "Periodic movement update";
cout << "Periodic movement update" << endl;
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted;
rt_mutex_release(&mutex_robotStarted);
@ -421,13 +426,23 @@ void Tasks::ReceiveFromMonTask(void *arg) {
cpMove = move;
rt_mutex_release(&mutex_move);
cout << " move: " << cpMove;
cout << "move: " << cpMove << endl;
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);
}
cout << endl;
}
}

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

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