software/raspberry/superviseur-robot/lib/comrobot.cpp

@@ -327,7 +327,6 @@ string ComRobot::MessageToString(Message *msg) {
             break;
         case MESSAGE_ROBOT_RELOAD_WD:
             s += LABEL_ROBOT_RELOAD_WD;
-            cout << "watchdog" << endl;
             break;
         case MESSAGE_ROBOT_BATTERY_GET:
             s += LABEL_ROBOT_GET_BATTERY;

software/raspberry/superviseur-robot/tasks.cpp

@@ -24,8 +24,8 @@
 #define PRIORITY_TMOVE 20
 #define PRIORITY_TSENDTOMON 22
 #define PRIORITY_TRECEIVEFROMMON 25
-#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 24
+#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22
-#define PRIORITY_TSTARTROBOTWITHWATCHDOG 23
+#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
 #define PRIORITY_TCAMERA 21
 
 
@@ -75,10 +75,10 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_mutex_create(&mutex_kill_battery, NULL)) {
+    /*if (err = rt_mutex_create(&mutex_robot_on, NULL)) {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
-    }
+    }*/
     
     
     cout << "Mutexes created successfully" << endl << flush;
@@ -106,10 +106,6 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_sem_create(&sem_askBattery, NULL, 0, S_FIFO)) {
-        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    }
     cout << "Semaphores created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -143,10 +139,6 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_task_create(&th_askBattery, "th_askBattery", 0, PRIORITY_TMOVE, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    }
     
     cout << "Tasks created successfully" << endl << flush;
 
@@ -196,11 +188,7 @@ void Tasks::Run() {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) {
+
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    }
-    
     cout << "Tasks launched" << endl << flush;
 }
 
@@ -362,16 +350,18 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
     int killBattery=0;
-    Message * msgSend;
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
     /**************************************************************************************/
+    Message* p_mess_answer_battery;
+    Message * msgSend;
+    rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
+    cout << "Start robot without watchdog (";
+    
     //Boolean to get the battery
     rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
     killBatteryBool=0;
     rt_mutex_release(&mutex_kill_battery);
-    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);
@@ -387,7 +377,13 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
         rt_task_set_periodic(NULL, TM_NOW, 500000000);
         while (killBattery==0) {
             rt_task_wait_period(NULL);
-            rt_sem_v(&sem_askBattery);
+            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;
             rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
             killBattery=killBatteryBool;
             rt_mutex_release(&mutex_kill_battery);
@@ -402,47 +398,56 @@ 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 killBattery=0;
+   
-    int cpt=0;
-    Message * msgSend;
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
     /**************************************************************************************/
-    //Boolean to get the battery
+    Message* p_mess_answer_battery;
-    rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
+    Message * msgSend;
-    killBatteryBool=0;
+    int cpt=1;
-    rt_mutex_release(&mutex_kill_battery);
+    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_kill_battery, TM_INFINITE);
+    killBatteryBool=0;
+    rt_mutex_release(&mutex_kill_battery);
+    
     cout << msgSend->GetID();
-    cout << ")" << endl;  
+    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 (killBattery==0) {
+        while (1) {
             cpt++;
-            if(cpt==2){
+            if(cpt%2==0){
+                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
                 robot.Write(robot.ReloadWD());
-                cpt=0;
+                rt_mutex_release(&mutex_robot);
             }
             rt_task_wait_period(NULL);
-            rt_sem_v(&sem_askBattery);
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-            rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
+            p_mess_answer_battery = robot.Write(robot.GetBattery());
-            killBattery=killBatteryBool;
+            rt_mutex_release(&mutex_robot);
-            rt_mutex_release(&mutex_kill_battery);
+            rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+            monitor.Write(p_mess_answer_battery);
+            rt_mutex_release(&mutex_monitor);
+            cout << endl << flush;
         }
     }
 }
@@ -477,7 +482,7 @@ void Tasks::MoveTask(void *arg) {
             cpMove = move;
             rt_mutex_release(&mutex_move);
             
-            //cout << " move: " << cpMove;
+            cout << " move: " << cpMove;
             
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
             robot.Write(new Message((MessageID)cpMove));
@@ -488,26 +493,6 @@ void Tasks::MoveTask(void *arg) {
 }
 
 
-void Tasks::AskBattery(void *arg){
-    Message* p_mess_answer_battery;
-    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                                                               */
-    /**************************************************************************************/
-    while(1){
-        rt_sem_p(&sem_askBattery, TM_INFINITE);
-        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-        p_mess_answer_battery = robot.Write(robot.GetBattery());
-        rt_mutex_release(&mutex_robot);
-        WriteInQueue(&q_messageToMon, p_mess_answer_battery);
-        cout << endl << flush;
-    }
-}
-
-
 /**
  * Write a message in a given queue
  * @param queue Queue identifier

software/raspberry/superviseur-robot/tasks.h

@@ -79,7 +79,6 @@ private:
     RT_TASK th_startRobotWithoutWatchdog;
     RT_TASK th_startRobotWithWatchdog;
     RT_TASK th_move;
-    RT_TASK th_askBattery;
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -98,7 +97,6 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobotWithoutWatchdog;
     RT_SEM sem_startRobotWithWatchdog;
-    RT_SEM sem_askBattery;
 
     /**********************************************************************/
     /* Message queues                                                     */
@@ -146,9 +144,6 @@ private:
     
 
     
-    
-    void AskBattery(void *arg);
-    
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/

software/simulateur/main.cpp

@@ -123,8 +123,7 @@ int main(int argc, char const *argv[]) {
         print_time(start_time);
         printf(" >>> I received a message : %s\n", buffer);
         string s = "";
-        //int error = simulate_error();
+        int error = simulate_error();
-        int error = 0;
         if (error == 0) {
             struct timespec t;
             long int e;