From 7be6073378f1b6aff36aa3fc52ab955bd49385d6 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sat, 28 Mar 2020 16:04:32 +0100 Subject: [PATCH 01/20] Create DetectLostSupRob --- .../raspberry/superviseur-robot/tasks.cpp | 84 ++++++++++++++++++- software/raspberry/superviseur-robot/tasks.h | 11 ++- 2 files changed, 92 insertions(+), 3 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index f4acecf..c812d0e 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -27,6 +27,7 @@ #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 22 #define PRIORITY_TCAMERA 21 +#define PRIORITY_DETECTLOSTSUPROB 21 /* @@ -155,9 +156,19 @@ void Tasks::Init() { 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); + } //Initialization for some specific sem: Allow to run the first time rt_sem_v(&sem_allowStartReceive); + rt_sem_v(&sem_allowStartDetectLostSupRob); cout << "Semaphores created successfully" << endl << flush; @@ -193,6 +204,11 @@ void Tasks::Init() { exit(EXIT_FAILURE); } + if (err = rt_task_create(&th_detectLostSupRob, "th_detectLostSupRob", 0, PRIORITY_DETECTLOSTSUPROB, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + cout << "Tasks created successfully" << endl << flush; /**************************************************************************************/ @@ -242,6 +258,11 @@ void Tasks::Run() { exit(EXIT_FAILURE); } + if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + cout << "Tasks launched" << endl << flush; } @@ -337,6 +358,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { /**************************************************************************************/ 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; @@ -363,7 +385,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_release(&mutex_killBattery); rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); - killDetectlostSupRob=1; + killDetectLostSupRob=1; rt_mutex_release(&mutex_killDetectLostSupRob); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); @@ -483,6 +505,9 @@ void Tasks::OpenComRobot(void *arg) { 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); } } @@ -536,6 +561,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { rt_mutex_release(&mutex_killBattery); } } + + //Trigger New Opening of the Communication with the Robot + //rt_sem_v(&sem_openComRobot); } @@ -639,6 +667,60 @@ 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; + + //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); + + //Initialize the variable for the loop condition + rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); + killDetectLostSupRob=0; + rt_mutex_release(&mutex_killDetectLostSupRob); + + while(!kill_detectLostSupRobOk){ + + //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=0; + rt_mutex_release(&mutex_killDetectLostSupRob); + + //Trigger Kill of the Battery acquisition + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=0; + 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); + } + + rt_sem_v(&sem_allowStartDetectLostSupRob); +} /** * Write a message in a given queue diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 83c57c7..21d3786 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -71,7 +71,7 @@ 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; @@ -87,6 +87,8 @@ private: RT_TASK th_startRobotWithoutWatchdog; RT_TASK th_startRobotWithWatchdog; RT_TASK th_move; + RT_TASK th_detectLostSupRob; + /**********************************************************************/ /* Mutex */ @@ -116,6 +118,8 @@ private: RT_SEM sem_startRobotWithoutWatchdog; RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_allowStartReceive; + RT_SEM sem_allowStartDetectLostSupRob; + RT_SEM sem_DetectLostSupRob; /**********************************************************************/ /* Message queues */ @@ -161,7 +165,10 @@ private: */ void MoveTask(void *arg); - + /** + * @brief Thread handling the loss of the communication between the robot and th supervisor. + */ + void DetectLostSupRob(void *arg); /**********************************************************************/ /* Queue services */ From 67f843142d44f777a96a425403e627aa6bca92b4 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sat, 28 Mar 2020 17:19:31 +0100 Subject: [PATCH 02/20] Modified detectLostSupRob --- .../raspberry/superviseur-robot/tasks.cpp | 157 +++++++++++++----- software/raspberry/superviseur-robot/tasks.h | 7 +- 2 files changed, 118 insertions(+), 46 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index c812d0e..a3c726f 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -114,7 +114,12 @@ void Tasks::Init() { if (err = rt_mutex_create(&mutex_acquireImage, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); - } + } + + if (err = rt_mutex_create(&mutex_killOpenComRobot, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } @@ -161,14 +166,31 @@ void Tasks::Init() { exit(EXIT_FAILURE); } - if (err = rt_sem_create(&sem_DetectLostSupRob, NULL, 0, S_FIFO)) { + 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_restart, 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)) { 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; @@ -226,44 +248,49 @@ void Tasks::Init() { * @brief Démarrage des tâches */ void Tasks::Run() { - 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; - 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_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } - cout << "Tasks launched" << endl << flush; + while(1){ + rt_sem_p(&sem_restart, TM_INFINITE); + + 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; + 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_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + + cout << "Tasks launched" << endl << flush; + } } /** @@ -314,24 +341,40 @@ 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); + + 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); /**************************************************************************************/ /* The task sendToMon starts here */ /**************************************************************************************/ rt_sem_p(&sem_serverOk, TM_INFINITE); - while (1) { + 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); } /** @@ -390,7 +433,10 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); acquireImage=0; - rt_mutex_release(&mutex_acquireImage); + rt_mutex_release(&mutex_acquireImage); + + //Restart all the process + rt_sem_v(&sem_restart); //exit(-1); @@ -418,7 +464,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_release(&mutex_searchArena); } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) { - cout << "Command Get Root Position Received" << endl << flush; + cout << "Command Get Robot Position Received" << endl << flush; rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); getPosition=1; rt_mutex_release(&mutex_getPosition); @@ -472,6 +518,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { } + cout << "Task ReceiveFromMon dies" << __PRETTY_FUNCTION__ << endl << flush; rt_sem_v(&sem_allowStartReceive); } @@ -481,15 +528,24 @@ void Tasks::ReceiveFromMonTask(void *arg) { void Tasks::OpenComRobot(void *arg) { 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); + rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE); + + rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE); + killOpenComRobot = 0; + rt_mutex_release(&mutex_killOpenComRobot); + + rt_sem_p(&sem_openComRobot, TM_INFINITE); + /**************************************************************************************/ /* The task openComRobot starts here */ /**************************************************************************************/ - while (1) { + while (!killOpenComRobot) { rt_sem_p(&sem_openComRobot, TM_INFINITE); cout << "Open serial com ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); @@ -507,8 +563,15 @@ void Tasks::OpenComRobot(void *arg) { WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon //Trigger Detection of Communication Loss with Robot - rt_sem_v(&sem_DetectLostSupRob); + rt_sem_v(&sem_detectLostSupRob); + + rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE); + killOpenComRobotOk = killOpenComRobot; + rt_mutex_release(&mutex_killOpenComRobot); + } + + rt_sem_v(&sem_allowOpenComRobot); } /** @@ -677,14 +740,18 @@ void Tasks::DetectLostSupRob(void *arg){ rt_sem_p(&sem_allowStartDetectLostSupRob, TM_INFINITE); //Wait the Communication with the Robot to be Set - rt_sem_p(&sem_DetectLostSupRob, TM_INFINITE); + 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, 500000000); + while(!kill_detectLostSupRobOk){ + rt_task_wait_period(NULL); //Test Communication with Robot rt_mutex_acquire(&mutex_robot, TM_INFINITE); diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 21d3786..bb3c08b 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -75,6 +75,7 @@ private: bool acquireImage=0; bool getPosition=0; bool drawArena=0; + bool killOpenComRobot=0; @@ -108,6 +109,7 @@ private: RT_MUTEX mutex_killDetectLostSupRob; RT_MUTEX mutex_killStartRob; RT_MUTEX mutex_acquireImage; + RT_MUTEX mutex_killOpenComRobot; /**********************************************************************/ /* Semaphores */ @@ -119,7 +121,10 @@ private: RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_allowStartReceive; RT_SEM sem_allowStartDetectLostSupRob; - RT_SEM sem_DetectLostSupRob; + RT_SEM sem_detectLostSupRob; + RT_SEM sem_restart; + RT_SEM sem_allowSendToMon; + RT_SEM sem_allowOpenComRobot; /**********************************************************************/ /* Message queues */ From 07c5afc82aab7a291612b8557a3ec96d4155921a Mon Sep 17 00:00:00 2001 From: Raphael Date: Sat, 28 Mar 2020 18:59:31 +0100 Subject: [PATCH 03/20] Modified semaphores for all function for detectLostSupRob --- .../raspberry/superviseur-robot/tasks.cpp | 33 ++++++++----------- 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index a3c726f..d810a68 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -171,11 +171,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); - } - if (err = rt_sem_create(&sem_allowSendToMon, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); @@ -248,9 +243,6 @@ void Tasks::Init() { * @brief Démarrage des tâches */ void Tasks::Run() { - - while(1){ - rt_sem_p(&sem_restart, TM_INFINITE); rt_task_set_priority(NULL, T_LOPRIO); int err; @@ -290,7 +282,7 @@ void Tasks::Run() { } cout << "Tasks launched" << endl << flush; - } + } /** @@ -436,7 +428,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_release(&mutex_acquireImage); //Restart all the process - rt_sem_v(&sem_restart); + //Tasks::Run(); //exit(-1); @@ -449,6 +441,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_START_WITHOUT_WD)) { cout << "Command Start Robot without Watchdog Received" << endl << flush; rt_sem_v(&sem_startRobotWithoutWatchdog); @@ -534,19 +527,19 @@ 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_sem_p(&sem_allowOpenComRobot, TM_INFINITE); rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE); killOpenComRobot = 0; - rt_mutex_release(&mutex_killOpenComRobot); - - rt_sem_p(&sem_openComRobot, TM_INFINITE); + rt_mutex_release(&mutex_killOpenComRobot);*/ /**************************************************************************************/ /* The task openComRobot starts here */ /**************************************************************************************/ - while (!killOpenComRobot) { - rt_sem_p(&sem_openComRobot, TM_INFINITE); + //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(); @@ -556,8 +549,10 @@ 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 @@ -565,9 +560,9 @@ void Tasks::OpenComRobot(void *arg) { //Trigger Detection of Communication Loss with Robot rt_sem_v(&sem_detectLostSupRob); - rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE); + /*rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE); killOpenComRobotOk = killOpenComRobot; - rt_mutex_release(&mutex_killOpenComRobot); + rt_mutex_release(&mutex_killOpenComRobot);*/ } @@ -748,7 +743,7 @@ void Tasks::DetectLostSupRob(void *arg){ rt_mutex_release(&mutex_killDetectLostSupRob); //Period = 1s - rt_task_set_periodic(NULL, TM_NOW, 500000000); + rt_task_set_periodic(NULL, TM_NOW, 1000000000); while(!kill_detectLostSupRobOk){ rt_task_wait_period(NULL); From 4826ac1aeb14dda5fc977c7c805f8655e384fe66 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 09:08:54 +0200 Subject: [PATCH 04/20] Connection with Robot Impossible --- .../raspberry/superviseur-robot/tasks.cpp | 558 +++++++++--------- software/raspberry/superviseur-robot/tasks.h | 4 - 2 files changed, 271 insertions(+), 291 deletions(-) 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 */ From 9adc001ccf9d37e1d5c661caeb1ab04519dbc432 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 09:29:21 +0200 Subject: [PATCH 05/20] Start and Restart Server work --- .../raspberry/superviseur-robot/tasks.cpp | 35 +++++++++---------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 216ce59..9c20262 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -281,7 +281,8 @@ void Tasks::Join() { rt_sem_broadcast(&sem_barrier); //Initialization for some specific sem: Allow to run the first time - rt_sem_broadcast(&sem_restart); + //rt_sem_broadcast(&sem_restart); + pause(); } @@ -292,13 +293,10 @@ 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){ - - //First time it's okay after it's managed by receiveFromMon - //rt_sem_p(&sem_restart, TM_INFINITE); + // Synchronization barrier (waiting that all tasks are started) + rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ /* The task server starts here */ @@ -329,18 +327,17 @@ void Tasks::SendToMonTask(void* arg) { // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - while(1){ - //rt_sem_p(&sem_restart, 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){ /**************************************************************************************/ /* The task sendToMon starts here */ /**************************************************************************************/ rt_sem_p(&sem_serverOk, 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 (!kill_sendToMonOk) { cout << "wait msg to send" << endl << flush; @@ -355,7 +352,7 @@ void Tasks::SendToMonTask(void* arg) { rt_mutex_release(&mutex_killSendMon); } - } + //} } /** @@ -369,7 +366,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){ //rt_sem_p(&sem_restart,TM_INFINITE); @@ -419,7 +416,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { //Restart all the process Tasks::Join(); - rt_sem_v(&sem_restart); + //rt_sem_v(&sem_restart); //exit(-1); @@ -513,7 +510,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_release(&mutex_killReceiveFromMon); } - } + //} } /** @@ -522,7 +519,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { void Tasks::OpenComRobot(void *arg) { int status; int err; - bool killOpenComRobotOk = 0; + //bool killOpenComRobotOk = 0; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) From 15772bb42028e1af9be4257f7c5d0270a445028b Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 09:48:27 +0200 Subject: [PATCH 06/20] Start and restart SendToMon ok --- .../raspberry/superviseur-robot/tasks.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 9c20262..38b074a 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -324,10 +324,11 @@ void Tasks::SendToMonTask(void* arg) { 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){ + while(1){ + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + /**************************************************************************************/ /* The task sendToMon starts here */ @@ -335,6 +336,7 @@ void Tasks::SendToMonTask(void* arg) { 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); @@ -352,7 +354,9 @@ void Tasks::SendToMonTask(void* arg) { rt_mutex_release(&mutex_killSendMon); } - //} + + cout << "SendToMon Task dies" << endl << flush; + } } /** @@ -400,7 +404,9 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); killSendMon=1; - rt_mutex_release(&mutex_killSendMon); + 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; @@ -413,7 +419,13 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); acquireImage=0; rt_mutex_release(&mutex_acquireImage); - + + //close Monitor + monitor.Close(); + robot.Close(); + + sleep(2); + //Restart all the process Tasks::Join(); //rt_sem_v(&sem_restart); @@ -545,7 +557,7 @@ void Tasks::OpenComRobot(void *arg) { 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); From e4f00f681686932a7e90eb317b7084e45b7aa9a8 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 10:25:23 +0200 Subject: [PATCH 07/20] Stop Receive From Monitor ok --- .../raspberry/superviseur-robot/tasks.cpp | 40 ++++++++++--------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 38b074a..4a8b907 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -366,12 +366,13 @@ void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; bool killReceiveFromMonOk=0; - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); - - //while(1){ - + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + + + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + + while(1){ //rt_sem_p(&sem_restart,TM_INFINITE); //Reinitialize control boolean @@ -419,16 +420,6 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); acquireImage=0; rt_mutex_release(&mutex_acquireImage); - - //close Monitor - monitor.Close(); - robot.Close(); - - sleep(2); - - //Restart all the process - Tasks::Join(); - //rt_sem_v(&sem_restart); //exit(-1); @@ -517,12 +508,23 @@ void Tasks::ReceiveFromMonTask(void *arg) { delete(msgRcv); // must be deleted manually, no consumer //Update loop condition + + //cout << "J'arrive la" << endl << flush; + rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); - killReceiveFromMonOk=killReceiveFromMon; + killReceiveFromMonOk = killReceiveFromMon; rt_mutex_release(&mutex_killReceiveFromMon); - + + cout << "Kill Receive From Mon Ok = " << killReceiveFromMonOk << endl << flush; } - //} + + monitor.Close(); + robot.Close(); + + cout << "ReceiveFromMon dies" << endl << flush; + //Restart all the process + Tasks::Join(); + } } /** From e912211b00b13168e1fd87fa3dee9c46204105c0 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 11:26:13 +0200 Subject: [PATCH 08/20] Restart receiveFromMon doesn't work --- .../raspberry/superviseur-robot/tasks.cpp | 48 ++++++++++--------- 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 4a8b907..e9d4f1e 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -19,11 +19,11 @@ #include // Déclaration des priorités des taches -#define PRIORITY_TSERVER 30 +#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 @@ -366,15 +366,14 @@ void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; bool killReceiveFromMonOk=0; - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + while(1){ // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - - while(1){ + //rt_sem_p(&sem_restart,TM_INFINITE); - + //Reinitialize control boolean rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); killReceiveFromMon=0; @@ -393,7 +392,6 @@ void Tasks::ReceiveFromMonTask(void *arg) { 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; @@ -421,6 +419,17 @@ void Tasks::ReceiveFromMonTask(void *arg) { acquireImage=0; rt_mutex_release(&mutex_acquireImage); + sleep(1); + + monitor.Close(); + robot.Close(); + + //Tasks::Join(); + + cout << "End sleep" << endl << flush; + rt_sem_broadcast(&sem_barrier); + cout << "Mes couilles" << endl << flush; + //exit(-1); @@ -508,29 +517,24 @@ void Tasks::ReceiveFromMonTask(void *arg) { delete(msgRcv); // must be deleted manually, no consumer //Update loop condition - - //cout << "J'arrive la" << endl << flush; - + + cout << "J'arrive la" << endl << flush; + rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); killReceiveFromMonOk = killReceiveFromMon; rt_mutex_release(&mutex_killReceiveFromMon); - + cout << "Kill Receive From Mon Ok = " << killReceiveFromMonOk << endl << flush; } - - monitor.Close(); - robot.Close(); - + cout << "ReceiveFromMon dies" << endl << flush; - //Restart all the process - Tasks::Join(); } } /** * @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; @@ -543,7 +547,6 @@ void Tasks::OpenComRobot(void *arg) { /* 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); @@ -758,12 +761,12 @@ void Tasks::DetectLostSupRob(void *arg){ if(cpt==3){ //Trigger Kill of DetectLostSupRob rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); - killDetectLostSupRob=0; + killDetectLostSupRob=1; rt_mutex_release(&mutex_killDetectLostSupRob); //Trigger Kill of the Battery acquisition rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); - killBattery=0; + killBattery=1; rt_mutex_release(&mutex_killBattery); } @@ -777,6 +780,7 @@ void Tasks::DetectLostSupRob(void *arg){ kill_detectLostSupRobOk=killDetectLostSupRob; rt_mutex_release(&mutex_killDetectLostSupRob); } + cout << "DetectLostSupRob dies"; } /** From 153fc3a049206330bae734d800b42eb3f643e518 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 12:12:36 +0200 Subject: [PATCH 09/20] Restart ReceiveMon + Server + SendToMon ok --- .../raspberry/superviseur-robot/tasks.cpp | 27 +++++++++---------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index e9d4f1e..64dfa74 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -368,15 +368,16 @@ void Tasks::ReceiveFromMonTask(void *arg) { 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); - //rt_sem_p(&sem_restart,TM_INFINITE); - //Reinitialize control boolean + //Reinitialize control boolean + killReceiveFromMonOk = 0; rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); - killReceiveFromMon=0; + killReceiveFromMon = 0; rt_mutex_release(&mutex_killReceiveFromMon); /**************************************************************************************/ @@ -390,7 +391,6 @@ void Tasks::ReceiveFromMonTask(void *arg) { cout << "Rcv <= " << msgRcv->ToString() << endl << flush; if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { - delete(msgRcv); cout << "Connection to monitor lost" << endl; rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); @@ -419,16 +419,19 @@ void Tasks::ReceiveFromMonTask(void *arg) { acquireImage=0; rt_mutex_release(&mutex_acquireImage); + //Wait every task to die sleep(1); - + + //All closes monitor.Close(); robot.Close(); //Tasks::Join(); - cout << "End sleep" << endl << flush; + //Release restarted tasks +// cout << "End sleep" << endl << flush; rt_sem_broadcast(&sem_barrier); - cout << "Mes couilles" << endl << flush; +// cout << "Mes couilles" << endl << flush; //exit(-1); @@ -518,13 +521,9 @@ void Tasks::ReceiveFromMonTask(void *arg) { //Update loop condition - cout << "J'arrive la" << endl << flush; - rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); killReceiveFromMonOk = killReceiveFromMon; rt_mutex_release(&mutex_killReceiveFromMon); - - cout << "Kill Receive From Mon Ok = " << killReceiveFromMonOk << endl << flush; } cout << "ReceiveFromMon dies" << endl << flush; @@ -642,8 +641,6 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) { int err; while(1){ - - rt_sem_p(&sem_restart, TM_INFINITE); rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); cout << "Start robot with watchdog ("; From e479fec3eb2cdc9017f90d2567381a4a44245d09 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 12:34:28 +0200 Subject: [PATCH 10/20] Restart detectLostSupMon ok but ping doesn't work after restart --- .../raspberry/superviseur-robot/tasks.cpp | 92 ++++++++++--------- 1 file changed, 50 insertions(+), 42 deletions(-) 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"; } /** From cffd879ccbb6e7fdefba8e2d0b376fc0d830211d Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 12:57:33 +0200 Subject: [PATCH 11/20] mutex for close Mon and Rob --- software/raspberry/superviseur-robot/tasks.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 30bbf1d..68af1b8 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -423,8 +423,14 @@ void Tasks::ReceiveFromMonTask(void *arg) { sleep(1); //All closes - monitor.Close(); - robot.Close(); + rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + monitor.Close(); + rt_mutex_release(&mutex_monitor); + + + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + robot.Close(); + rt_mutex_release(&mutex_robot); //Tasks::Join(); From 2805103953890aa26059e569951fdb782003f48a Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 13:41:13 +0200 Subject: [PATCH 12/20] Robot doesn't wait for client --- .../nbproject/private/private.xml | 9 ++++++++- software/raspberry/superviseur-robot/tasks.cpp | 17 ++++++++++++++++- .../simulateur/nbproject/private/private.xml | 4 +--- 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index aef7ea3..80a7712 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -6,6 +6,13 @@ - + + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/commonitor.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.h + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/main.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.h + diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 68af1b8..9702541 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -365,6 +365,7 @@ void Tasks::SendToMonTask(void* arg) { void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; bool killReceiveFromMonOk=0; + int status; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; @@ -429,8 +430,15 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_robot, TM_INFINITE); - robot.Close(); + status=robot.Close(); rt_mutex_release(&mutex_robot); + + if(status<0){ + cout << "Close Robot Fail" << endl << flush; + } + else{ + cout << "Close Robot Success" << endl << flush; + } //Tasks::Join(); @@ -462,6 +470,13 @@ 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)) { + 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; diff --git a/software/simulateur/nbproject/private/private.xml b/software/simulateur/nbproject/private/private.xml index 2683170..aef7ea3 100644 --- a/software/simulateur/nbproject/private/private.xml +++ b/software/simulateur/nbproject/private/private.xml @@ -6,8 +6,6 @@ - - file:/home/etud/dumber/software/simulateur/main.cpp - + From 9e5c6003fc53243d665b68d0978164fe0e8bc649 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sun, 29 Mar 2020 15:29:55 +0200 Subject: [PATCH 13/20] Robot doesn't wait for client 2 --- .../raspberry/superviseur-robot/tasks.cpp | 34 +++++++++++-------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 9702541..1422279 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -422,12 +422,6 @@ void Tasks::ReceiveFromMonTask(void *arg) { //Wait every task to die sleep(1); - - //All closes - rt_mutex_acquire(&mutex_monitor, TM_INFINITE); - monitor.Close(); - rt_mutex_release(&mutex_monitor); - rt_mutex_acquire(&mutex_robot, TM_INFINITE); status=robot.Close(); @@ -440,14 +434,13 @@ void Tasks::ReceiveFromMonTask(void *arg) { cout << "Close Robot Success" << endl << flush; } - //Tasks::Join(); + //All closes + rt_mutex_acquire(&mutex_monitor, TM_INFINITE); + monitor.Close(); + rt_mutex_release(&mutex_monitor); //Release restarted tasks -// cout << "End sleep" << endl << flush; rt_sem_broadcast(&sem_barrier); -// cout << "Mes couilles" << endl << flush; - - //exit(-1); } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) { @@ -760,9 +753,12 @@ void Tasks::DetectLostSupRob(void *arg){ //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 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); @@ -784,15 +780,23 @@ void Tasks::DetectLostSupRob(void *arg){ 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 + //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_release(&mutex_killBattery); + + rt_sem_v(&sem_openComRobot); + } } From eb5fc793f3aba8a013c55209a4648b9f03691cec Mon Sep 17 00:00:00 2001 From: Raphael Date: Mon, 30 Mar 2020 18:27:00 +0200 Subject: [PATCH 14/20] delete old simulator --- software/simulateur/main.cpp | 298 ----------------------------------- 1 file changed, 298 deletions(-) delete mode 100644 software/simulateur/main.cpp diff --git a/software/simulateur/main.cpp b/software/simulateur/main.cpp deleted file mode 100644 index 88ec86c..0000000 --- a/software/simulateur/main.cpp +++ /dev/null @@ -1,298 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -using namespace std; - -const char LABEL_ROBOT_PING = 'p'; -const char LABEL_ROBOT_RESET = 'r'; -const char LABEL_ROBOT_START_WITH_WD = 'W'; -const char LABEL_ROBOT_START_WITHOUT_WD = 'u'; -const char LABEL_ROBOT_RELOAD_WD = 'w'; -const char LABEL_ROBOT_MOVE = 'M'; -const char LABEL_ROBOT_TURN = 'T'; -const char LABEL_ROBOT_GET_BATTERY = 'v'; -const char LABEL_ROBOT_GET_STATE = 'b'; -const char LABEL_ROBOT_POWEROFF = 'z'; - -const char LABEL_ROBOT_OK = 'O'; -const char LABEL_ROBOT_ERROR = 'E'; -const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C'; - -const char LABEL_ROBOT_SEPARATOR_CHAR = '='; -const char LABEL_ROBOT_ENDING_CHAR = 0x0D; - -int server_fd, new_socket, valread; -struct sockaddr_in address; -int addrlen; -#define PORT 6699 - -int status = 0; -int noerr = 0; -int isWD = 0; - -long int ellapse(struct timespec ref, struct timespec cur) { - long int e; - e = cur.tv_sec - ref.tv_sec; - e *= 1000000000; - e += cur.tv_nsec - ref.tv_nsec; - return e; -} - -void print_time(struct timespec start_time) { - struct timespec t; - clock_gettime(CLOCK_REALTIME, &t); - long int e = ellapse(start_time, t); - fprintf(stdout, "%9ld", e / 1000000); -} - -int simulate_error() { - int r = rand() % 1000; - if (r > 950) { - printf("[I don't understand what you said (-1)]\n"); - return -1; - } else if (r > 900) { - printf("[I'm mute, because I never got your message (-2)]\n"); - return -2; - } - printf("[WILCO (0)] "); - return 0; -} - -void simulate_transmission_time() { - usleep((rand() % 30) * 1000); -} - -void open_server() { - - int opt = 1; - // Creating socket file descriptor - if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) { - perror("socket failed"); - exit(EXIT_FAILURE); - } - - // Forcefully attaching socket to the port 8080 - if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, - &opt, sizeof (opt))) { - perror("setsockopt"); - exit(EXIT_FAILURE); - } - - cout << "<<< simulator >>>" << endl; - cout << ">>> Hello, I'm Mr " ; - if (noerr) cout << "perfect "; - cout << "Robot" << endl; - - // Forcefully attaching socket to the port 8080 - if (bind(server_fd, (struct sockaddr *) &address, - sizeof (address)) < 0) { - perror("bind failed"); - exit(EXIT_FAILURE); - } - cout << ">>> I create a server" << endl; - cout << ">>> ..." << endl; - -} - -void wait_connection() { - if (listen(server_fd, 3) < 0) { - perror("listen"); - exit(EXIT_FAILURE); - } - - cout << ">>> I'm waiting a client" << endl; - if ((new_socket = accept(server_fd, (struct sockaddr *) &address, - (socklen_t*) & addrlen)) < 0) { - perror("accept"); - exit(EXIT_FAILURE); - } -} - -void reset(){ - isWD = 0; - status = 0; - cout << ">>> XX I stop XX" << endl; -} - -int main(int argc, char const *argv[]) { - if (argc != 1){ - if (argv[1] == std::string("noerror")){ - noerr = 1; - } - } - - char buffer[1024] = {0}; - addrlen = sizeof (address); - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(PORT); - - srand(time(NULL)); - - open_server(); - wait_connection(); - cout << ">>> I'm ready to receive something" << endl; - struct timespec start_time; - clock_gettime(CLOCK_REALTIME, &start_time); - struct timespec start_wd; - - - struct timespec t; - - long int e; - struct timespec last_call; - clock_gettime(CLOCK_REALTIME, &last_call); - isWD = 0; - - while (status < 3) { - - if (isWD) { - clock_gettime(CLOCK_REALTIME, &t); - e = ellapse(last_call, t); - if ((e / 1000000000) > 3) { - cout << ">>> You break my heart, you never talk at the right time." << endl; - break; - } - } - - - valread = read(new_socket, buffer, 1024); - if (valread <= 0) { - if (errno == EAGAIN) { - status = 3; - cout << ">>> You break my heart, I've been waiting too long for you." << endl; - break; - } else { - cout << ">>> Why did you hang up? Please, contact me again." << endl; - reset(); - wait_connection(); - clock_gettime(CLOCK_REALTIME, &last_call); - } - } - string s = ""; - int error = 0; - - if (!noerr) error = simulate_error(); - - if (error == 0) { - - print_time(start_time); - printf(": I received a message %s\n", buffer); - switch (buffer[0]) { - case LABEL_ROBOT_START_WITHOUT_WD: - cout << ">>> I start without watchdog" << endl; - s += LABEL_ROBOT_OK; - break; - case LABEL_ROBOT_PING: - cout << ">>> ...Pong" << endl; - s += LABEL_ROBOT_OK; - break; - case LABEL_ROBOT_START_WITH_WD: - clock_gettime(CLOCK_REALTIME, &start_wd); - clock_gettime(CLOCK_REALTIME, &last_call); - struct timeval tv; - tv.tv_sec = 3; - tv.tv_usec = 0; - setsockopt(new_socket, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); - - cout << ">>> I start with watchdog" << endl; - s += LABEL_ROBOT_OK; - isWD = 1; - break; - case LABEL_ROBOT_MOVE: - switch (buffer[2]) { - case '0': - cout << ">>> XX I stop XX" << endl; - break; - case '-': - cout << ">>> \\/ I move backward \\/" << endl; - break; - default: - cout << ">>> /\\ I move forward /\\" << endl; - break; - } - s += LABEL_ROBOT_OK; - break; - case LABEL_ROBOT_TURN: - switch (buffer[2]) { - case '-': - cout << ">>> << I turn to the left <<" << endl; - break; - default: - cout << ">>> >> I turn to the right >>" << endl; - break; - } - s += LABEL_ROBOT_OK; - break; - case LABEL_ROBOT_GET_BATTERY: - cout << ">>> I give you my battery level :-o" << endl; - clock_gettime(CLOCK_REALTIME, &t); - e = ellapse(start_time, t); - if (e > 20000000000) { - s += '0'; - } else { - if (e > 10000000000) { - s += '1'; - } else { - s += '2'; - } - } - break; - case LABEL_ROBOT_RELOAD_WD: - clock_gettime(CLOCK_REALTIME, &t); - e = ellapse(start_wd, t); - e = (e / 1000000) % 1000; - if (isWD) { - if ((e < 50) || (e > 950)) { - cout << ">>> Just in time for a reload " << e << "ms" << endl; - last_call = t; - status = 0; - s += LABEL_ROBOT_OK; - } else { - status++; - cout << ">>> You missed the date, -1 point " << e << "ms (" << status << ")" << endl; - - s += LABEL_ROBOT_UNKNOWN_COMMAND; - } - } else { - cout << "Why you said that, I do nothing" << endl; - } - break; - case LABEL_ROBOT_POWEROFF: - cout << ">>> Bye bye, see you soon" << endl; - s += LABEL_ROBOT_OK; - status = 10; - break; - case LABEL_ROBOT_RESET: - cout << ">>> I reset" << endl; - s += LABEL_ROBOT_OK; - reset(); - break; - case LABEL_ROBOT_GET_STATE: - cout << ">>> I'm fine, thank you" << endl; - s += LABEL_ROBOT_OK; - break; - default: - //msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); - cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << buffer << ")" << endl << flush; - } - simulate_transmission_time(); - send(new_socket, s.c_str(), s.length(), 0); - } else if (error == -1) { - s += LABEL_ROBOT_UNKNOWN_COMMAND; - simulate_transmission_time(); - send(new_socket, s.c_str(), s.length(), 0); - } else if (error == -2) { - /* Do nothing */ - } - } - - cout << "The robot is out. End of story. " << endl; - cout << " /\\_/\\" << endl << "( o.o )" << endl << " > ^ <" << endl; - return 0; -} \ No newline at end of file From 3f0581fc34acb7eca4fe33276ede986dbac194c4 Mon Sep 17 00:00:00 2001 From: Raphael Date: Mon, 30 Mar 2020 18:27:33 +0200 Subject: [PATCH 15/20] Add new simulator --- software/simulateur/main.cpp | 298 +++++++++++++++++++++++++++++++++++ 1 file changed, 298 insertions(+) create mode 100644 software/simulateur/main.cpp diff --git a/software/simulateur/main.cpp b/software/simulateur/main.cpp new file mode 100644 index 0000000..88ec86c --- /dev/null +++ b/software/simulateur/main.cpp @@ -0,0 +1,298 @@ +#include +#include +#include +#include +#include +#include +#include +#include +using namespace std; + +const char LABEL_ROBOT_PING = 'p'; +const char LABEL_ROBOT_RESET = 'r'; +const char LABEL_ROBOT_START_WITH_WD = 'W'; +const char LABEL_ROBOT_START_WITHOUT_WD = 'u'; +const char LABEL_ROBOT_RELOAD_WD = 'w'; +const char LABEL_ROBOT_MOVE = 'M'; +const char LABEL_ROBOT_TURN = 'T'; +const char LABEL_ROBOT_GET_BATTERY = 'v'; +const char LABEL_ROBOT_GET_STATE = 'b'; +const char LABEL_ROBOT_POWEROFF = 'z'; + +const char LABEL_ROBOT_OK = 'O'; +const char LABEL_ROBOT_ERROR = 'E'; +const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C'; + +const char LABEL_ROBOT_SEPARATOR_CHAR = '='; +const char LABEL_ROBOT_ENDING_CHAR = 0x0D; + +int server_fd, new_socket, valread; +struct sockaddr_in address; +int addrlen; +#define PORT 6699 + +int status = 0; +int noerr = 0; +int isWD = 0; + +long int ellapse(struct timespec ref, struct timespec cur) { + long int e; + e = cur.tv_sec - ref.tv_sec; + e *= 1000000000; + e += cur.tv_nsec - ref.tv_nsec; + return e; +} + +void print_time(struct timespec start_time) { + struct timespec t; + clock_gettime(CLOCK_REALTIME, &t); + long int e = ellapse(start_time, t); + fprintf(stdout, "%9ld", e / 1000000); +} + +int simulate_error() { + int r = rand() % 1000; + if (r > 950) { + printf("[I don't understand what you said (-1)]\n"); + return -1; + } else if (r > 900) { + printf("[I'm mute, because I never got your message (-2)]\n"); + return -2; + } + printf("[WILCO (0)] "); + return 0; +} + +void simulate_transmission_time() { + usleep((rand() % 30) * 1000); +} + +void open_server() { + + int opt = 1; + // Creating socket file descriptor + if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) { + perror("socket failed"); + exit(EXIT_FAILURE); + } + + // Forcefully attaching socket to the port 8080 + if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, + &opt, sizeof (opt))) { + perror("setsockopt"); + exit(EXIT_FAILURE); + } + + cout << "<<< simulator >>>" << endl; + cout << ">>> Hello, I'm Mr " ; + if (noerr) cout << "perfect "; + cout << "Robot" << endl; + + // Forcefully attaching socket to the port 8080 + if (bind(server_fd, (struct sockaddr *) &address, + sizeof (address)) < 0) { + perror("bind failed"); + exit(EXIT_FAILURE); + } + cout << ">>> I create a server" << endl; + cout << ">>> ..." << endl; + +} + +void wait_connection() { + if (listen(server_fd, 3) < 0) { + perror("listen"); + exit(EXIT_FAILURE); + } + + cout << ">>> I'm waiting a client" << endl; + if ((new_socket = accept(server_fd, (struct sockaddr *) &address, + (socklen_t*) & addrlen)) < 0) { + perror("accept"); + exit(EXIT_FAILURE); + } +} + +void reset(){ + isWD = 0; + status = 0; + cout << ">>> XX I stop XX" << endl; +} + +int main(int argc, char const *argv[]) { + if (argc != 1){ + if (argv[1] == std::string("noerror")){ + noerr = 1; + } + } + + char buffer[1024] = {0}; + addrlen = sizeof (address); + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(PORT); + + srand(time(NULL)); + + open_server(); + wait_connection(); + cout << ">>> I'm ready to receive something" << endl; + struct timespec start_time; + clock_gettime(CLOCK_REALTIME, &start_time); + struct timespec start_wd; + + + struct timespec t; + + long int e; + struct timespec last_call; + clock_gettime(CLOCK_REALTIME, &last_call); + isWD = 0; + + while (status < 3) { + + if (isWD) { + clock_gettime(CLOCK_REALTIME, &t); + e = ellapse(last_call, t); + if ((e / 1000000000) > 3) { + cout << ">>> You break my heart, you never talk at the right time." << endl; + break; + } + } + + + valread = read(new_socket, buffer, 1024); + if (valread <= 0) { + if (errno == EAGAIN) { + status = 3; + cout << ">>> You break my heart, I've been waiting too long for you." << endl; + break; + } else { + cout << ">>> Why did you hang up? Please, contact me again." << endl; + reset(); + wait_connection(); + clock_gettime(CLOCK_REALTIME, &last_call); + } + } + string s = ""; + int error = 0; + + if (!noerr) error = simulate_error(); + + if (error == 0) { + + print_time(start_time); + printf(": I received a message %s\n", buffer); + switch (buffer[0]) { + case LABEL_ROBOT_START_WITHOUT_WD: + cout << ">>> I start without watchdog" << endl; + s += LABEL_ROBOT_OK; + break; + case LABEL_ROBOT_PING: + cout << ">>> ...Pong" << endl; + s += LABEL_ROBOT_OK; + break; + case LABEL_ROBOT_START_WITH_WD: + clock_gettime(CLOCK_REALTIME, &start_wd); + clock_gettime(CLOCK_REALTIME, &last_call); + struct timeval tv; + tv.tv_sec = 3; + tv.tv_usec = 0; + setsockopt(new_socket, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); + + cout << ">>> I start with watchdog" << endl; + s += LABEL_ROBOT_OK; + isWD = 1; + break; + case LABEL_ROBOT_MOVE: + switch (buffer[2]) { + case '0': + cout << ">>> XX I stop XX" << endl; + break; + case '-': + cout << ">>> \\/ I move backward \\/" << endl; + break; + default: + cout << ">>> /\\ I move forward /\\" << endl; + break; + } + s += LABEL_ROBOT_OK; + break; + case LABEL_ROBOT_TURN: + switch (buffer[2]) { + case '-': + cout << ">>> << I turn to the left <<" << endl; + break; + default: + cout << ">>> >> I turn to the right >>" << endl; + break; + } + s += LABEL_ROBOT_OK; + break; + case LABEL_ROBOT_GET_BATTERY: + cout << ">>> I give you my battery level :-o" << endl; + clock_gettime(CLOCK_REALTIME, &t); + e = ellapse(start_time, t); + if (e > 20000000000) { + s += '0'; + } else { + if (e > 10000000000) { + s += '1'; + } else { + s += '2'; + } + } + break; + case LABEL_ROBOT_RELOAD_WD: + clock_gettime(CLOCK_REALTIME, &t); + e = ellapse(start_wd, t); + e = (e / 1000000) % 1000; + if (isWD) { + if ((e < 50) || (e > 950)) { + cout << ">>> Just in time for a reload " << e << "ms" << endl; + last_call = t; + status = 0; + s += LABEL_ROBOT_OK; + } else { + status++; + cout << ">>> You missed the date, -1 point " << e << "ms (" << status << ")" << endl; + + s += LABEL_ROBOT_UNKNOWN_COMMAND; + } + } else { + cout << "Why you said that, I do nothing" << endl; + } + break; + case LABEL_ROBOT_POWEROFF: + cout << ">>> Bye bye, see you soon" << endl; + s += LABEL_ROBOT_OK; + status = 10; + break; + case LABEL_ROBOT_RESET: + cout << ">>> I reset" << endl; + s += LABEL_ROBOT_OK; + reset(); + break; + case LABEL_ROBOT_GET_STATE: + cout << ">>> I'm fine, thank you" << endl; + s += LABEL_ROBOT_OK; + break; + default: + //msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); + cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << buffer << ")" << endl << flush; + } + simulate_transmission_time(); + send(new_socket, s.c_str(), s.length(), 0); + } else if (error == -1) { + s += LABEL_ROBOT_UNKNOWN_COMMAND; + simulate_transmission_time(); + send(new_socket, s.c_str(), s.length(), 0); + } else if (error == -2) { + /* Do nothing */ + } + } + + cout << "The robot is out. End of story. " << endl; + cout << " /\\_/\\" << endl << "( o.o )" << endl << " > ^ <" << endl; + return 0; +} \ No newline at end of file From 74d31964913c277e18264d277966e0cc3e3cc964 Mon Sep 17 00:00:00 2001 From: Raphael Date: Tue, 31 Mar 2020 13:46:29 +0200 Subject: [PATCH 16/20] remove old comrobot --- .../superviseur-robot/lib/comrobot.cpp | 412 ------------------ .../superviseur-robot/lib/comrobot.h | 198 --------- 2 files changed, 610 deletions(-) delete mode 100644 software/raspberry/superviseur-robot/lib/comrobot.cpp delete mode 100644 software/raspberry/superviseur-robot/lib/comrobot.h diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp deleted file mode 100644 index cbc618a..0000000 --- a/software/raspberry/superviseur-robot/lib/comrobot.cpp +++ /dev/null @@ -1,412 +0,0 @@ -/* - * Copyright (C) 2018 dimercur - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include "comrobot.h" - -#include -#include -#include -#include -#include - -#include -#include - -#ifdef __SIMULATION__ -#include -#include -#include -int sock = 0; -#define PORT 6699 -#endif - -#ifdef __FOR_PC__ -#define USART_FILENAME "/dev/ttyUSB0" -#else -#define USART_FILENAME "/dev/ttyS0" -#endif /* __FOR_PC__ */ - -/* - * Constants to be used for communicating with robot. Contains command tag - */ -const char LABEL_ROBOT_PING = 'p'; -const char LABEL_ROBOT_RESET = 'r'; -const char LABEL_ROBOT_START_WITH_WD = 'W'; -const char LABEL_ROBOT_START_WITHOUT_WD = 'u'; -const char LABEL_ROBOT_RELOAD_WD = 'w'; -const char LABEL_ROBOT_MOVE = 'M'; -const char LABEL_ROBOT_TURN = 'T'; -const char LABEL_ROBOT_GET_BATTERY = 'v'; -const char LABEL_ROBOT_GET_STATE = 'b'; -const char LABEL_ROBOT_POWEROFF = 'z'; - -const char LABEL_ROBOT_OK = 'O'; -const char LABEL_ROBOT_ERROR = 'E'; -const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C'; - -const char LABEL_ROBOT_SEPARATOR_CHAR = '='; -const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r) - -/** - * Open serial link with robot - * @return File descriptor - * @throw std::runtime_error if it fails - */ -int ComRobot::Open() { - return this->Open(USART_FILENAME); -} - -/** - * Open serial link with robot - * @param usart Filename of usart to open - * @return File descriptor - * @throw std::runtime_error if it fails - */ -int ComRobot::Open(string usart) { - struct termios options; - -#ifdef __SIMULATION__ - - struct sockaddr_in serv_addr; - if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { - printf("\n Socket creation error \n"); - return -1; - } - struct timeval tv; - tv.tv_sec = 0; - tv.tv_usec = 80000; - setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); - - serv_addr.sin_family = AF_INET; - serv_addr.sin_port = htons(PORT); - - // Convert IPv4 and IPv6 addresses from text to binary form - if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) { - printf("\nInvalid address/ Address not supported \n"); - return -1; - } - - if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) { - return -1; - } - return 1; -#else - - fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode - if (fd == -1) { - cerr << "[" << __PRETTY_FUNCTION__ << "] Unable to open UART (" << usart << "). Ensure it is not in use by another application" << endl << flush; - throw std::runtime_error{"Unable to open UART"}; - exit(EXIT_FAILURE); - } else { - fcntl(fd, F_SETFL, 0); - tcgetattr(fd, &options); - options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); - cfsetospeed(&options, B9600); - cfsetispeed(&options, B9600); - options.c_cc[VMIN] = 0; - options.c_cc[VTIME] = 1; /* Timeout of 100 ms per character */ - tcsetattr(fd, TCSANOW, &options); - } - - return fd; -#endif -} - -/** - * Close serial link - * @return Success if above 0, failure if below 0 - */ -int ComRobot::Close() { - return close(fd); -} - -/** - * Send a message to robot - * @param msg Message to send to robot - * @return 1 if success, 0 otherwise - * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself - * @attention Write is blocking until message is written into buffer (linux side) - * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously - */ -Message *ComRobot::Write(Message* msg) { - Message *msgAnswer; - string s; - - if (this->fd != -1) { - - Write_Pre(); - - s = MessageToString(msg); -#ifdef __SIMULATION__ - - char buffer[1024] = {0}; - cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush; - send(sock, s.c_str(), s.length(), 0); - - int valread = read(sock, buffer, 1024); - if (valread < 0) { - 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; -#else - AddChecksum(s); - - //cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write - - if (count < 0) { - cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush; - msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR); - } else { /* write successfull, read answer from robot */ - - try { - s = Read(); - //cout << "Answer = "<fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) - if (rxLength == 0) { // timeout - // try again - rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) - if (rxLength == 0) { // re-timeout: it sucks ! - throw std::runtime_error{"ComRobot::Read: Timeout when reading from com port"}; - } - } else if (rxLength < 0) { // big pb ! - throw std::runtime_error{"ComRobot::Read: Unknown problem when reading from com port"}; - } else { // everything ok - if ((receivedChar != '\r') && (receivedChar != '\n')) s += receivedChar; - } - } while ((receivedChar != '\r') && (receivedChar != '\n')); - - return s; -} - -Message *ComRobot::SendCommand(Message* msg, MessageID answerID, int maxRetries) { - int counter = maxRetries; - Message *msgSend; - Message *msgRcv; - Message *msgTmp; - - do { - msgSend = msg->Copy(); - cout << "S => " << msgSend->ToString() << endl << flush; - msgTmp = Write(msgSend); - cout << "R <= " << msgTmp->ToString() << endl << flush; - - if (msgTmp->CompareID(answerID)) counter = 0; - else counter--; - - if (counter == 0) msgRcv = msgTmp->Copy(); - - delete(msgTmp); - } while (counter); - - delete (msg); - - return msgRcv; -} - -/** - * Convert an array of char to its message representation (when receiving data from stm32) - * @param bytes Array of char - * @return Message corresponding to received array of char - */ -Message* ComRobot::StringToMessage(string s) { - Message *msg; - - switch (s[0]) { - case LABEL_ROBOT_OK: - msg = new Message(MESSAGE_ANSWER_ACK); - break; - case LABEL_ROBOT_ERROR: - msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); - break; - case LABEL_ROBOT_UNKNOWN_COMMAND: - msg = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND); - break; - case '0': - msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY); - break; - case '1': - msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW); - break; - case '2': - msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL); - break; - default: - msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); - cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << s << ")" << endl << flush; - } - - return msg; -} - -/** - * Convert a message to its array of char representation (for sending command to stm32) - * @param msg Message to be sent to robot - * @param buffer Array of char, image of message to send - */ -string ComRobot::MessageToString(Message *msg) { - string s; - - float val_f; - int val_i; - unsigned char *b; - - switch (msg->GetID()) { - case MESSAGE_ROBOT_PING: - s += LABEL_ROBOT_PING; - break; - case MESSAGE_ROBOT_RESET: - s += LABEL_ROBOT_RESET; - break; - case MESSAGE_ROBOT_POWEROFF: - s += LABEL_ROBOT_POWEROFF; - break; - case MESSAGE_ROBOT_START_WITHOUT_WD: - s += LABEL_ROBOT_START_WITHOUT_WD; - break; - case MESSAGE_ROBOT_START_WITH_WD: - s += LABEL_ROBOT_START_WITH_WD; - break; - case MESSAGE_ROBOT_RELOAD_WD: - s += LABEL_ROBOT_RELOAD_WD; - break; - case MESSAGE_ROBOT_BATTERY_GET: - s += LABEL_ROBOT_GET_BATTERY; - break; - case MESSAGE_ROBOT_STATE_GET: - s += LABEL_ROBOT_GET_STATE; - break; - case MESSAGE_ROBOT_GO_FORWARD: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(500000)); - break; - case MESSAGE_ROBOT_GO_BACKWARD: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(-500000)); - break; - case MESSAGE_ROBOT_GO_LEFT: - s += LABEL_ROBOT_TURN; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(-500000)); - break; - case MESSAGE_ROBOT_GO_RIGHT: - s += LABEL_ROBOT_TURN; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(500000)); - break; - case MESSAGE_ROBOT_STOP: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(0)); - break; - case MESSAGE_ROBOT_MOVE: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(((MessageInt*) msg)->GetValue())); - break; - case MESSAGE_ROBOT_TURN: - s += LABEL_ROBOT_TURN; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(((MessageInt*) msg)->GetValue())); - break; - default: - cerr << "[" << __PRETTY_FUNCTION__ << "] Invalid message for robot (" << msg->ToString() << ")" << endl << flush; - throw std::runtime_error{"Invalid message"}; - } - - return s; -} - -/** - * Add a checksum and carriage return to a command string - * @param[in,out] s String containing command for robot, without ending char (carriage return) - */ -void ComRobot::AddChecksum(string &s) { - unsigned char checksum = 0; - - for (string::iterator it = s.begin(); it != s.end(); ++it) { - checksum ^= (unsigned char) *it; - } - - s += (char) checksum; // Add calculated checksum - s += (char) LABEL_ROBOT_ENDING_CHAR; -} - -/** - * Verify if checksum of an incoming answer from robot is valid, - * then remove checksum from incoming answer (if checksum is ok) - * @param[in,out] s String containing incoming answer from robot - * @return true is checksum is valid, false otherwise. - */ -bool ComRobot::VerifyChecksum(string &s) { - unsigned char checksum = 0; - - for (string::iterator it = s.begin(); it != s.end(); ++it) { - checksum ^= (unsigned char) *it; - } - - if (checksum == 0) { // checksum is ok, remove last char of string (checksum) - s.pop_back(); // remove last char - return true; - } else return false; -} diff --git a/software/raspberry/superviseur-robot/lib/comrobot.h b/software/raspberry/superviseur-robot/lib/comrobot.h deleted file mode 100644 index d6bdd11..0000000 --- a/software/raspberry/superviseur-robot/lib/comrobot.h +++ /dev/null @@ -1,198 +0,0 @@ -/* - * Copyright (C) 2018 dimercur - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#ifndef __COMROBOT_H__ -#define __COMROBOT_H__ - -#include "messages.h" -#include - -using namespace std; - -/** - * Class used for communicating with robot over serial - * - * @brief Communication class with robot - * - */ -class ComRobot { -public: - - /** - * Constructor - */ - ComRobot() { - } - - /** - * Destructor - */ - virtual ~ComRobot() { - } - - /** - * Open serial link with robot - * @return File descriptor - * @throw std::runtime_error if it fails - */ - int Open(); - - /** - * Open serial link with robot - * @param usart Filename of usart to open - * @return File descriptor - * @throw std::runtime_error if it fails - */ - int Open(string usart); - - /** - * Close serial link - * @return Success if above 0, failure if below 0 - */ - int Close(); - - /** - * Send a message to robot - * @param msg Message to send to robot - * @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command - * @attention Input message is destroyed (delete) after being sent. You do not need to delete it yourself - * @attention Write produce an answer message. You have to dispose it (delete) when you have finished using it - * @attention Write is blocking until message is written into buffer (linux side) - * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously - */ - Message *Write(Message* msg); - - /** - * Function called at beginning of Write method - * Use it to do some synchronization (call of mutex, for example) - */ - virtual void Write_Pre() { - } - - /** - * Function called at end of Write method - * Use it to do some synchronization (call of mutex, for example) - */ - virtual void Write_Post() { - } - - Message *SendCommand(Message* msg, MessageID answerID, int maxRetries); - - static Message *Ping() { - return new Message(MESSAGE_ROBOT_PING); - } - - static Message *Reset() { - return new Message(MESSAGE_ROBOT_RESET); - } - - static Message *PowerOff() { - return new Message(MESSAGE_ROBOT_POWEROFF); - } - - static Message *StartWithWD() { - return new Message(MESSAGE_ROBOT_START_WITH_WD); - } - - static Message *StartWithoutWD() { - return new Message(MESSAGE_ROBOT_START_WITHOUT_WD); - } - - static Message *ReloadWD() { - return new Message(MESSAGE_ROBOT_RELOAD_WD); - } - - static Message *Move(int length) { - return new MessageInt(MESSAGE_ROBOT_MOVE, length); - } - - static Message *Turn(int angle) { - return new MessageInt(MESSAGE_ROBOT_TURN, angle); - } - - static Message *Stop() { - return new Message(MESSAGE_ROBOT_STOP); - } - - static Message *GoForward() { - return new Message(MESSAGE_ROBOT_GO_FORWARD); - } - - static Message *GoBackward() { - return new Message(MESSAGE_ROBOT_GO_BACKWARD); - } - - static Message *GoLeft() { - return new Message(MESSAGE_ROBOT_GO_LEFT); - } - - static Message *GoRight() { - return new Message(MESSAGE_ROBOT_GO_RIGHT); - } - - static Message *GetBattery() { - return new Message(MESSAGE_ROBOT_BATTERY_GET); - } - - static Message *GetState() { - return new Message(MESSAGE_ROBOT_STATE_GET); - } - -protected: - /** - * Serial link file descriptor - */ - int fd; - - /** - * Get an answer from robot - * @return String containing answer from robot - * @attention Read method is blocking until a message is received (timeout of 500 ms) - * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously - */ - string Read(); - - /** - * Convert a string to its message representation (when receiving data from robot) - * @param s String from robot containing answer - * @return Message corresponding to received array of char - */ - Message* StringToMessage(string s); - - /** - * Convert a message to its string representation (for sending command to robot) - * @param msg Message to be sent to robot - * @return String containing command to robot - */ - string MessageToString(Message *msg); - - /** - * Add a checksum and carriage return to a command string - * @param[in,out] s String containing command for robot, without ending char (carriage return) - */ - void AddChecksum(string &s); - - /** - * Verify if checksum of an incoming answer from robot is valid, - * then remove checksum from incoming answer (if checksum is ok) - * @param[in,out] s String containing incoming answer from robot - * @return true is checksum is valid, false otherwise. - */ - bool VerifyChecksum(string &s); -}; - -#endif /* __COMROBOT_H__ */ From 4004ec12c31b4099c95e1f8890e66710e6e7cb1e Mon Sep 17 00:00:00 2001 From: Raphael Date: Tue, 31 Mar 2020 13:47:01 +0200 Subject: [PATCH 17/20] add new comrobot --- .../superviseur-robot/lib/comrobot.cpp | 421 ++++++++++++++++++ .../superviseur-robot/lib/comrobot.h | 198 ++++++++ .../nbproject/private/private.xml | 5 +- .../simulateur/nbproject/private/private.xml | 4 +- 4 files changed, 625 insertions(+), 3 deletions(-) create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.cpp create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.h diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp new file mode 100644 index 0000000..cd1c2f8 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.cpp @@ -0,0 +1,421 @@ +/* + * Copyright (C) 2018 dimercur + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "comrobot.h" + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef __SIMULATION__ +#include +#include +#include +int sock = 0; +#define PORT 6699 +#endif + +#ifdef __FOR_PC__ +#define USART_FILENAME "/dev/ttyUSB0" +#else +#define USART_FILENAME "/dev/ttyS0" +#endif /* __FOR_PC__ */ + +/* + * Constants to be used for communicating with robot. Contains command tag + */ +const char LABEL_ROBOT_PING = 'p'; +const char LABEL_ROBOT_RESET = 'r'; +const char LABEL_ROBOT_START_WITH_WD = 'W'; +const char LABEL_ROBOT_START_WITHOUT_WD = 'u'; +const char LABEL_ROBOT_RELOAD_WD = 'w'; +const char LABEL_ROBOT_MOVE = 'M'; +const char LABEL_ROBOT_TURN = 'T'; +const char LABEL_ROBOT_GET_BATTERY = 'v'; +const char LABEL_ROBOT_GET_STATE = 'b'; +const char LABEL_ROBOT_POWEROFF = 'z'; + +const char LABEL_ROBOT_OK = 'O'; +const char LABEL_ROBOT_ERROR = 'E'; +const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C'; + +const char LABEL_ROBOT_SEPARATOR_CHAR = '='; +const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r) + +/** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ +int ComRobot::Open() { + return this->Open(USART_FILENAME); +} + +/** + * Open serial link with robot + * @param usart Filename of usart to open + * @return File descriptor + * @throw std::runtime_error if it fails + */ +int ComRobot::Open(string usart) { + struct termios options; + +#ifdef __SIMULATION__ + + struct sockaddr_in serv_addr; + if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + printf("\n Socket creation error \n"); + return -1; + } + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 80000; + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); + + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(PORT); + + // Convert IPv4 and IPv6 addresses from text to binary form + if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) { + printf("\nInvalid address/ Address not supported \n"); + return -1; + } + + if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) { + return -1; + } + return 1; +#else + + fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode + if (fd == -1) { + cerr << "[" << __PRETTY_FUNCTION__ << "] Unable to open UART (" << usart << "). Ensure it is not in use by another application" << endl << flush; + throw std::runtime_error{"Unable to open UART"}; + exit(EXIT_FAILURE); + } else { + fcntl(fd, F_SETFL, 0); + tcgetattr(fd, &options); + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + cfsetospeed(&options, B9600); + cfsetispeed(&options, B9600); + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 1; /* Timeout of 100 ms per character */ + tcsetattr(fd, TCSANOW, &options); + } + + return fd; +#endif +} + +/** + * Close serial link + * @return Success if above 0, failure if below 0 + */ +int ComRobot::Close() { +#ifdef __SIMULATION__ + return close(sock); +#elif + return close(fd); +#endif +} + +/** + * Send a message to robot + * @param msg Message to send to robot + * @return 1 if success, 0 otherwise + * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ +Message *ComRobot::Write(Message* msg) { + Message *msgAnswer; + string s; + + if (this->fd != -1) { + + Write_Pre(); + + s = MessageToString(msg); +#ifdef __SIMULATION__ + + char buffer[1024] = {0}; + cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush; + send(sock, s.c_str(), s.length(), MSG_NOSIGNAL); + + int valread = read(sock, buffer, 1024); + + 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); + cout << "Response: " << buffer << ", id: " << msgAnswer->GetID() << endl; + } + +#else + AddChecksum(s); + + //cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write + + if (count < 0) { + cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush; + msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR); + } else { /* write successfull, read answer from robot */ + + try { + s = Read(); + //cout << "Answer = "<fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) + if (rxLength == 0) { // timeout + // try again + rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) + if (rxLength == 0) { // re-timeout: it sucks ! + throw std::runtime_error{"ComRobot::Read: Timeout when reading from com port"}; + } + } else if (rxLength < 0) { // big pb ! + throw std::runtime_error{"ComRobot::Read: Unknown problem when reading from com port"}; + } else { // everything ok + if ((receivedChar != '\r') && (receivedChar != '\n')) s += receivedChar; + } + } while ((receivedChar != '\r') && (receivedChar != '\n')); + + return s; +} + +Message *ComRobot::SendCommand(Message* msg, MessageID answerID, int maxRetries) { + int counter = maxRetries; + Message *msgSend; + Message *msgRcv; + Message *msgTmp; + + do { + msgSend = msg->Copy(); + cout << "S => " << msgSend->ToString() << endl << flush; + msgTmp = Write(msgSend); + cout << "R <= " << msgTmp->ToString() << endl << flush; + + if (msgTmp->CompareID(answerID)) counter = 0; + else counter--; + + if (counter == 0) msgRcv = msgTmp->Copy(); + + delete(msgTmp); + } while (counter); + + delete (msg); + + return msgRcv; +} + +/** + * Convert an array of char to its message representation (when receiving data from stm32) + * @param bytes Array of char + * @return Message corresponding to received array of char + */ +Message* ComRobot::StringToMessage(string s) { + Message *msg; + + switch (s[0]) { + case LABEL_ROBOT_OK: + msg = new Message(MESSAGE_ANSWER_ACK); + break; + case LABEL_ROBOT_ERROR: + msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); + break; + case LABEL_ROBOT_UNKNOWN_COMMAND: + msg = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND); + break; + case '0': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY); + break; + case '1': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW); + break; + case '2': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL); + break; + default: + msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); + cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << s << ")" << endl << flush; + } + + return msg; +} + +/** + * Convert a message to its array of char representation (for sending command to stm32) + * @param msg Message to be sent to robot + * @param buffer Array of char, image of message to send + */ +string ComRobot::MessageToString(Message *msg) { + string s; + + float val_f; + int val_i; + unsigned char *b; + + switch (msg->GetID()) { + case MESSAGE_ROBOT_PING: + s += LABEL_ROBOT_PING; + break; + case MESSAGE_ROBOT_RESET: + s += LABEL_ROBOT_RESET; + break; + case MESSAGE_ROBOT_POWEROFF: + s += LABEL_ROBOT_POWEROFF; + break; + case MESSAGE_ROBOT_START_WITHOUT_WD: + s += LABEL_ROBOT_START_WITHOUT_WD; + break; + case MESSAGE_ROBOT_START_WITH_WD: + s += LABEL_ROBOT_START_WITH_WD; + break; + case MESSAGE_ROBOT_RELOAD_WD: + s += LABEL_ROBOT_RELOAD_WD; + break; + case MESSAGE_ROBOT_BATTERY_GET: + s += LABEL_ROBOT_GET_BATTERY; + break; + case MESSAGE_ROBOT_STATE_GET: + s += LABEL_ROBOT_GET_STATE; + break; + case MESSAGE_ROBOT_GO_FORWARD: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(500000)); + break; + case MESSAGE_ROBOT_GO_BACKWARD: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(-500000)); + break; + case MESSAGE_ROBOT_GO_LEFT: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(-500000)); + break; + case MESSAGE_ROBOT_GO_RIGHT: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(500000)); + break; + case MESSAGE_ROBOT_STOP: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(0)); + break; + case MESSAGE_ROBOT_MOVE: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(((MessageInt*) msg)->GetValue())); + break; + case MESSAGE_ROBOT_TURN: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(((MessageInt*) msg)->GetValue())); + break; + default: + cerr << "[" << __PRETTY_FUNCTION__ << "] Invalid message for robot (" << msg->ToString() << ")" << endl << flush; + throw std::runtime_error{"Invalid message"}; + } + + return s; +} + +/** + * Add a checksum and carriage return to a command string + * @param[in,out] s String containing command for robot, without ending char (carriage return) + */ +void ComRobot::AddChecksum(string &s) { + unsigned char checksum = 0; + + for (string::iterator it = s.begin(); it != s.end(); ++it) { + checksum ^= (unsigned char) *it; + } + + s += (char) checksum; // Add calculated checksum + s += (char) LABEL_ROBOT_ENDING_CHAR; +} + +/** + * Verify if checksum of an incoming answer from robot is valid, + * then remove checksum from incoming answer (if checksum is ok) + * @param[in,out] s String containing incoming answer from robot + * @return true is checksum is valid, false otherwise. + */ +bool ComRobot::VerifyChecksum(string &s) { + unsigned char checksum = 0; + + for (string::iterator it = s.begin(); it != s.end(); ++it) { + checksum ^= (unsigned char) *it; + } + + if (checksum == 0) { // checksum is ok, remove last char of string (checksum) + s.pop_back(); // remove last char + return true; + } else return false; +} diff --git a/software/raspberry/superviseur-robot/lib/comrobot.h b/software/raspberry/superviseur-robot/lib/comrobot.h new file mode 100644 index 0000000..d6bdd11 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.h @@ -0,0 +1,198 @@ +/* + * Copyright (C) 2018 dimercur + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef __COMROBOT_H__ +#define __COMROBOT_H__ + +#include "messages.h" +#include + +using namespace std; + +/** + * Class used for communicating with robot over serial + * + * @brief Communication class with robot + * + */ +class ComRobot { +public: + + /** + * Constructor + */ + ComRobot() { + } + + /** + * Destructor + */ + virtual ~ComRobot() { + } + + /** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ + int Open(); + + /** + * Open serial link with robot + * @param usart Filename of usart to open + * @return File descriptor + * @throw std::runtime_error if it fails + */ + int Open(string usart); + + /** + * Close serial link + * @return Success if above 0, failure if below 0 + */ + int Close(); + + /** + * Send a message to robot + * @param msg Message to send to robot + * @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command + * @attention Input message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write produce an answer message. You have to dispose it (delete) when you have finished using it + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ + Message *Write(Message* msg); + + /** + * Function called at beginning of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Pre() { + } + + /** + * Function called at end of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Post() { + } + + Message *SendCommand(Message* msg, MessageID answerID, int maxRetries); + + static Message *Ping() { + return new Message(MESSAGE_ROBOT_PING); + } + + static Message *Reset() { + return new Message(MESSAGE_ROBOT_RESET); + } + + static Message *PowerOff() { + return new Message(MESSAGE_ROBOT_POWEROFF); + } + + static Message *StartWithWD() { + return new Message(MESSAGE_ROBOT_START_WITH_WD); + } + + static Message *StartWithoutWD() { + return new Message(MESSAGE_ROBOT_START_WITHOUT_WD); + } + + static Message *ReloadWD() { + return new Message(MESSAGE_ROBOT_RELOAD_WD); + } + + static Message *Move(int length) { + return new MessageInt(MESSAGE_ROBOT_MOVE, length); + } + + static Message *Turn(int angle) { + return new MessageInt(MESSAGE_ROBOT_TURN, angle); + } + + static Message *Stop() { + return new Message(MESSAGE_ROBOT_STOP); + } + + static Message *GoForward() { + return new Message(MESSAGE_ROBOT_GO_FORWARD); + } + + static Message *GoBackward() { + return new Message(MESSAGE_ROBOT_GO_BACKWARD); + } + + static Message *GoLeft() { + return new Message(MESSAGE_ROBOT_GO_LEFT); + } + + static Message *GoRight() { + return new Message(MESSAGE_ROBOT_GO_RIGHT); + } + + static Message *GetBattery() { + return new Message(MESSAGE_ROBOT_BATTERY_GET); + } + + static Message *GetState() { + return new Message(MESSAGE_ROBOT_STATE_GET); + } + +protected: + /** + * Serial link file descriptor + */ + int fd; + + /** + * Get an answer from robot + * @return String containing answer from robot + * @attention Read method is blocking until a message is received (timeout of 500 ms) + * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously + */ + string Read(); + + /** + * Convert a string to its message representation (when receiving data from robot) + * @param s String from robot containing answer + * @return Message corresponding to received array of char + */ + Message* StringToMessage(string s); + + /** + * Convert a message to its string representation (for sending command to robot) + * @param msg Message to be sent to robot + * @return String containing command to robot + */ + string MessageToString(Message *msg); + + /** + * Add a checksum and carriage return to a command string + * @param[in,out] s String containing command for robot, without ending char (carriage return) + */ + void AddChecksum(string &s); + + /** + * Verify if checksum of an incoming answer from robot is valid, + * then remove checksum from incoming answer (if checksum is ok) + * @param[in,out] s String containing incoming answer from robot + * @return true is checksum is valid, false otherwise. + */ + bool VerifyChecksum(string &s); +}; + +#endif /* __COMROBOT_H__ */ diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 80a7712..e6d6c63 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -8,11 +8,12 @@ file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.cpp - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.cpp - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/commonitor.cpp file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.h file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/main.cpp file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.h + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/commonitor.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/messages.h diff --git a/software/simulateur/nbproject/private/private.xml b/software/simulateur/nbproject/private/private.xml index aef7ea3..9b42543 100644 --- a/software/simulateur/nbproject/private/private.xml +++ b/software/simulateur/nbproject/private/private.xml @@ -6,6 +6,8 @@ - + + file:/home/raphael/Documents/real_time/software/simulateur/main.cpp + From 33f7b5ee5a2ed7545f154fdbbdbf1d0cfb1d04d0 Mon Sep 17 00:00:00 2001 From: Raphael Date: Tue, 31 Mar 2020 15:16:19 +0200 Subject: [PATCH 18/20] remove old comrobot --- .../superviseur-robot/lib/comrobot.cpp | 421 ------------------ .../superviseur-robot/lib/comrobot.h | 198 -------- 2 files changed, 619 deletions(-) delete mode 100644 software/raspberry/superviseur-robot/lib/comrobot.cpp delete mode 100644 software/raspberry/superviseur-robot/lib/comrobot.h diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp deleted file mode 100644 index cd1c2f8..0000000 --- a/software/raspberry/superviseur-robot/lib/comrobot.cpp +++ /dev/null @@ -1,421 +0,0 @@ -/* - * Copyright (C) 2018 dimercur - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include "comrobot.h" - -#include -#include -#include -#include -#include - -#include -#include - -#ifdef __SIMULATION__ -#include -#include -#include -int sock = 0; -#define PORT 6699 -#endif - -#ifdef __FOR_PC__ -#define USART_FILENAME "/dev/ttyUSB0" -#else -#define USART_FILENAME "/dev/ttyS0" -#endif /* __FOR_PC__ */ - -/* - * Constants to be used for communicating with robot. Contains command tag - */ -const char LABEL_ROBOT_PING = 'p'; -const char LABEL_ROBOT_RESET = 'r'; -const char LABEL_ROBOT_START_WITH_WD = 'W'; -const char LABEL_ROBOT_START_WITHOUT_WD = 'u'; -const char LABEL_ROBOT_RELOAD_WD = 'w'; -const char LABEL_ROBOT_MOVE = 'M'; -const char LABEL_ROBOT_TURN = 'T'; -const char LABEL_ROBOT_GET_BATTERY = 'v'; -const char LABEL_ROBOT_GET_STATE = 'b'; -const char LABEL_ROBOT_POWEROFF = 'z'; - -const char LABEL_ROBOT_OK = 'O'; -const char LABEL_ROBOT_ERROR = 'E'; -const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C'; - -const char LABEL_ROBOT_SEPARATOR_CHAR = '='; -const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r) - -/** - * Open serial link with robot - * @return File descriptor - * @throw std::runtime_error if it fails - */ -int ComRobot::Open() { - return this->Open(USART_FILENAME); -} - -/** - * Open serial link with robot - * @param usart Filename of usart to open - * @return File descriptor - * @throw std::runtime_error if it fails - */ -int ComRobot::Open(string usart) { - struct termios options; - -#ifdef __SIMULATION__ - - struct sockaddr_in serv_addr; - if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { - printf("\n Socket creation error \n"); - return -1; - } - struct timeval tv; - tv.tv_sec = 0; - tv.tv_usec = 80000; - setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); - - serv_addr.sin_family = AF_INET; - serv_addr.sin_port = htons(PORT); - - // Convert IPv4 and IPv6 addresses from text to binary form - if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) { - printf("\nInvalid address/ Address not supported \n"); - return -1; - } - - if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) { - return -1; - } - return 1; -#else - - fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode - if (fd == -1) { - cerr << "[" << __PRETTY_FUNCTION__ << "] Unable to open UART (" << usart << "). Ensure it is not in use by another application" << endl << flush; - throw std::runtime_error{"Unable to open UART"}; - exit(EXIT_FAILURE); - } else { - fcntl(fd, F_SETFL, 0); - tcgetattr(fd, &options); - options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); - cfsetospeed(&options, B9600); - cfsetispeed(&options, B9600); - options.c_cc[VMIN] = 0; - options.c_cc[VTIME] = 1; /* Timeout of 100 ms per character */ - tcsetattr(fd, TCSANOW, &options); - } - - return fd; -#endif -} - -/** - * Close serial link - * @return Success if above 0, failure if below 0 - */ -int ComRobot::Close() { -#ifdef __SIMULATION__ - return close(sock); -#elif - return close(fd); -#endif -} - -/** - * Send a message to robot - * @param msg Message to send to robot - * @return 1 if success, 0 otherwise - * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself - * @attention Write is blocking until message is written into buffer (linux side) - * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously - */ -Message *ComRobot::Write(Message* msg) { - Message *msgAnswer; - string s; - - if (this->fd != -1) { - - Write_Pre(); - - s = MessageToString(msg); -#ifdef __SIMULATION__ - - char buffer[1024] = {0}; - cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush; - send(sock, s.c_str(), s.length(), MSG_NOSIGNAL); - - int valread = read(sock, buffer, 1024); - - 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); - cout << "Response: " << buffer << ", id: " << msgAnswer->GetID() << endl; - } - -#else - AddChecksum(s); - - //cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write - - if (count < 0) { - cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush; - msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR); - } else { /* write successfull, read answer from robot */ - - try { - s = Read(); - //cout << "Answer = "<fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) - if (rxLength == 0) { // timeout - // try again - rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) - if (rxLength == 0) { // re-timeout: it sucks ! - throw std::runtime_error{"ComRobot::Read: Timeout when reading from com port"}; - } - } else if (rxLength < 0) { // big pb ! - throw std::runtime_error{"ComRobot::Read: Unknown problem when reading from com port"}; - } else { // everything ok - if ((receivedChar != '\r') && (receivedChar != '\n')) s += receivedChar; - } - } while ((receivedChar != '\r') && (receivedChar != '\n')); - - return s; -} - -Message *ComRobot::SendCommand(Message* msg, MessageID answerID, int maxRetries) { - int counter = maxRetries; - Message *msgSend; - Message *msgRcv; - Message *msgTmp; - - do { - msgSend = msg->Copy(); - cout << "S => " << msgSend->ToString() << endl << flush; - msgTmp = Write(msgSend); - cout << "R <= " << msgTmp->ToString() << endl << flush; - - if (msgTmp->CompareID(answerID)) counter = 0; - else counter--; - - if (counter == 0) msgRcv = msgTmp->Copy(); - - delete(msgTmp); - } while (counter); - - delete (msg); - - return msgRcv; -} - -/** - * Convert an array of char to its message representation (when receiving data from stm32) - * @param bytes Array of char - * @return Message corresponding to received array of char - */ -Message* ComRobot::StringToMessage(string s) { - Message *msg; - - switch (s[0]) { - case LABEL_ROBOT_OK: - msg = new Message(MESSAGE_ANSWER_ACK); - break; - case LABEL_ROBOT_ERROR: - msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); - break; - case LABEL_ROBOT_UNKNOWN_COMMAND: - msg = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND); - break; - case '0': - msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY); - break; - case '1': - msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW); - break; - case '2': - msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL); - break; - default: - msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); - cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << s << ")" << endl << flush; - } - - return msg; -} - -/** - * Convert a message to its array of char representation (for sending command to stm32) - * @param msg Message to be sent to robot - * @param buffer Array of char, image of message to send - */ -string ComRobot::MessageToString(Message *msg) { - string s; - - float val_f; - int val_i; - unsigned char *b; - - switch (msg->GetID()) { - case MESSAGE_ROBOT_PING: - s += LABEL_ROBOT_PING; - break; - case MESSAGE_ROBOT_RESET: - s += LABEL_ROBOT_RESET; - break; - case MESSAGE_ROBOT_POWEROFF: - s += LABEL_ROBOT_POWEROFF; - break; - case MESSAGE_ROBOT_START_WITHOUT_WD: - s += LABEL_ROBOT_START_WITHOUT_WD; - break; - case MESSAGE_ROBOT_START_WITH_WD: - s += LABEL_ROBOT_START_WITH_WD; - break; - case MESSAGE_ROBOT_RELOAD_WD: - s += LABEL_ROBOT_RELOAD_WD; - break; - case MESSAGE_ROBOT_BATTERY_GET: - s += LABEL_ROBOT_GET_BATTERY; - break; - case MESSAGE_ROBOT_STATE_GET: - s += LABEL_ROBOT_GET_STATE; - break; - case MESSAGE_ROBOT_GO_FORWARD: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(500000)); - break; - case MESSAGE_ROBOT_GO_BACKWARD: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(-500000)); - break; - case MESSAGE_ROBOT_GO_LEFT: - s += LABEL_ROBOT_TURN; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(-500000)); - break; - case MESSAGE_ROBOT_GO_RIGHT: - s += LABEL_ROBOT_TURN; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(500000)); - break; - case MESSAGE_ROBOT_STOP: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(0)); - break; - case MESSAGE_ROBOT_MOVE: - s += LABEL_ROBOT_MOVE; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(((MessageInt*) msg)->GetValue())); - break; - case MESSAGE_ROBOT_TURN: - s += LABEL_ROBOT_TURN; - s += LABEL_ROBOT_SEPARATOR_CHAR; - s.append(to_string(((MessageInt*) msg)->GetValue())); - break; - default: - cerr << "[" << __PRETTY_FUNCTION__ << "] Invalid message for robot (" << msg->ToString() << ")" << endl << flush; - throw std::runtime_error{"Invalid message"}; - } - - return s; -} - -/** - * Add a checksum and carriage return to a command string - * @param[in,out] s String containing command for robot, without ending char (carriage return) - */ -void ComRobot::AddChecksum(string &s) { - unsigned char checksum = 0; - - for (string::iterator it = s.begin(); it != s.end(); ++it) { - checksum ^= (unsigned char) *it; - } - - s += (char) checksum; // Add calculated checksum - s += (char) LABEL_ROBOT_ENDING_CHAR; -} - -/** - * Verify if checksum of an incoming answer from robot is valid, - * then remove checksum from incoming answer (if checksum is ok) - * @param[in,out] s String containing incoming answer from robot - * @return true is checksum is valid, false otherwise. - */ -bool ComRobot::VerifyChecksum(string &s) { - unsigned char checksum = 0; - - for (string::iterator it = s.begin(); it != s.end(); ++it) { - checksum ^= (unsigned char) *it; - } - - if (checksum == 0) { // checksum is ok, remove last char of string (checksum) - s.pop_back(); // remove last char - return true; - } else return false; -} diff --git a/software/raspberry/superviseur-robot/lib/comrobot.h b/software/raspberry/superviseur-robot/lib/comrobot.h deleted file mode 100644 index d6bdd11..0000000 --- a/software/raspberry/superviseur-robot/lib/comrobot.h +++ /dev/null @@ -1,198 +0,0 @@ -/* - * Copyright (C) 2018 dimercur - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#ifndef __COMROBOT_H__ -#define __COMROBOT_H__ - -#include "messages.h" -#include - -using namespace std; - -/** - * Class used for communicating with robot over serial - * - * @brief Communication class with robot - * - */ -class ComRobot { -public: - - /** - * Constructor - */ - ComRobot() { - } - - /** - * Destructor - */ - virtual ~ComRobot() { - } - - /** - * Open serial link with robot - * @return File descriptor - * @throw std::runtime_error if it fails - */ - int Open(); - - /** - * Open serial link with robot - * @param usart Filename of usart to open - * @return File descriptor - * @throw std::runtime_error if it fails - */ - int Open(string usart); - - /** - * Close serial link - * @return Success if above 0, failure if below 0 - */ - int Close(); - - /** - * Send a message to robot - * @param msg Message to send to robot - * @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command - * @attention Input message is destroyed (delete) after being sent. You do not need to delete it yourself - * @attention Write produce an answer message. You have to dispose it (delete) when you have finished using it - * @attention Write is blocking until message is written into buffer (linux side) - * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously - */ - Message *Write(Message* msg); - - /** - * Function called at beginning of Write method - * Use it to do some synchronization (call of mutex, for example) - */ - virtual void Write_Pre() { - } - - /** - * Function called at end of Write method - * Use it to do some synchronization (call of mutex, for example) - */ - virtual void Write_Post() { - } - - Message *SendCommand(Message* msg, MessageID answerID, int maxRetries); - - static Message *Ping() { - return new Message(MESSAGE_ROBOT_PING); - } - - static Message *Reset() { - return new Message(MESSAGE_ROBOT_RESET); - } - - static Message *PowerOff() { - return new Message(MESSAGE_ROBOT_POWEROFF); - } - - static Message *StartWithWD() { - return new Message(MESSAGE_ROBOT_START_WITH_WD); - } - - static Message *StartWithoutWD() { - return new Message(MESSAGE_ROBOT_START_WITHOUT_WD); - } - - static Message *ReloadWD() { - return new Message(MESSAGE_ROBOT_RELOAD_WD); - } - - static Message *Move(int length) { - return new MessageInt(MESSAGE_ROBOT_MOVE, length); - } - - static Message *Turn(int angle) { - return new MessageInt(MESSAGE_ROBOT_TURN, angle); - } - - static Message *Stop() { - return new Message(MESSAGE_ROBOT_STOP); - } - - static Message *GoForward() { - return new Message(MESSAGE_ROBOT_GO_FORWARD); - } - - static Message *GoBackward() { - return new Message(MESSAGE_ROBOT_GO_BACKWARD); - } - - static Message *GoLeft() { - return new Message(MESSAGE_ROBOT_GO_LEFT); - } - - static Message *GoRight() { - return new Message(MESSAGE_ROBOT_GO_RIGHT); - } - - static Message *GetBattery() { - return new Message(MESSAGE_ROBOT_BATTERY_GET); - } - - static Message *GetState() { - return new Message(MESSAGE_ROBOT_STATE_GET); - } - -protected: - /** - * Serial link file descriptor - */ - int fd; - - /** - * Get an answer from robot - * @return String containing answer from robot - * @attention Read method is blocking until a message is received (timeout of 500 ms) - * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously - */ - string Read(); - - /** - * Convert a string to its message representation (when receiving data from robot) - * @param s String from robot containing answer - * @return Message corresponding to received array of char - */ - Message* StringToMessage(string s); - - /** - * Convert a message to its string representation (for sending command to robot) - * @param msg Message to be sent to robot - * @return String containing command to robot - */ - string MessageToString(Message *msg); - - /** - * Add a checksum and carriage return to a command string - * @param[in,out] s String containing command for robot, without ending char (carriage return) - */ - void AddChecksum(string &s); - - /** - * Verify if checksum of an incoming answer from robot is valid, - * then remove checksum from incoming answer (if checksum is ok) - * @param[in,out] s String containing incoming answer from robot - * @return true is checksum is valid, false otherwise. - */ - bool VerifyChecksum(string &s); -}; - -#endif /* __COMROBOT_H__ */ From 978713bd1f7ebf2fad88d9ae096a81f33ae1824a Mon Sep 17 00:00:00 2001 From: Raphael Date: Tue, 31 Mar 2020 15:16:47 +0200 Subject: [PATCH 19/20] add new comrobot --- .../superviseur-robot/lib/comrobot.cpp | 421 ++++++++++++++++++ .../superviseur-robot/lib/comrobot.h | 198 ++++++++ 2 files changed, 619 insertions(+) create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.cpp create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.h diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp new file mode 100644 index 0000000..cd1c2f8 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.cpp @@ -0,0 +1,421 @@ +/* + * Copyright (C) 2018 dimercur + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "comrobot.h" + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef __SIMULATION__ +#include +#include +#include +int sock = 0; +#define PORT 6699 +#endif + +#ifdef __FOR_PC__ +#define USART_FILENAME "/dev/ttyUSB0" +#else +#define USART_FILENAME "/dev/ttyS0" +#endif /* __FOR_PC__ */ + +/* + * Constants to be used for communicating with robot. Contains command tag + */ +const char LABEL_ROBOT_PING = 'p'; +const char LABEL_ROBOT_RESET = 'r'; +const char LABEL_ROBOT_START_WITH_WD = 'W'; +const char LABEL_ROBOT_START_WITHOUT_WD = 'u'; +const char LABEL_ROBOT_RELOAD_WD = 'w'; +const char LABEL_ROBOT_MOVE = 'M'; +const char LABEL_ROBOT_TURN = 'T'; +const char LABEL_ROBOT_GET_BATTERY = 'v'; +const char LABEL_ROBOT_GET_STATE = 'b'; +const char LABEL_ROBOT_POWEROFF = 'z'; + +const char LABEL_ROBOT_OK = 'O'; +const char LABEL_ROBOT_ERROR = 'E'; +const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C'; + +const char LABEL_ROBOT_SEPARATOR_CHAR = '='; +const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r) + +/** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ +int ComRobot::Open() { + return this->Open(USART_FILENAME); +} + +/** + * Open serial link with robot + * @param usart Filename of usart to open + * @return File descriptor + * @throw std::runtime_error if it fails + */ +int ComRobot::Open(string usart) { + struct termios options; + +#ifdef __SIMULATION__ + + struct sockaddr_in serv_addr; + if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + printf("\n Socket creation error \n"); + return -1; + } + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 80000; + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); + + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(PORT); + + // Convert IPv4 and IPv6 addresses from text to binary form + if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) { + printf("\nInvalid address/ Address not supported \n"); + return -1; + } + + if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) { + return -1; + } + return 1; +#else + + fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode + if (fd == -1) { + cerr << "[" << __PRETTY_FUNCTION__ << "] Unable to open UART (" << usart << "). Ensure it is not in use by another application" << endl << flush; + throw std::runtime_error{"Unable to open UART"}; + exit(EXIT_FAILURE); + } else { + fcntl(fd, F_SETFL, 0); + tcgetattr(fd, &options); + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + cfsetospeed(&options, B9600); + cfsetispeed(&options, B9600); + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 1; /* Timeout of 100 ms per character */ + tcsetattr(fd, TCSANOW, &options); + } + + return fd; +#endif +} + +/** + * Close serial link + * @return Success if above 0, failure if below 0 + */ +int ComRobot::Close() { +#ifdef __SIMULATION__ + return close(sock); +#elif + return close(fd); +#endif +} + +/** + * Send a message to robot + * @param msg Message to send to robot + * @return 1 if success, 0 otherwise + * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ +Message *ComRobot::Write(Message* msg) { + Message *msgAnswer; + string s; + + if (this->fd != -1) { + + Write_Pre(); + + s = MessageToString(msg); +#ifdef __SIMULATION__ + + char buffer[1024] = {0}; + cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush; + send(sock, s.c_str(), s.length(), MSG_NOSIGNAL); + + int valread = read(sock, buffer, 1024); + + 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); + cout << "Response: " << buffer << ", id: " << msgAnswer->GetID() << endl; + } + +#else + AddChecksum(s); + + //cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write + + if (count < 0) { + cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush; + msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR); + } else { /* write successfull, read answer from robot */ + + try { + s = Read(); + //cout << "Answer = "<fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) + if (rxLength == 0) { // timeout + // try again + rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) + if (rxLength == 0) { // re-timeout: it sucks ! + throw std::runtime_error{"ComRobot::Read: Timeout when reading from com port"}; + } + } else if (rxLength < 0) { // big pb ! + throw std::runtime_error{"ComRobot::Read: Unknown problem when reading from com port"}; + } else { // everything ok + if ((receivedChar != '\r') && (receivedChar != '\n')) s += receivedChar; + } + } while ((receivedChar != '\r') && (receivedChar != '\n')); + + return s; +} + +Message *ComRobot::SendCommand(Message* msg, MessageID answerID, int maxRetries) { + int counter = maxRetries; + Message *msgSend; + Message *msgRcv; + Message *msgTmp; + + do { + msgSend = msg->Copy(); + cout << "S => " << msgSend->ToString() << endl << flush; + msgTmp = Write(msgSend); + cout << "R <= " << msgTmp->ToString() << endl << flush; + + if (msgTmp->CompareID(answerID)) counter = 0; + else counter--; + + if (counter == 0) msgRcv = msgTmp->Copy(); + + delete(msgTmp); + } while (counter); + + delete (msg); + + return msgRcv; +} + +/** + * Convert an array of char to its message representation (when receiving data from stm32) + * @param bytes Array of char + * @return Message corresponding to received array of char + */ +Message* ComRobot::StringToMessage(string s) { + Message *msg; + + switch (s[0]) { + case LABEL_ROBOT_OK: + msg = new Message(MESSAGE_ANSWER_ACK); + break; + case LABEL_ROBOT_ERROR: + msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); + break; + case LABEL_ROBOT_UNKNOWN_COMMAND: + msg = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND); + break; + case '0': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY); + break; + case '1': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW); + break; + case '2': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL); + break; + default: + msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); + cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << s << ")" << endl << flush; + } + + return msg; +} + +/** + * Convert a message to its array of char representation (for sending command to stm32) + * @param msg Message to be sent to robot + * @param buffer Array of char, image of message to send + */ +string ComRobot::MessageToString(Message *msg) { + string s; + + float val_f; + int val_i; + unsigned char *b; + + switch (msg->GetID()) { + case MESSAGE_ROBOT_PING: + s += LABEL_ROBOT_PING; + break; + case MESSAGE_ROBOT_RESET: + s += LABEL_ROBOT_RESET; + break; + case MESSAGE_ROBOT_POWEROFF: + s += LABEL_ROBOT_POWEROFF; + break; + case MESSAGE_ROBOT_START_WITHOUT_WD: + s += LABEL_ROBOT_START_WITHOUT_WD; + break; + case MESSAGE_ROBOT_START_WITH_WD: + s += LABEL_ROBOT_START_WITH_WD; + break; + case MESSAGE_ROBOT_RELOAD_WD: + s += LABEL_ROBOT_RELOAD_WD; + break; + case MESSAGE_ROBOT_BATTERY_GET: + s += LABEL_ROBOT_GET_BATTERY; + break; + case MESSAGE_ROBOT_STATE_GET: + s += LABEL_ROBOT_GET_STATE; + break; + case MESSAGE_ROBOT_GO_FORWARD: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(500000)); + break; + case MESSAGE_ROBOT_GO_BACKWARD: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(-500000)); + break; + case MESSAGE_ROBOT_GO_LEFT: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(-500000)); + break; + case MESSAGE_ROBOT_GO_RIGHT: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(500000)); + break; + case MESSAGE_ROBOT_STOP: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(0)); + break; + case MESSAGE_ROBOT_MOVE: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(((MessageInt*) msg)->GetValue())); + break; + case MESSAGE_ROBOT_TURN: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(((MessageInt*) msg)->GetValue())); + break; + default: + cerr << "[" << __PRETTY_FUNCTION__ << "] Invalid message for robot (" << msg->ToString() << ")" << endl << flush; + throw std::runtime_error{"Invalid message"}; + } + + return s; +} + +/** + * Add a checksum and carriage return to a command string + * @param[in,out] s String containing command for robot, without ending char (carriage return) + */ +void ComRobot::AddChecksum(string &s) { + unsigned char checksum = 0; + + for (string::iterator it = s.begin(); it != s.end(); ++it) { + checksum ^= (unsigned char) *it; + } + + s += (char) checksum; // Add calculated checksum + s += (char) LABEL_ROBOT_ENDING_CHAR; +} + +/** + * Verify if checksum of an incoming answer from robot is valid, + * then remove checksum from incoming answer (if checksum is ok) + * @param[in,out] s String containing incoming answer from robot + * @return true is checksum is valid, false otherwise. + */ +bool ComRobot::VerifyChecksum(string &s) { + unsigned char checksum = 0; + + for (string::iterator it = s.begin(); it != s.end(); ++it) { + checksum ^= (unsigned char) *it; + } + + if (checksum == 0) { // checksum is ok, remove last char of string (checksum) + s.pop_back(); // remove last char + return true; + } else return false; +} diff --git a/software/raspberry/superviseur-robot/lib/comrobot.h b/software/raspberry/superviseur-robot/lib/comrobot.h new file mode 100644 index 0000000..d6bdd11 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.h @@ -0,0 +1,198 @@ +/* + * Copyright (C) 2018 dimercur + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef __COMROBOT_H__ +#define __COMROBOT_H__ + +#include "messages.h" +#include + +using namespace std; + +/** + * Class used for communicating with robot over serial + * + * @brief Communication class with robot + * + */ +class ComRobot { +public: + + /** + * Constructor + */ + ComRobot() { + } + + /** + * Destructor + */ + virtual ~ComRobot() { + } + + /** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ + int Open(); + + /** + * Open serial link with robot + * @param usart Filename of usart to open + * @return File descriptor + * @throw std::runtime_error if it fails + */ + int Open(string usart); + + /** + * Close serial link + * @return Success if above 0, failure if below 0 + */ + int Close(); + + /** + * Send a message to robot + * @param msg Message to send to robot + * @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command + * @attention Input message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write produce an answer message. You have to dispose it (delete) when you have finished using it + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ + Message *Write(Message* msg); + + /** + * Function called at beginning of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Pre() { + } + + /** + * Function called at end of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Post() { + } + + Message *SendCommand(Message* msg, MessageID answerID, int maxRetries); + + static Message *Ping() { + return new Message(MESSAGE_ROBOT_PING); + } + + static Message *Reset() { + return new Message(MESSAGE_ROBOT_RESET); + } + + static Message *PowerOff() { + return new Message(MESSAGE_ROBOT_POWEROFF); + } + + static Message *StartWithWD() { + return new Message(MESSAGE_ROBOT_START_WITH_WD); + } + + static Message *StartWithoutWD() { + return new Message(MESSAGE_ROBOT_START_WITHOUT_WD); + } + + static Message *ReloadWD() { + return new Message(MESSAGE_ROBOT_RELOAD_WD); + } + + static Message *Move(int length) { + return new MessageInt(MESSAGE_ROBOT_MOVE, length); + } + + static Message *Turn(int angle) { + return new MessageInt(MESSAGE_ROBOT_TURN, angle); + } + + static Message *Stop() { + return new Message(MESSAGE_ROBOT_STOP); + } + + static Message *GoForward() { + return new Message(MESSAGE_ROBOT_GO_FORWARD); + } + + static Message *GoBackward() { + return new Message(MESSAGE_ROBOT_GO_BACKWARD); + } + + static Message *GoLeft() { + return new Message(MESSAGE_ROBOT_GO_LEFT); + } + + static Message *GoRight() { + return new Message(MESSAGE_ROBOT_GO_RIGHT); + } + + static Message *GetBattery() { + return new Message(MESSAGE_ROBOT_BATTERY_GET); + } + + static Message *GetState() { + return new Message(MESSAGE_ROBOT_STATE_GET); + } + +protected: + /** + * Serial link file descriptor + */ + int fd; + + /** + * Get an answer from robot + * @return String containing answer from robot + * @attention Read method is blocking until a message is received (timeout of 500 ms) + * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously + */ + string Read(); + + /** + * Convert a string to its message representation (when receiving data from robot) + * @param s String from robot containing answer + * @return Message corresponding to received array of char + */ + Message* StringToMessage(string s); + + /** + * Convert a message to its string representation (for sending command to robot) + * @param msg Message to be sent to robot + * @return String containing command to robot + */ + string MessageToString(Message *msg); + + /** + * Add a checksum and carriage return to a command string + * @param[in,out] s String containing command for robot, without ending char (carriage return) + */ + void AddChecksum(string &s); + + /** + * Verify if checksum of an incoming answer from robot is valid, + * then remove checksum from incoming answer (if checksum is ok) + * @param[in,out] s String containing incoming answer from robot + * @return true is checksum is valid, false otherwise. + */ + bool VerifyChecksum(string &s); +}; + +#endif /* __COMROBOT_H__ */ From 4208c91f0e5a049722dafb5b61cc5757e2b019d7 Mon Sep 17 00:00:00 2001 From: Raphael Date: Tue, 31 Mar 2020 15:39:09 +0200 Subject: [PATCH 20/20] robot whitout WD perfect + (with WD and vision not ok) --- .../nbproject/private/private.xml | 4 - .../raspberry/superviseur-robot/tasks.cpp | 81 +++++++++++-------- .../simulateur/nbproject/private/private.xml | 4 +- 3 files changed, 49 insertions(+), 40 deletions(-) diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index e6d6c63..9166cfc 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -9,10 +9,6 @@ file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.cpp file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.h - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/main.cpp - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.h - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/commonitor.cpp - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.cpp file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/messages.h diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 376438e..7ff7692 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -428,6 +428,10 @@ void Tasks::ReceiveFromMonTask(void *arg) { 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); @@ -603,39 +607,42 @@ void Tasks::OpenComRobot(void *arg) { //PAS DE SOUCIS AU REDEMARAGE * @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); + } } } } @@ -647,18 +654,22 @@ 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 */ - /**************************************************************************************/ 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 */ + /**************************************************************************************/ + + //Boolean to get the battery rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); killBattery=0; @@ -795,6 +806,10 @@ void Tasks::DetectLostSupRob(void *arg){ 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); diff --git a/software/simulateur/nbproject/private/private.xml b/software/simulateur/nbproject/private/private.xml index 9b42543..aef7ea3 100644 --- a/software/simulateur/nbproject/private/private.xml +++ b/software/simulateur/nbproject/private/private.xml @@ -6,8 +6,6 @@ - - file:/home/raphael/Documents/real_time/software/simulateur/main.cpp - +