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
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TOPENCOMROBOT 50
#define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25
@ -156,73 +156,52 @@ void Tasks::Init() {
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)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
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;
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;
/**************************************************************************************/
/* Tasks creation */
/**************************************************************************************/
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);
}
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);
}
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);
}
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);
}
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);
}
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);
}
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);
}
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);
}
@ -244,40 +223,42 @@ void Tasks::Init() {
*/
void Tasks::Run() {
cout << "Coucou" << endl << flush;
rt_task_set_priority(NULL, T_LOPRIO);
int err;
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);
}
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);
}
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);
}
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);
}
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);
}
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);
}
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);
}
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);
}
@ -298,6 +279,9 @@ void Tasks::Stop() {
void Tasks::Join() {
cout << "Tasks synchronized" << endl << flush;
rt_sem_broadcast(&sem_barrier);
//Initialization for some specific sem: Allow to run the first time
rt_sem_broadcast(&sem_restart);
pause();
}
@ -311,6 +295,11 @@ void Tasks::ServerTask(void *arg) {
// Synchronization barrier (waiting that all tasks are started)
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 */
/**************************************************************************************/
@ -327,6 +316,7 @@ void Tasks::ServerTask(void *arg) {
cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
rt_sem_broadcast(&sem_serverOk);
}
}
/**
* @brief Thread sending data to monitor.
@ -339,7 +329,8 @@ void Tasks::SendToMonTask(void* arg) {
// Synchronization barrier (waiting that all tasks are starting)
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);
@ -364,9 +355,7 @@ void Tasks::SendToMonTask(void* arg) {
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)
rt_sem_p(&sem_barrier, TM_INFINITE);
//Wait twin task to die if not first round
rt_sem_p(&sem_allowStartReceive, TM_INFINITE);
while(1){
//rt_sem_p(&sem_restart,TM_INFINITE);
//Reinitialize control boolean
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
@ -428,15 +418,28 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_acquireImage);
//Restart all the process
//Tasks::Run();
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);
@ -510,9 +513,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
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)
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);
@ -549,10 +543,8 @@ void Tasks::OpenComRobot(void *arg) {
Message * msgSend;
if (status < 0) {
cout << "J'ai raté la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_NACK);
} else {
cout << "J'ai réussi la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
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
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);
}
}
//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;
int cpt=1;
int err;
while(1){
rt_sem_p(&sem_restart, TM_INFINITE);
rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
cout << "Start robot with watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@ -684,6 +672,7 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
}
}
}
}
@ -731,9 +720,6 @@ void Tasks::DetectLostSupRob(void *arg){
bool kill_detectLostSupRobOk=0;
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
rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
@ -780,8 +766,6 @@ void Tasks::DetectLostSupRob(void *arg){
kill_detectLostSupRobOk=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_startRobotWithoutWatchdog;
RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_allowStartReceive;
RT_SEM sem_allowStartDetectLostSupRob;
RT_SEM sem_detectLostSupRob;
RT_SEM sem_restart;
RT_SEM sem_allowSendToMon;
RT_SEM sem_allowOpenComRobot;
/**********************************************************************/
/* Message queues */