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

128
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
@ -156,73 +156,52 @@ void Tasks::Init() {
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;
/**************************************************************************************/ /**************************************************************************************/
/* 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);
} }
@ -244,40 +223,42 @@ void Tasks::Init() {
*/ */
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,6 +295,11 @@ 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){
//First time it's okay after it's managed by receiveFromMon
//rt_sem_p(&sem_restart, TM_INFINITE);
/**************************************************************************************/ /**************************************************************************************/
/* The task server starts here */ /* The task server starts here */
/**************************************************************************************/ /**************************************************************************************/
@ -326,6 +315,7 @@ void Tasks::ServerTask(void *arg) {
monitor.AcceptClient(); // Wait the monitor client monitor.AcceptClient(); // Wait the monitor client
cout << "Rock'n'Roll baby, client accepted!" << endl << flush; cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
rt_sem_broadcast(&sem_serverOk); rt_sem_broadcast(&sem_serverOk);
}
} }
/** /**
@ -339,7 +329,8 @@ 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 //Initialize the loop condition
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
@ -364,9 +355,7 @@ void Tasks::SendToMonTask(void* arg) {
rt_mutex_release(&mutex_killSendMon); rt_mutex_release(&mutex_killSendMon);
} }
}
cout << "Task SendToMon dies" << endl << flush;
rt_sem_v(&sem_allowSendToMon);
} }
/** /**
@ -380,8 +369,9 @@ 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);
//rt_sem_p(&sem_restart,TM_INFINITE);
//Reinitialize control boolean //Reinitialize control boolean
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
@ -428,15 +418,28 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_acquireImage); rt_mutex_release(&mutex_acquireImage);
//Restart all the process //Restart all the process
//Tasks::Run(); Tasks::Join();
rt_sem_v(&sem_restart);
//exit(-1); //exit(-1);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) { } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
cout << "Command Open Camera Received" << endl << flush; cout << "Command Open Camera Received" << endl << flush;
//start task Vision //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)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
cout << "Command Open Communication with Robot Received" << endl << flush; cout << "Command Open Communication with Robot Received" << endl << flush;
rt_sem_v(&sem_openComRobot); rt_sem_v(&sem_openComRobot);
@ -510,9 +513,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_killReceiveFromMon); rt_mutex_release(&mutex_killReceiveFromMon);
} }
}
cout << "Task ReceiveFromMon dies" << __PRETTY_FUNCTION__ << endl << flush;
rt_sem_v(&sem_allowStartReceive);
} }
/** /**
@ -527,16 +528,9 @@ void Tasks::OpenComRobot(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_allowOpenComRobot, TM_INFINITE);
rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
killOpenComRobot = 0;
rt_mutex_release(&mutex_killOpenComRobot);*/
/**************************************************************************************/ /**************************************************************************************/
/* The task openComRobot starts here */ /* The task openComRobot starts here */
/**************************************************************************************/ /**************************************************************************************/
//while (!killOpenComRobot) {
while (1) { while (1) {
rt_sem_p(&sem_openComRobot, TM_INFINITE); rt_sem_p(&sem_openComRobot, TM_INFINITE);
@ -549,10 +543,8 @@ void Tasks::OpenComRobot(void *arg) {
Message * msgSend; Message * msgSend;
if (status < 0) { if (status < 0) {
cout << "J'ai raté la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_NACK); msgSend = new Message(MESSAGE_ANSWER_NACK);
} else { } else {
cout << "J'ai réussi la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_ACK); msgSend = new Message(MESSAGE_ANSWER_ACK);
} }
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
@ -560,13 +552,7 @@ void Tasks::OpenComRobot(void *arg) {
//Trigger Detection of Communication Loss with Robot //Trigger Detection of Communication Loss with Robot
rt_sem_v(&sem_detectLostSupRob); 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,6 +626,11 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
Message * msgSend; Message * msgSend;
int cpt=1; int cpt=1;
int err; int err;
while(1){
rt_sem_p(&sem_restart, TM_INFINITE);
rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
cout << "Start robot with watchdog ("; cout << "Start robot with watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@ -683,6 +671,7 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
cout << endl << flush; 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 */