Robot terminé : Tester les fonctionnalités

software/raspberry/superviseur-robot/nbproject/private/configurations.xml

@@ -41,7 +41,7 @@
     </conf>
     <conf name="Debug__RPI_" type="1">
       <toolsSet>
-        <developmentServer>pi@10.105.1.08:22</developmentServer>
+        <developmentServer>pi@10.105.1.07:22</developmentServer>
         <platform>2</platform>
       </toolsSet>
       <dbx_gdbdebugger version="1">

software/raspberry/superviseur-robot/nbproject/private/private.xml

@@ -7,12 +7,11 @@
     <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
     <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
         <group>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/tasks.h</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
+            <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.h</file>
+            <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.cpp</file>
+            <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
+            <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.h</file>
+            <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h</file>
         </group>
     </open-files>
 </project-private>

software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.07-pi-22

@@ -0,0 +1,38 @@
+#Wed Mar 17 12:14:13 CET 2021
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.h=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/superviseur.doxygen=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.xml=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/.gitignore=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.h=c1615975893000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/.gitignore=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.h=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/.dep.inc=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1615800255000
+VERSION=1.3
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/Makefile=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.h=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.h=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.cpp=c1615979649000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.h=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/README.md=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/README.md=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/main.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/gdbsudo.sh=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1615800255000
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.properties=c1615800255000

software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.7-pi-22

@@ -0,0 +1,38 @@
+#Fri Feb 26 10:29:03 CET 2021
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/README.md=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.xml=c1614324902000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.h=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.h=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.h=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/.dep.inc=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/.gitignore=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/.gitignore=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.cpp=c1614324901000
+VERSION=1.3
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.cpp=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.h=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.cpp=c1614331741000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/Makefile=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.h=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/README.md=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.properties=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/main.cpp=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/superviseur.doxygen=c1614324902000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/gdbsudo.sh=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.h=c1614329334000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1614324901000
+/home/alartigu/Bureau/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.cpp=c1614324901000

software/raspberry/superviseur-robot/tasks.cpp

@@ -26,6 +26,10 @@
 #define PRIORITY_TRECEIVEFROMMON 25
 #define PRIORITY_TSTARTROBOT 20
 #define PRIORITY_TCAMERA 21
+#define PRIORITY_TBATTERY 19
+#define PRIORITY_TWATCHDOG 31
+
+
 
 /*
  * Some remarks:
@@ -94,6 +98,14 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_sem_create(&sem_restartServer, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_sem_create(&sem_robotStopped, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Semaphores created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -123,6 +135,14 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_create(&th_getBattery, "th_getBattery", 0, PRIORITY_TBATTERY, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_task_create(&th_watchDog, "th_watchDog", 0, PRIORITY_TWATCHDOG, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -132,8 +152,21 @@ void Tasks::Init() {
         cerr << "Error msg queue create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    cout << "Queues created successfully" << endl << flush;
+    if ((err = rt_queue_create(&q_messageComRobot, "q_messageComRobot", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
+        cerr << "Error messageComRobot queue create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     
+    if ((err = rt_queue_create(&q_messageControlRobot, "q_messageControlRobot", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
+        cerr << "Error messageControlRobot queue create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if ((err = rt_queue_create(&q_messageControlCam, "q_messageControlCam", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
+        cerr << "Error messageControlCam queue create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    } 
+    cout << "Queues created successfully" << endl << flush;
 }
 
 /**
@@ -167,7 +200,14 @@ void Tasks::Run() {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-
+    if (err = rt_task_start(&th_getBattery, (void(*)(void*)) & Tasks::ReadBattery, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_task_start(&th_watchDog, (void(*)(void*)) & Tasks::WatchDog, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Tasks launched" << endl << flush;
 }
 
@@ -200,6 +240,7 @@ void Tasks::ServerTask(void *arg) {
     /**************************************************************************************/
     /* The task server starts here                                                        */
     /**************************************************************************************/
+    while(1){
         rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
         status = monitor.Open(SERVER_PORT);
         rt_mutex_release(&mutex_monitor);
@@ -212,6 +253,12 @@ void Tasks::ServerTask(void *arg) {
         monitor.AcceptClient(); // Wait the monitor client
         cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
         rt_sem_broadcast(&sem_serverOk);
+        rt_sem_p(&sem_restartServer, TM_INFINITE);
+        rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+        monitor.Close();
+        rt_mutex_release(&mutex_monitor);
+    }
+
 }
 
 /**
@@ -227,6 +274,7 @@ void Tasks::SendToMonTask(void* arg) {
     /**************************************************************************************/
     /* The task sendToMon starts here                                                     */
     /**************************************************************************************/
+    
     rt_sem_p(&sem_serverOk, TM_INFINITE);
 
     while (1) {
@@ -252,6 +300,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     /**************************************************************************************/
     /* The task receiveFromMon starts here                                                */
     /**************************************************************************************/
+    while(1){
     rt_sem_p(&sem_serverOk, TM_INFINITE);
     cout << "Received message from monitor activated" << endl << flush;
 
@@ -261,23 +310,43 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 
             if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
                 delete(msgRcv);
-            exit(-1);
-        } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
-            rt_sem_v(&sem_openComRobot);
-        } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
-            rt_sem_v(&sem_startRobot);
-        } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
+                WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
+                WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
+                WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
+                rt_sem_v(&sem_restartServer);  // A VERIFIER
+                break;
+
+            } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
+                WriteInQueue(&q_messageComRobot, msgRcv);
+                //rt_sem_v(&sem_openComRobot);
+            } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD) || msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
+                WriteInQueue(&q_messageControlRobot, msgRcv);
+                //rt_sem_v(&sem_startRobot);
+            } 
+            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);
             }
+
+            else if (msgRcv->CompareID(MESSAGE_CAM_OPEN) || 
+                    msgRcv->CompareID(MESSAGE_CAM_CLOSE) || 
+                    msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA) || 
+                    msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || 
+                    msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) || 
+                    msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) || 
+                    msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP))
+            {
+                WriteInQueue(&q_messageControlCam, msgRcv);
+            }
             delete(msgRcv); // mus be deleted manually, no consumer
         }
+        
+    }
 }
 
 /**
@@ -295,7 +364,9 @@ void Tasks::OpenComRobot(void *arg) {
     /* The task openComRobot starts here                                                  */
     /**************************************************************************************/
     while (1) {
-        rt_sem_p(&sem_openComRobot, TM_INFINITE);
+        Message * msg;
+        msg=ReadInQueue(&q_messageComRobot); 
+        if(msg->CompareID(MESSAGE_ROBOT_COM_OPEN)){
             cout << "Open serial com (";
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
             status = robot.Open();
@@ -310,6 +381,26 @@ void Tasks::OpenComRobot(void *arg) {
                 msgSend = new Message(MESSAGE_ANSWER_ACK);
             }
             WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
+        }else if(msg->CompareID(MESSAGE_ROBOT_COM_CLOSE)){ 
+            rt_sem_p(&sem_robotStopped, TM_INFINITE);
+            cout << "Close serial com (";
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            status = robot.Close();
+            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
+        }
+        //delete(msg);
+        //rt_sem_p(&sem_openComRobot, TM_INFINITE);
+        
     }
 }
 
@@ -326,23 +417,80 @@ void Tasks::StartRobotTask(void *arg) {
     /**************************************************************************************/
     while (1) {
 
+        Message * tmp;
         Message * msgSend;
-        rt_sem_p(&sem_startRobot, TM_INFINITE);
+        tmp = ReadInQueue(&q_messageControlRobot);
+        if (tmp -> CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)){
             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);
             }
+        } else if (tmp -> CompareID(MESSAGE_ROBOT_START_WITH_WD)){
+            cout << "Start robot with watchdog (";
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            msgSend = robot.Write(robot.StartWithWD());
+            rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
+            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);
+            } else {
+                rt_task_set_periodic(&th_watchDog,TM_NOW,0);
+            }
+            
+        } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
+            rt_task_set_periodic(&th_watchDog,TM_NOW,0);
+            cout << "Stopping Robot (";
+            rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+            robotStarted = 0;
+            rt_mutex_release(&mutex_robotStarted);
+            rt_task_sleep(100000000);
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            msgSend = robot.Write(robot.Reset());
+            rt_mutex_release(&mutex_robot);
+            rt_mutex_acquire(&mutex_move, TM_INFINITE);
+            move = MESSAGE_ROBOT_STOP;
+            rt_mutex_release(&mutex_move);
+            cout << msgSend->GetID();
+            cout << ")" << endl;
+            cout << "Movement answer: " << msgSend->ToString() << endl << flush;
+            WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
+            
+            rt_sem_v(&sem_robotStopped);
+        } 
+        
+   
+    }
+}
+
+
+void Tasks::WatchDog(void *arg) {
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+    // Synchronization barrier (waiting that all tasks are starting)
+    rt_sem_p(&sem_barrier, TM_INFINITE);
+    rt_task_set_periodic(NULL, TM_NOW, 0);
+    while (1) {
+        cout << "Realoading" << endl << flush;
+        rt_task_wait_period(NULL);
+        Message * msgSend;
+        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+        msgSend = robot.ReloadWD();
+        robot.Write(msgSend);
+        rt_mutex_release(&mutex_robot);
     }
 }
 
@@ -351,7 +499,9 @@ void Tasks::StartRobotTask(void *arg) {
  */
 void Tasks::MoveTask(void *arg) {
     int rs;
+    int previousMove = MESSAGE_ROBOT_GO_FORWARD;
     int cpMove;
+    int nb_error = 0;
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
@@ -362,25 +512,44 @@ void Tasks::MoveTask(void *arg) {
     /**************************************************************************************/
     rt_task_set_periodic(NULL, TM_NOW, 100000000);
 
+    Message * msgSend;
     while (1) {
         rt_task_wait_period(NULL);
-        cout << "Periodic movement update";
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
         rs = robotStarted;
         rt_mutex_release(&mutex_robotStarted);
         if (rs == 1) {
+            cout << "Periodic movement update";
             rt_mutex_acquire(&mutex_move, TM_INFINITE);
             cpMove = move;
             rt_mutex_release(&mutex_move);
-            
+            if (cpMove != previousMove || cpMove != MESSAGE_ROBOT_STOP) {
                 cout << " move: " << cpMove;
 
                 rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-            robot.Write(new Message((MessageID)cpMove));
+                msgSend = robot.Write(new Message((MessageID)cpMove));
                 rt_mutex_release(&mutex_robot);
+                if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
+                    nb_error = 0;
+                    previousMove = cpMove;
+                } else {
+                    nb_error++;
+                }
+                if (nb_error > 2) {
+                    cout << "ERREUR ROBOT DETECTED";
+                    WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); 
+                    rt_mutex_acquire(&mutex_move, TM_INFINITE);
+                    move = MESSAGE_ROBOT_STOP;
+                    rt_mutex_release(&mutex_move);
+                    rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+                    robotStarted = 0;
+                    rt_mutex_release(&mutex_robotStarted);
+                    rt_task_set_periodic(&th_watchDog,TM_NOW,0);
+                }
             }
             cout << endl << flush;
         }
+    }
 }
 
 /**
@@ -415,3 +584,33 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
     return msg;
 }
 
+void Tasks::ReadBattery(void *arg){    
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+    // Synchronization barrier (waiting that all tasks are starting)
+    rt_sem_p(&sem_barrier, TM_INFINITE);
+    
+    /**************************************************************************************/
+    /* The task starts here                                                               */
+    /**************************************************************************************/
+    rt_task_set_periodic(NULL, TM_NOW, 500000000);
+    
+    int rs;
+
+    while (1) {
+        rt_task_wait_period(NULL);
+        rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+        rs = robotStarted;
+        rt_mutex_release(&mutex_robotStarted);
+        if (rs == 1) {
+            cout << "Periodic battery get lvl" << endl << flush;
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            Message *msg = robot.Write( robot.GetBattery() ) ; 
+            rt_mutex_release(&mutex_robot);
+            if (msg->CompareID((MessageID)MESSAGE_ROBOT_BATTERY_LEVEL)) {
+                rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+                monitor.Write(msg);
+                rt_mutex_release(&mutex_monitor);
+            }
+        }
+    }
+}

software/raspberry/superviseur-robot/tasks.h

@@ -76,6 +76,10 @@ private:
     RT_TASK th_openComRobot;
     RT_TASK th_startRobot;
     RT_TASK th_move;
+    RT_TASK th_getBattery;
+    RT_TASK th_watchDog;
+    RT_TASK th_controlCamera;
+    RT_TASK th_picture;
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -85,6 +89,9 @@ private:
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_move;
     
+    RT_MUTEX mutex_camera;
+    RT_MUTEX mutex_controlStruct;
+    
     /**********************************************************************/
     /* Semaphores                                                         */
     /**********************************************************************/
@@ -92,12 +99,20 @@ private:
     RT_SEM sem_openComRobot;
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobot;
+    RT_SEM sem_watchdogStart;
+    RT_SEM sem_restartServer;
+    RT_SEM sem_robotStopped;
+    
+    
 
     /**********************************************************************/
     /* Message queues                                                     */
     /**********************************************************************/
     int MSG_QUEUE_SIZE;
     RT_QUEUE q_messageToMon;
+    RT_QUEUE q_messageComRobot;
+    RT_QUEUE q_messageControlRobot;
+    RT_QUEUE q_messageControlCam;
     
     /**********************************************************************/
     /* Tasks' functions                                                   */
@@ -149,6 +164,19 @@ private:
      */
     Message *ReadInQueue(RT_QUEUE *queue);
     
+    
+    /**
+     * Read the battery level of the robot
+     * @return Nothing
+     */
+    void ReadBattery(void *arg);
+    
+    /**
+     * Sends periodically a message to the robot
+     * @return Nothing
+     */
+    void WatchDog(void *arg);
+
 };
 
 #endif // __TASKS_H__