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);
     }
+    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,18 +240,25 @@ void Tasks::ServerTask(void *arg) {
     /**************************************************************************************/
     /* The task server starts here                                                        */
     /**************************************************************************************/
-    rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
-    status = monitor.Open(SERVER_PORT);
-    rt_mutex_release(&mutex_monitor);
+    while(1){
+        rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+        status = monitor.Open(SERVER_PORT);
+        rt_mutex_release(&mutex_monitor);
 
-    cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
+        cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
+
+        if (status < 0) throw std::runtime_error {
+            "Unable to start server on port " + std::to_string(SERVER_PORT)
+        };
+        monitor.AcceptClient(); // Wait the monitor client
+        cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
+        rt_sem_broadcast(&sem_serverOk);
+        rt_sem_p(&sem_restartServer, TM_INFINITE);
+        rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+        monitor.Close();
+        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);
 }
 
 /**
@@ -227,6 +274,7 @@ void Tasks::SendToMonTask(void* arg) {
     /**************************************************************************************/
     /* The task sendToMon starts here                                                     */
     /**************************************************************************************/
+    
     rt_sem_p(&sem_serverOk, TM_INFINITE);
 
     while (1) {
@@ -252,31 +300,52 @@ 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;
 
-    while (1) {
-        msgRcv = monitor.Read();
-        cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
+        while (1) {
+            msgRcv = monitor.Read();
+            cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
 
-        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) ||
-                msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
-                msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
-                msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
-                msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
+            if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
+                delete(msgRcv);
+                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;
 
-            rt_mutex_acquire(&mutex_move, TM_INFINITE);
-            move = msgRcv->GetID();
-            rt_mutex_release(&mutex_move);
+            } 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
         }
-        delete(msgRcv); // mus be deleted manually, no consumer
+        
     }
 }
 
@@ -295,21 +364,43 @@ 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);
-        status = robot.Open();
-        rt_mutex_release(&mutex_robot);
-        cout << status;
-        cout << ")" << endl << flush;
+        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();
+            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);
+            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
+        }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
         }
-        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);
-        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) {
+        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 = 1;
+            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,24 +512,43 @@ 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);
-            
-            cout << " move: " << cpMove;
-            
-            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-            robot.Write(new Message((MessageID)cpMove));
-            rt_mutex_release(&mutex_robot);
+            if (cpMove != previousMove || cpMove != MESSAGE_ROBOT_STOP) {
+                cout << " move: " << cpMove;
+
+                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+                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;
         }
-        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                                                              */
@@ -84,7 +88,10 @@ private:
     RT_MUTEX mutex_robot;
     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                                                   */
@@ -148,6 +163,19 @@ private:
      * @return Message read
      */
     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);
 
 };