diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp index cbc618a..cd1c2f8 100644 --- a/software/raspberry/superviseur-robot/lib/comrobot.cpp +++ b/software/raspberry/superviseur-robot/lib/comrobot.cpp @@ -131,7 +131,11 @@ int ComRobot::Open(string usart) { * @return Success if above 0, failure if below 0 */ int ComRobot::Close() { +#ifdef __SIMULATION__ + return close(sock); +#elif return close(fd); +#endif } /** @@ -155,17 +159,22 @@ Message *ComRobot::Write(Message* msg) { char buffer[1024] = {0}; cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush; - send(sock, s.c_str(), s.length(), 0); + send(sock, s.c_str(), s.length(), MSG_NOSIGNAL); int valread = read(sock, buffer, 1024); - if (valread < 0) { + + if (valread == 0) { + cout << "The communication is out of order" << endl; + msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR); + } else if (valread < 0) { + cout << "Timeout" << endl; msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT); } else { string s(&buffer[0], valread); msgAnswer = StringToMessage(s); - //msgAnswer = new Message(MESSAGE_ANSWER_ACK); + cout << "Response: " << buffer << ", id: " << msgAnswer->GetID() << endl; } - cout << "response: " << buffer << ", id: " << msgAnswer->GetID() << endl; + #else AddChecksum(s); diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index ae5b94d..f698b47 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -19,17 +19,17 @@ #include // Déclaration des priorités des taches -#define PRIORITY_TSERVER 30 -#define PRIORITY_TOPENCOMROBOT 20 +#define PRIORITY_TSERVER 10 +#define PRIORITY_TOPENCOMROBOT 50 #define PRIORITY_TMOVE 20 #define PRIORITY_TSENDTOMON 22 -#define PRIORITY_TRECEIVEFROMMON 25 +#define PRIORITY_TRECEIVEFROMMON 50 #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 22 #define PRIORITY_TCAMERA 21 +#define PRIORITY_DETECTLOSTSUPROB 21 #define PRIORITY_TVISION 26 - /* * Some remarks: * 1- This program is mostly a template. It shows you how to create tasks, semaphore @@ -146,21 +146,21 @@ void Tasks::Init() { 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_askBattery, NULL, 0, S_FIFO)) { 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_allowStartCapture, 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); + 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; @@ -168,31 +168,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 detectLostSupRob: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_askBattery, "th_askBattery", 0, PRIORITY_TMOVE, 0)) { @@ -220,46 +225,55 @@ void Tasks::Init() { * @brief Démarrage des tâches */ void Tasks::Run() { - rt_task_set_priority(NULL, T_LOPRIO); - int err; + + cout << "Coucou" << endl << flush; - if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) { - cerr << "Error task start: " << 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; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) { - cerr << "Error task start: " << 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; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) { - cerr << "Error task start: " << 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; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - if (err = rt_task_start(&th_vision, (void(*)(void*)) & Tasks::Vision, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - cout << "Tasks launched" << 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 server: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) { + 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 receiveFromMon: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) { + 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 startRobotWithoutWatchdog: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) { + 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 move: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + + if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) { + cerr << "Error task start detectLostSupRob: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + + if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_vision, (void(*)(void*)) & Tasks::Vision, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + cout << "Tasks launched" << endl << flush; } /** @@ -275,6 +289,10 @@ 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(); } @@ -285,24 +303,27 @@ void Tasks::ServerTask(void *arg) { int status; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are started) - rt_sem_p(&sem_barrier, TM_INFINITE); + + while(1){ + // 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); + /**************************************************************************************/ + /* The task server starts here */ + /**************************************************************************************/ + rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + status = monitor.Open(SERVER_PORT); + rt_mutex_release(&mutex_monitor); - cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; + 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); + 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); + } } /** @@ -310,23 +331,41 @@ void Tasks::ServerTask(void *arg) { */ void Tasks::SendToMonTask(void* arg) { Message *msg; + bool kill_sendToMonOk=0; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); + + while(1){ + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + - /**************************************************************************************/ - /* The task sendToMon starts here */ - /**************************************************************************************/ - rt_sem_p(&sem_serverOk, TM_INFINITE); + /**************************************************************************************/ + /* The task sendToMon starts here */ + /**************************************************************************************/ + rt_sem_p(&sem_serverOk, TM_INFINITE); + + //Initialize the loop condition + kill_sendToMonOk=0; + rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); + killSendMon=0; // The message is deleted with the Write + rt_mutex_release(&mutex_killSendMon); - while (1) { - 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); + 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 << "SendToMon Task dies" << endl << flush; } } @@ -336,137 +375,184 @@ void Tasks::SendToMonTask(void* arg) { void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; bool killReceiveFromMonOk=0; + int status; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // 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); - - //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; + while(1){ - 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); - - //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; - rt_sem_v(&sem_startRobotWithWatchdog); - - } 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 Root 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 + //Reinitialize control boolean + killReceiveFromMonOk = 0; 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)) { + cout << "Connection to monitor lost" << endl; + + 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); + //Write fake message in queue to unblock Read function + WriteInQueue(&q_messageToMon, new Message(MESSAGE_EMPTY)); + + 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); + + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted=0; + rt_mutex_release(&mutex_robotStarted); + + //Wait every task to die + sleep(1); + + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + status=robot.Close(); + rt_mutex_release(&mutex_robot); + + if(status<0){ + cout << "Close Robot Fail" << endl << flush; + } + else{ + cout << "Close Robot Success" << endl << flush; + } + + //All closes + rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + monitor.Close(); + rt_mutex_release(&mutex_monitor); + + //Release restarted tasks + rt_sem_broadcast(&sem_barrier); + + + } 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_COM_CLOSE)) { + cout << "Command Close Communication with Robot Received" << endl << flush; + + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + status=robot.Close(); + rt_mutex_release(&mutex_robot); + + + } 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; + rt_sem_v(&sem_startRobotWithWatchdog); + + } 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 << "ReceiveFromMon dies" << endl << flush; } - - rt_sem_v(&sem_allowStartReceive); } @@ -475,34 +561,39 @@ void Tasks::ReceiveFromMonTask(void *arg) { /** * @brief Thread opening communication with the robot. */ -void Tasks::OpenComRobot(void *arg) { +void Tasks::OpenComRobot(void *arg) { //PAS DE SOUCIS AU REDEMARAGE int status; int err; + //bool killOpenComRobotOk = 0; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - - /**************************************************************************************/ - /* 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); + /**************************************************************************************/ + /* 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 - } } @@ -516,39 +607,42 @@ void Tasks::OpenComRobot(void *arg) { * @brief Thread starting the communication with the robot. */ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); + int killBatteryOk=0; Message * msgSend; /**************************************************************************************/ /* The task startRobot starts here */ /**************************************************************************************/ - //Boolean to get the battery - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBatteryOk=0; - rt_mutex_release(&mutex_killBattery); - rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE); - cout << "Start robot without watchdog ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(robot.StartWithoutWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; - cout << "Movement answer: " << msgSend->ToString() << endl << flush; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + while(1){ + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + //Boolean to get the battery + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=0; + rt_mutex_release(&mutex_killBattery); + rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE); + cout << "Start robot without watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(robot.StartWithoutWD()); + rt_mutex_release(&mutex_robot); + 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 (killBatteryOk==0) { - rt_task_wait_period(NULL); - rt_sem_v(&sem_askBattery); - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBatteryOk=killBattery; - rt_mutex_release(&mutex_killBattery); + 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 (killBatteryOk==0) { + rt_task_wait_period(NULL); + rt_sem_v(&sem_askBattery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=killBattery; + rt_mutex_release(&mutex_killBattery); + } } } } @@ -560,47 +654,58 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { * @brief Thread starting the communication with the robot. */ void Tasks::StartRobotTaskWithWatchdog(void *arg) { - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); + int killBatteryOk=0; int cpt=1; Message * msgSend; - /**************************************************************************************/ - /* The task startRobot starts here */ - /**************************************************************************************/ - //Boolean to get the battery - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=0; - rt_mutex_release(&mutex_killBattery); - 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); - cout << msgSend->GetID(); - cout << ")" << endl; - cout << "Movement answer: " << msgSend->ToString() << endl << flush; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + int err; + + while(1){ + + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + /**************************************************************************************/ + /* The task startRobot starts here */ + /**************************************************************************************/ - 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 (killBatteryOk==0) { - cpt++; - if(cpt==2){ - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - robot.Write(robot.ReloadWD()); - rt_mutex_release(&mutex_robot); - cpt=0; + + //Boolean to get the battery + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=0; + rt_mutex_release(&mutex_killBattery); + + 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); + + 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 (killBatteryOk==0) { + cpt++; + if(cpt==2){ + cpt=0; + } + rt_task_wait_period(NULL); + rt_sem_v(&sem_askBattery); + + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=killBattery; + rt_mutex_release(&mutex_killBattery); } - rt_task_wait_period(NULL); - rt_sem_v(&sem_askBattery); - rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBatteryOk=killBattery; - rt_mutex_release(&mutex_killBattery); } } } @@ -644,6 +749,86 @@ void Tasks::MoveTask(void *arg) { } } + +void Tasks::DetectLostSupRob(void *arg){ + //counter to detect loss of communication + int cpt=0; + bool kill_detectLostSupRobOk=0; + Message* msgSend; + + //Period = 1s + rt_task_set_periodic(NULL, TM_NOW, 1000000000); + + while(1){ + + //Wait the Communication with the Robot to be Set + rt_sem_p(&sem_detectLostSupRob, TM_INFINITE); + cout << "Start DetectLostSupRob" << endl << flush; + + //Initialize counter to detect loss + cpt = 0; + + //Initialize the variable for the loop condition + kill_detectLostSupRobOk = 0; + rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); + 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){ + + //acknowledge loss communication with robot + //WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST)); + + cout << "Restart Communication with Robot" << endl << flush; + //Trigger Kill of DetectLostSupRob + rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); + killDetectLostSupRob=1; + rt_mutex_release(&mutex_killDetectLostSupRob); + + //Trigger Kill of the Battery acquisition and therefore Robot with or without WD + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=1; + rt_mutex_release(&mutex_killBattery); + + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted=0; + rt_mutex_release(&mutex_robotStarted); + + rt_sem_v(&sem_openComRobot); + + + } + } + + 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"; + } +} + + void Tasks::AskBattery(void *arg){ Message* p_mess_answer_battery; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index c01e5c2..43c73de 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -71,10 +71,11 @@ private: bool killVision=0; bool searchArena=0; bool killSendMon=0; - bool killDetectlostSupRob=0; + bool killDetectLostSupRob=0; bool acquireImage=0; bool getPosition=0; bool drawArena=0; + bool killOpenComRobot=0; /* Tasks */ @@ -86,6 +87,7 @@ private: RT_TASK th_startRobotWithoutWatchdog; RT_TASK th_startRobotWithWatchdog; RT_TASK th_move; + RT_TASK th_detectLostSupRob; RT_TASK th_vision; RT_TASK th_askBattery; @@ -107,6 +109,8 @@ private: RT_MUTEX mutex_killDetectLostSupRob; RT_MUTEX mutex_killStartRob; RT_MUTEX mutex_acquireImage; + + RT_MUTEX mutex_killOpenComRobot; /**********************************************************************/ /* Semaphores */ /**********************************************************************/ @@ -115,9 +119,9 @@ private: RT_SEM sem_serverOk; RT_SEM sem_startRobotWithoutWatchdog; RT_SEM sem_startRobotWithWatchdog; - RT_SEM sem_allowStartReceive; + RT_SEM sem_detectLostSupRob; + RT_SEM sem_restart; RT_SEM sem_askBattery; - RT_SEM sem_allowStartCapture; /**********************************************************************/ /* Message queues */ @@ -163,6 +167,10 @@ private: */ void MoveTask(void *arg); + /** + * @brief Thread handling the loss of the communication between the robot and th supervisor. + */ + void DetectLostSupRob(void *arg); void AskBattery(void *arg); @@ -170,7 +178,6 @@ private: void Vision(void *arg); - /**********************************************************************/ /* Queue services */ /**********************************************************************/