batterie ok

software/raspberry/superviseur-robot/tasks.cpp

@@ -26,6 +26,8 @@
 #define PRIORITY_TRECEIVEFROMMON 25
 #define PRIORITY_TSTARTROBOT 20
 #define PRIORITY_TCAMERA 21
+#define PRIORITY_TBATTERY 31
+
 
 /*
  * Some remarks:
@@ -73,6 +75,9 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    
+    
+    
     cout << "Mutexes created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -94,6 +99,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;
 
     /**************************************************************************************/
@@ -123,6 +132,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_TBATTERY, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -167,6 +180,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::Ask_Battery, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
 
     cout << "Tasks launched" << endl << flush;
 }
@@ -320,6 +337,7 @@ void Tasks::StartRobotTask(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_v(&sem_askBattery);
     
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
@@ -383,6 +401,34 @@ void Tasks::MoveTask(void *arg) {
     }
 }
 
+
+void Tasks::Ask_Battery(void *arg){
+    Message* p_mess_answer_battery;
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+    
+    rt_sem_p(&sem_barrier, TM_INFINITE);
+    
+    rt_sem_p(&sem_askBattery, TM_INFINITE);
+    
+    /**************************************************************************************/
+    /* The task starts here                                                               */
+    /**************************************************************************************/
+    rt_task_set_periodic(NULL, TM_NOW, 10000000000);
+    while (1) {
+        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;
+    }
+}
+
+
+
+
 /**
  * Write a message in a given queue
  * @param queue Queue identifier

software/raspberry/superviseur-robot/tasks.h

@@ -76,6 +76,7 @@ private:
     RT_TASK th_openComRobot;
     RT_TASK th_startRobot;
     RT_TASK th_move;
+    RT_TASK th_askBattery;
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -84,6 +85,7 @@ private:
     RT_MUTEX mutex_robot;
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_move;
+    RT_MUTEX mutex_askBattery;
 
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -92,6 +94,7 @@ private:
     RT_SEM sem_openComRobot;
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobot;
+    RT_SEM sem_askBattery;
 
     /**********************************************************************/
     /* Message queues                                                     */
@@ -132,6 +135,11 @@ private:
      */
     void MoveTask(void *arg);
     
+    
+    
+    void Ask_Battery(void *arg);
+    
+    
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/