gestion des booléens de boucle

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

@@ -327,6 +327,7 @@ 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 22
-#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
+#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 24
+#define PRIORITY_TSTARTROBOTWITHWATCHDOG 23
 #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_robot_on, NULL)) {
+    if (err = rt_mutex_create(&mutex_kill_battery, NULL)) {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
-    }*/
+    }
     
     
     cout << "Mutexes created successfully" << endl << flush;
@@ -106,6 +106,10 @@ 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;
 
     /**************************************************************************************/
@@ -139,6 +143,10 @@ 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;
 
@@ -188,6 +196,10 @@ 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;
 }
@@ -350,18 +362,16 @@ 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);
@@ -377,13 +387,7 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
         rt_task_set_periodic(NULL, TM_NOW, 500000000);
         while (killBattery==0) {
             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;
+            rt_sem_v(&sem_askBattery);
             rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
             killBattery=killBatteryBool;
             rt_mutex_release(&mutex_kill_battery);
@@ -398,56 +402,47 @@ 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                                                    */
     /**************************************************************************************/
-    Message* p_mess_answer_battery;
-    Message * msgSend;
-    int cpt=1;
-    int err;
+    //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_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 << "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) {
+        while (killBattery==0) {
             cpt++;
-            if(cpt%2==0){
-                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            if(cpt==2){
                 robot.Write(robot.ReloadWD());
-                rt_mutex_release(&mutex_robot);
+                cpt=0;
             }
             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;
+            rt_sem_v(&sem_askBattery);
+            rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
+            killBattery=killBatteryBool;
+            rt_mutex_release(&mutex_kill_battery);
         }
     }
 }
@@ -482,7 +477,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));
@@ -493,6 +488,26 @@ 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,6 +79,7 @@ private:
     RT_TASK th_startRobotWithoutWatchdog;
     RT_TASK th_startRobotWithWatchdog;
     RT_TASK th_move;
+    RT_TASK th_askBattery;
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -97,6 +98,7 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobotWithoutWatchdog;
     RT_SEM sem_startRobotWithWatchdog;
+    RT_SEM sem_askBattery;
 
     /**********************************************************************/
     /* Message queues                                                     */
@@ -144,6 +146,9 @@ private:
     
 
     
+    
+    void AskBattery(void *arg);
+    
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/

software/simulateur/main.cpp

@@ -123,7 +123,8 @@ 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;