diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index d810a68..216ce59 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -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 @@ -155,37 +155,16 @@ void Tasks::Init() { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; 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; @@ -193,36 +172,36 @@ void Tasks::Init() { /* 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); } @@ -243,41 +222,43 @@ 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; 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,21 +295,27 @@ void Tasks::ServerTask(void *arg) { // Synchronization barrier (waiting that all tasks are started) rt_sem_p(&sem_barrier, TM_INFINITE); - /**************************************************************************************/ - /* The task server starts here */ - /**************************************************************************************/ - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - status = monitor.Open(SERVER_PORT); - rt_mutex_release(&mutex_monitor); + while(1){ + + //First time it's okay after it's managed by receiveFromMon + //rt_sem_p(&sem_restart, TM_INFINITE); - 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 { - "Unable to start server on port " + std::to_string(SERVER_PORT) - }; - monitor.AcceptClient(); // Wait the monitor client - cout << "Rock'n'Roll baby, client accepted!" << endl << flush; - rt_sem_broadcast(&sem_serverOk); + cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; + + if (status < 0) throw std::runtime_error { + "Unable to start server on port " + std::to_string(SERVER_PORT) + }; + 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) rt_sem_p(&sem_barrier, TM_INFINITE); - rt_sem_p(&sem_allowSendToMon, 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); + while(1){ + //rt_sem_p(&sem_restart, TM_INFINITE); - /**************************************************************************************/ - /* 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); - + //Initialize the loop condition rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); - kill_sendToMonOk=killSendMon; // The message is deleted with the Write - rt_mutex_release(&mutex_killSendMon); - + killSendMon=0; // The message is deleted with the Write + 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) 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){ - //Reinitialize control boolean - rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); - killReceiveFromMon=0; - 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_sem_p(&sem_restart,TM_INFINITE); + + //Reinitialize control boolean rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); - killReceiveFromMonOk=killReceiveFromMon; + killReceiveFromMon=0; 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; // 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); - 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) { - 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); + /**************************************************************************************/ + /* The task openComRobot starts here */ + /**************************************************************************************/ + 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) { + 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); } } - - //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; int cpt=1; 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 - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=0; - rt_mutex_release(&mutex_killBattery); + while(1){ - cout << msgSend->GetID(); - cout << ")" << endl; + rt_sem_p(&sem_restart, TM_INFINITE); - cout << "Movement answer: " << msgSend->ToString() << endl << flush; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + 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); - 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){ + //boolean to ask battery + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=0; + rt_mutex_release(&mutex_killBattery); + + cout << msgSend->GetID(); + cout << ")" << endl; + + 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); - robot.Write(robot.ReloadWD()); + 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; } - 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; 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); } /** diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index bb3c08b..9d14621 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -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 */