diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 64dfa74..30bbf1d 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -731,53 +731,61 @@ void Tasks::DetectLostSupRob(void *arg){ bool kill_detectLostSupRobOk=0; Message* msgSend; - //Wait the Communication with the Robot to be Set - rt_sem_p(&sem_detectLostSupRob, TM_INFINITE); - - //Initialize the variable for the loop condition - rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); - killDetectLostSupRob=0; - rt_mutex_release(&mutex_killDetectLostSupRob); - //Period = 1s rt_task_set_periodic(NULL, TM_NOW, 1000000000); + + while(1){ - while(!kill_detectLostSupRobOk){ - rt_task_wait_period(NULL); - - //Test Communication with Robot - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(robot.Ping()); - rt_mutex_release(&mutex_robot); - - if(msgSend->GetID() == MESSAGE_ANSWER_COM_ERROR || msgSend->GetID() == MESSAGE_ANSWER_ROBOT_TIMEOUT){ - - cout << "Didn't Get Any Answer from Robot" << endl << flush; - cpt++; - - if(cpt==3){ - //Trigger Kill of DetectLostSupRob - rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); - killDetectLostSupRob=1; - rt_mutex_release(&mutex_killDetectLostSupRob); - - //Trigger Kill of the Battery acquisition - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=1; - rt_mutex_release(&mutex_killBattery); - - } - } - - else{ - cout << "I got an Answer from Robot" << endl << flush; - } - + //Wait the Communication with the Robot to be Set + rt_sem_p(&sem_detectLostSupRob, TM_INFINITE); + cout << "Start DetectLostSupRob" << endl << flush; + + kill_detectLostSupRobOk = 0; + //Initialize the variable for the loop condition rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); - kill_detectLostSupRobOk=killDetectLostSupRob; - rt_mutex_release(&mutex_killDetectLostSupRob); + killDetectLostSupRob = 0; + rt_mutex_release(&mutex_killDetectLostSupRob); + + + while(!kill_detectLostSupRobOk){ + rt_task_wait_period(NULL); + + //Test Communication with Robot + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(robot.Ping()); + rt_mutex_release(&mutex_robot); + cout << "J'écris un message" << endl << flush; + + + if(msgSend->GetID() == MESSAGE_ANSWER_COM_ERROR || msgSend->GetID() == MESSAGE_ANSWER_ROBOT_TIMEOUT){ + + cout << "Didn't Get Any Answer from Robot" << endl << flush; + cpt++; + + if(cpt==3){ + //Trigger Kill of DetectLostSupRob + rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); + killDetectLostSupRob=1; + rt_mutex_release(&mutex_killDetectLostSupRob); + + //Trigger Kill of the Battery acquisition + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=1; + rt_mutex_release(&mutex_killBattery); + + } + } + + else{ + cout << "I got an Answer from Robot" << endl << flush; + } + + rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); + kill_detectLostSupRobOk=killDetectLostSupRob; + rt_mutex_release(&mutex_killDetectLostSupRob); + } + cout << "DetectLostSupRob dies"; } - cout << "DetectLostSupRob dies"; } /**