diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 169f37a..b0d7d8f 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -7,11 +7,9 @@ - file:/home/romainv/Documents/temps_Reel/real_time/software/raspberry/superviseur-robot/lib/messages.h - file:/home/romainv/Documents/temps_Reel/real_time/software/raspberry/superviseur-robot/tasks.cpp - file:/home/romainv/Documents/temps_Reel/real_time/software/raspberry/superviseur-robot/lib/img.cpp - file:/home/romainv/Documents/temps_Reel/real_time/software/raspberry/superviseur-robot/lib/img.h - file:/home/romainv/Documents/temps_Reel/real_time/software/raspberry/superviseur-robot/tasks.h + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/messages.h + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.h diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 29943d0..88ad24d 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -19,15 +19,15 @@ #include // Déclaration des priorités des taches -#define PRIORITY_TSERVER 10 -#define PRIORITY_TOPENCOMROBOT 50 -#define PRIORITY_TMOVE 20 -#define PRIORITY_TSENDTOMON 22 -#define PRIORITY_TRECEIVEFROMMON 50 -#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22 -#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22 +#define PRIORITY_TSERVER 50 //Important but only once per connection so high priority it doesn't interfer with other tasks +#define PRIORITY_TOPENCOMROBOT 45 //Important but only once per connection so high priority it doesn't matter +#define PRIORITY_TMOVE 20 //Lower priority because periodicity is high it will interfer with other tasks +#define PRIORITY_TSENDTOMON 25 //Second highest priority of the periofic tasks +#define PRIORITY_TRECEIVEFROMMON 30 //Highest priority of the periodic tasks +#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 40 //Important but only once per connection so high priority it doesn't interfer with other tasks +#define PRIORITY_TSTARTROBOTWITHWATCHDOG 41 //Same as start without WD #define PRIORITY_TCAMERA 21 -#define PRIORITY_DETECTLOSTSUPROB 21 +#define PRIORITY_DETECTLOSTSUPROB 26 //Important so high priority compared to other tasks #define PRIORITY_TVISION 26 /* @@ -157,11 +157,6 @@ void Tasks::Init() { exit(EXIT_FAILURE); } - if (err = rt_sem_create(&sem_restart, NULL, 0, S_FIFO)) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - cout << "Semaphores created successfully" << endl << flush; /**************************************************************************************/ @@ -225,8 +220,6 @@ void Tasks::Init() { * @brief Démarrage des tâches */ void Tasks::Run() { - - cout << "Coucou" << endl << flush; rt_task_set_priority(NULL, T_LOPRIO); int err; @@ -289,9 +282,6 @@ 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(); } @@ -304,7 +294,8 @@ void Tasks::ServerTask(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - while(1){ + while(1){ //Will be used to restart process + // Synchronization barrier (waiting that all tasks are started) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -364,8 +355,6 @@ void Tasks::SendToMonTask(void* arg) { rt_mutex_release(&mutex_killSendMon); } - - cout << "SendToMon Task dies" << endl << flush; } } @@ -382,7 +371,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - while(1){ + while(1){ //Will be used to restart process //Reinitialize control boolean killReceiveFromMonOk = 0; @@ -402,33 +391,40 @@ void Tasks::ReceiveFromMonTask(void *arg) { if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { cout << "Connection to monitor lost" << endl; - + + //Kill this task by exiting the main loop rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); killReceiveFromMon=1; rt_mutex_release(&mutex_killReceiveFromMon); - + + //Kill the vision taks by exiting the main loop rt_mutex_acquire(&mutex_killVision, TM_INFINITE); killVision=1; rt_mutex_release(&mutex_killVision); - + + //Kill the SendToMon taks by exiting the main loop rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); killSendMon=1; rt_mutex_release(&mutex_killSendMon); //Write fake message in queue to unblock Read function WriteInQueue(&q_messageToMon, new Message(MESSAGE_EMPTY)); - + + //Kill the different start robot tasks (with or without WD) by exiting their main loop rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); killBattery=1; rt_mutex_release(&mutex_killBattery); + //Kill the DetectLostSupRon taks by exiting the main loop rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); killDetectLostSupRob=1; rt_mutex_release(&mutex_killDetectLostSupRob); + //Update variable acquireImage to start or stop acquiring in Vision task rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); acquireImage=0; rt_mutex_release(&mutex_acquireImage); + //Block the move task rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); robotStarted=0; rt_mutex_release(&mutex_robotStarted); @@ -447,7 +443,6 @@ void Tasks::ReceiveFromMonTask(void *arg) { cout << "Close Robot Success" << endl << flush; } - //All closes rt_mutex_acquire(&mutex_monitor, TM_INFINITE); monitor.Close(); rt_mutex_release(&mutex_monitor); @@ -458,13 +453,10 @@ void Tasks::ReceiveFromMonTask(void *arg) { } 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; @@ -474,7 +466,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { cout << "Command Open Communication with Robot Received" << endl << flush; rt_sem_v(&sem_openComRobot); - } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) { + } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) { //Actually we never get this message cout << "Command Close Communication with Robot Received" << endl << flush; rt_mutex_acquire(&mutex_robot, TM_INFINITE); @@ -511,25 +503,25 @@ void Tasks::ReceiveFromMonTask(void *arg) { } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { cout << "Command Confirm Arena Received" << endl << flush; rt_mutex_acquire(&mutex_drawArena, TM_INFINITE); - drawArena=1; + drawArena=1; //Trigger the drawing of the arena in Vision task rt_mutex_release(&mutex_drawArena); rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); - searchArena=0; + searchArena=0; //Stop searching arena in Vision task rt_mutex_release(&mutex_searchArena); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); - acquireImage=1; + acquireImage=1; //Start acquiring again because we had to stop during the search of the arena in Vision Task 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; + searchArena=0; //We stop searching the arena in Vision Task rt_mutex_release(&mutex_searchArena); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); - acquireImage=1; + acquireImage=1; //Start acquiring again because we had to stop during the search of the arena in Vision Task rt_mutex_release(&mutex_acquireImage); } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || @@ -545,13 +537,10 @@ void Tasks::ReceiveFromMonTask(void *arg) { 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 << "ReceiveFromMon dies" << endl << flush; } } @@ -573,7 +562,7 @@ void Tasks::OpenComRobot(void *arg) { //PAS DE SOUCIS AU REDEMARAGE /**************************************************************************************/ /* The task openComRobot starts here */ /**************************************************************************************/ - while (1) { + while (1) { //Is used to restart process rt_sem_p(&sem_openComRobot, TM_INFINITE); cout << "Open serial com ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); @@ -733,7 +722,7 @@ void Tasks::MoveTask(void *arg) { rt_task_wait_period(NULL); cout << "Periodic movement update"; rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - rs = robotStarted; + rs = robotStarted; //Value is updated in ReceiveFromMon when needed rt_mutex_release(&mutex_robotStarted); if (rs == 1) { rt_mutex_acquire(&mutex_move, TM_INFINITE); @@ -760,7 +749,7 @@ void Tasks::DetectLostSupRob(void *arg){ //Period = 1s rt_task_set_periodic(NULL, TM_NOW, 1000000000); - while(1){ + while(1){//Used to restart process //Wait the Communication with the Robot to be Set rt_sem_p(&sem_detectLostSupRob, TM_INFINITE); @@ -791,9 +780,9 @@ void Tasks::DetectLostSupRob(void *arg){ cout << "Didn't Get Any Answer from Robot" << endl << flush; cpt++; - if(cpt==3){ + if(cpt==3){ //If 3 errors in the row - //acknowledge loss communication with robot + //acknowledge loss communication with robot I THINK YOU MADE A MISTAKE IN THE CONCEPTION DOCUMENT THIS MESSAGE DOESN'T WORK //WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST)); cout << "Restart Communication with Robot" << endl << flush; @@ -821,11 +810,11 @@ void Tasks::DetectLostSupRob(void *arg){ cout << "I got an Answer from Robot" << endl << flush; } + //Update loop variable rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); kill_detectLostSupRobOk=killDetectLostSupRob; rt_mutex_release(&mutex_killDetectLostSupRob); } - cout << "DetectLostSupRob dies"; } } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 43c73de..f5a23e5 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -120,7 +120,6 @@ private: RT_SEM sem_startRobotWithoutWatchdog; RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_detectLostSupRob; - RT_SEM sem_restart; RT_SEM sem_askBattery; /**********************************************************************/