benistan/real_time
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Connection with Robot Impossible

Browse source
This commit is contained in:
Raphael Benistant 2020-03-29 09:08:54 +02:00
parent 07c5afc82a
commit 4826ac1aeb
2 changed files with 271 additions and 291 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -20,7 +20,7 @@
// Déclaration des priorités des taches // Déclaration des priorités des taches
#define PRIORITY_TSERVER 30 #define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20 #define PRIORITY_TOPENCOMROBOT 50
#define PRIORITY_TMOVE 20 #define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22 #define PRIORITY_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TRECEIVEFROMMON 25
@ -155,37 +155,16 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_allowStartDetectLostSupRob, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) { if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_sem_create(&sem_allowSendToMon, NULL, 0, S_FIFO)) { if (err = rt_sem_create(&sem_restart, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_sem_create(&sem_allowOpenComRobot, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
//Initialization for some specific sem: Allow to run the first time
rt_sem_v(&sem_allowStartReceive);
rt_sem_v(&sem_allowStartDetectLostSupRob);
rt_sem_v(&sem_restart);
rt_sem_v(&sem_allowOpenComRobot);
cout << "Semaphores created successfully" << endl << flush; cout << "Semaphores created successfully" << endl << flush;
@ -193,36 +172,36 @@ void Tasks::Init() {
/* Tasks creation */ /* Tasks creation */
/**************************************************************************************/ /**************************************************************************************/
if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) { if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create server: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) { if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create sendtoMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) { if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create receiveFromMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) { if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create openComRobot: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) { if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create startRobotWithoutWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) { if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create startRobotWithWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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: " << strerror(-err) << endl << flush; cerr << "Error task create move: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_create(&th_detectLostSupRob, "th_detectLostSupRob", 0, PRIORITY_DETECTLOSTSUPROB, 0)) { if (err = rt_task_create(&th_detectLostSupRob, "th_detectLostSupRob", 0, PRIORITY_DETECTLOSTSUPROB, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create detectLostSupRob: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
@ -243,41 +222,43 @@ void Tasks::Init() {
* @brief Démarrage des tâches * @brief Démarrage des tâches
*/ */
void Tasks::Run() { void Tasks::Run() {
cout << "Coucou" << endl << flush;
rt_task_set_priority(NULL, T_LOPRIO); rt_task_set_priority(NULL, T_LOPRIO);
int err; int err;
if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) { if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start server: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) { if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start sendToMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) { if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start receiveFromMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) { if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start openComRobot: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) { if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start startRobotWithoutWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) { if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start startRobotWithWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) { if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start move: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) { if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start detectLostSupRob: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
@ -298,6 +279,9 @@ void Tasks::Stop() {
void Tasks::Join() { void Tasks::Join() {
cout << "Tasks synchronized" << endl << flush; cout << "Tasks synchronized" << endl << flush;
rt_sem_broadcast(&sem_barrier); rt_sem_broadcast(&sem_barrier);
//Initialization for some specific sem: Allow to run the first time
rt_sem_broadcast(&sem_restart);
pause(); pause();
} }
@ -311,21 +295,27 @@ void Tasks::ServerTask(void *arg) {
// Synchronization barrier (waiting that all tasks are started) // Synchronization barrier (waiting that all tasks are started)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/ while(1){
/* The task server starts here */
/**************************************************************************************/ //First time it's okay after it's managed by receiveFromMon
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); //rt_sem_p(&sem_restart, TM_INFINITE);
status = monitor.Open(SERVER_PORT);
rt_mutex_release(&mutex_monitor);
cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; /**************************************************************************************/
/* The task server starts here */
/**************************************************************************************/
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
status = monitor.Open(SERVER_PORT);
rt_mutex_release(&mutex_monitor);
if (status < 0) throw std::runtime_error { cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
"Unable to start server on port " + std::to_string(SERVER_PORT)
}; if (status < 0) throw std::runtime_error {
monitor.AcceptClient(); // Wait the monitor client "Unable to start server on port " + std::to_string(SERVER_PORT)
cout << "Rock'n'Roll baby, client accepted!" << endl << flush; };
rt_sem_broadcast(&sem_serverOk); monitor.AcceptClient(); // Wait the monitor client
cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
rt_sem_broadcast(&sem_serverOk);
}
} }
/** /**
@ -339,34 +329,33 @@ void Tasks::SendToMonTask(void* arg) {
// 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);
rt_sem_p(&sem_allowSendToMon, TM_INFINITE); while(1){
//rt_sem_p(&sem_restart, TM_INFINITE);
//Initialize the loop condition
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
killSendMon=0; // The message is deleted with the Write
rt_mutex_release(&mutex_killSendMon);
/**************************************************************************************/ //Initialize the loop condition
/* The task sendToMon starts here */
/**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
while (!kill_sendToMonOk) {
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);
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
kill_sendToMonOk=killSendMon; // The message is deleted with the Write killSendMon=0; // The message is deleted with the Write
rt_mutex_release(&mutex_killSendMon); rt_mutex_release(&mutex_killSendMon);
/**************************************************************************************/
/* The task sendToMon starts here */
/**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
while (!kill_sendToMonOk) {
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);
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
kill_sendToMonOk=killSendMon; // The message is deleted with the Write
rt_mutex_release(&mutex_killSendMon);
}
} }
cout << "Task SendToMon dies" << endl << flush;
rt_sem_v(&sem_allowSendToMon);
} }
/** /**
@ -380,139 +369,151 @@ void Tasks::ReceiveFromMonTask(void *arg) {
// 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);
//Wait twin task to die if not first round while(1){
rt_sem_p(&sem_allowStartReceive, TM_INFINITE);
//Reinitialize control boolean //rt_sem_p(&sem_restart,TM_INFINITE);
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMon=0; //Reinitialize control boolean
rt_mutex_release(&mutex_killReceiveFromMon);
/**************************************************************************************/
/* The task receiveFromMon starts here */
/**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Received message from monitor activated" << endl << flush;
while (!killReceiveFromMonOk) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
delete(msgRcv);
cout << "Connection to monitor lost" << endl;
monitor.Close();
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMon=1;
rt_mutex_release(&mutex_killReceiveFromMon);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=1;
rt_mutex_release(&mutex_killVision);
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
killSendMon=1;
rt_mutex_release(&mutex_killSendMon);
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=1;
rt_mutex_release(&mutex_killBattery);
rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
killDetectLostSupRob=1;
rt_mutex_release(&mutex_killDetectLostSupRob);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=0;
rt_mutex_release(&mutex_acquireImage);
//Restart all the process
//Tasks::Run();
//exit(-1);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
cout << "Command Open Camera Received" << endl << flush;
//start task Vision
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
cout << "Command Open Communication with Robot Received" << endl << flush;
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
cout << "Command Start Robot without Watchdog Received" << endl << flush;
rt_sem_v(&sem_startRobotWithoutWatchdog);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
cout << "Command Start Robot with Watchdog Received" << endl << flush;
//start task robot with watchdog
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
cout << "Command Search Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=1;
rt_mutex_release(&mutex_searchArena);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
cout << "Command Get Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=1;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
cout << "Command Stop Getting Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
cout << "Command Confirm Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
drawArena=1;
rt_mutex_release(&mutex_drawArena);
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0;
rt_mutex_release(&mutex_searchArena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
cout << "Command Infirm Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0;
rt_mutex_release(&mutex_searchArena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
} 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); // must be deleted manually, no consumer
//Update loop condition
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMonOk=killReceiveFromMon; killReceiveFromMon=0;
rt_mutex_release(&mutex_killReceiveFromMon); rt_mutex_release(&mutex_killReceiveFromMon);
/**************************************************************************************/
/* The task receiveFromMon starts here */
/**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Received message from monitor activated" << endl << flush;
while (!killReceiveFromMonOk) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
delete(msgRcv);
cout << "Connection to monitor lost" << endl;
monitor.Close();
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMon=1;
rt_mutex_release(&mutex_killReceiveFromMon);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=1;
rt_mutex_release(&mutex_killVision);
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
killSendMon=1;
rt_mutex_release(&mutex_killSendMon);
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=1;
rt_mutex_release(&mutex_killBattery);
rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
killDetectLostSupRob=1;
rt_mutex_release(&mutex_killDetectLostSupRob);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=0;
rt_mutex_release(&mutex_acquireImage);
//Restart all the process
Tasks::Join();
rt_sem_v(&sem_restart);
//exit(-1);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
cout << "Command Open Camera Received" << endl << flush;
//start task Vision
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
cout << "Command Close Camera Received" << endl << flush;
//Trigger killVision
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=1;
rt_mutex_release(&mutex_killVision);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
cout << "Command Open Communication with Robot Received" << endl << flush;
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
cout << "Command Start Robot without Watchdog Received" << endl << flush;
rt_sem_v(&sem_startRobotWithoutWatchdog);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
cout << "Command Start Robot with Watchdog Received" << endl << flush;
//start task robot with watchdog
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
cout << "Command Search Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=1;
rt_mutex_release(&mutex_searchArena);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
cout << "Command Get Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=1;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
cout << "Command Stop Getting Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
cout << "Command Confirm Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
drawArena=1;
rt_mutex_release(&mutex_drawArena);
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0;
rt_mutex_release(&mutex_searchArena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
cout << "Command Infirm Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0;
rt_mutex_release(&mutex_searchArena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
} 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); // must be deleted manually, no consumer
//Update loop condition
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMonOk=killReceiveFromMon;
rt_mutex_release(&mutex_killReceiveFromMon);
}
} }
cout << "Task ReceiveFromMon dies" << __PRETTY_FUNCTION__ << endl << flush;
rt_sem_v(&sem_allowStartReceive);
} }
/** /**
@ -526,47 +527,32 @@ void Tasks::OpenComRobot(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// 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);
/*rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE);
rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
killOpenComRobot = 0;
rt_mutex_release(&mutex_killOpenComRobot);*/
/**************************************************************************************/
/* The task openComRobot starts here */
/**************************************************************************************/
//while (!killOpenComRobot) {
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) { /* The task openComRobot starts here */
cout << "J'ai raté la co" << endl << flush; /**************************************************************************************/
msgSend = new Message(MESSAGE_ANSWER_NACK); while (1) {
} else {
cout << "J'ai réussi la co" << endl << flush; rt_sem_p(&sem_openComRobot, TM_INFINITE);
msgSend = new Message(MESSAGE_ANSWER_ACK); 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
//Trigger Detection of Communication Loss with Robot
rt_sem_v(&sem_detectLostSupRob);
} }
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
//Trigger Detection of Communication Loss with Robot
rt_sem_v(&sem_detectLostSupRob);
/*rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
killOpenComRobotOk = killOpenComRobot;
rt_mutex_release(&mutex_killOpenComRobot);*/
}
rt_sem_v(&sem_allowOpenComRobot);
} }
/** /**
@ -619,9 +605,6 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
rt_mutex_release(&mutex_killBattery); rt_mutex_release(&mutex_killBattery);
} }
} }
//Trigger New Opening of the Communication with the Robot
//rt_sem_v(&sem_openComRobot);
} }
@ -643,44 +626,50 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
Message * msgSend; Message * msgSend;
int cpt=1; int cpt=1;
int err; int err;
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);
//boolean to ask battery while(1){
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=0;
rt_mutex_release(&mutex_killBattery);
cout << msgSend->GetID(); rt_sem_p(&sem_restart, TM_INFINITE);
cout << ")" << endl;
cout << "Movement answer: " << msgSend->ToString() << endl << flush; rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
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(robot.StartWithWD());
rt_mutex_release(&mutex_robot);
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { //boolean to ask battery
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); killBattery=0;
robotStarted = 1; rt_mutex_release(&mutex_killBattery);
rt_mutex_release(&mutex_robotStarted);
rt_task_set_periodic(NULL, TM_NOW, 500000000); cout << msgSend->GetID();
while (1) { cout << ")" << endl;
cpt++;
if(cpt%2==0){ 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 (1) {
cpt++;
if(cpt%2==0){
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(robot.ReloadWD());
rt_mutex_release(&mutex_robot);
}
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(robot.ReloadWD()); p_mess_answer_battery = robot.Write(robot.GetBattery());
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(p_mess_answer_battery);
rt_mutex_release(&mutex_monitor);
cout << endl << flush;
} }
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
p_mess_answer_battery = robot.Write(robot.GetBattery());
rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(p_mess_answer_battery);
rt_mutex_release(&mutex_monitor);
cout << endl << flush;
} }
} }
} }
@ -731,9 +720,6 @@ void Tasks::DetectLostSupRob(void *arg){
bool kill_detectLostSupRobOk=0; bool kill_detectLostSupRobOk=0;
Message* msgSend; Message* msgSend;
//garanties twin task is dead if applicable
rt_sem_p(&sem_allowStartDetectLostSupRob, TM_INFINITE);
//Wait the Communication with the Robot to be Set //Wait the Communication with the Robot to be Set
rt_sem_p(&sem_detectLostSupRob, TM_INFINITE); rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
@ -780,8 +766,6 @@ void Tasks::DetectLostSupRob(void *arg){
kill_detectLostSupRobOk=killDetectLostSupRob; kill_detectLostSupRobOk=killDetectLostSupRob;
rt_mutex_release(&mutex_killDetectLostSupRob); rt_mutex_release(&mutex_killDetectLostSupRob);
} }
rt_sem_v(&sem_allowStartDetectLostSupRob);
} }
/** /**

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

@ -119,12 +119,8 @@ private:
RT_SEM sem_serverOk; RT_SEM sem_serverOk;
RT_SEM sem_startRobotWithoutWatchdog; RT_SEM sem_startRobotWithoutWatchdog;
RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_allowStartReceive;
RT_SEM sem_allowStartDetectLostSupRob;
RT_SEM sem_detectLostSupRob; RT_SEM sem_detectLostSupRob;
RT_SEM sem_restart; RT_SEM sem_restart;
RT_SEM sem_allowSendToMon;
RT_SEM sem_allowOpenComRobot;
/**********************************************************************/ /**********************************************************************/
/* Message queues */ /* Message queues */