Add battery functionality

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.5:22</developmentServer>
         <platform>2</platform>
       </toolsSet>
       <dbx_gdbdebugger version="1">

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

@@ -7,12 +7,15 @@
     <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/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/main.cpp</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/tasks.h</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/lib/comrobot.h</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
-            <file>file:/home_pers/pehladik/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/lib/commonitor.h</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/tasks.h</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
+            <file>file:/home/touchais/Documents/4A/RT/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</file>
         </group>
     </open-files>
 </project-private>

software/raspberry/superviseur-robot/tasks.cpp

@@ -26,6 +26,7 @@
 #define PRIORITY_TRECEIVEFROMMON 25
 #define PRIORITY_TSTARTROBOT 20
 #define PRIORITY_TCAMERA 21
+#define PRIORITY_TBATTERY 31
 
 /*
  * Some remarks:
@@ -123,6 +124,10 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_create(&th_checkBattery, "th_checkBattery", 0, PRIORITY_TBATTERY, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -167,6 +172,10 @@ void Tasks::Run() {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_start(&th_checkBattery, (void(*)(void*)) & Tasks::CheckBatteryTask, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
 
     cout << "Tasks launched" << endl << flush;
 }
@@ -383,6 +392,41 @@ void Tasks::MoveTask(void *arg) {
     }
 }
 
+/**
+ * @brief Thread printing the robot battery
+ */
+void Tasks::CheckBatteryTask(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                                                               */
+    /**************************************************************************************/
+    
+    MessageBattery * batLevel;
+    int rs;
+    
+    rt_task_set_periodic(NULL, TM_NOW, 500000000);
+    
+    while(1) {
+        rt_task_wait_period(NULL);
+        
+        rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+        rs = robotStarted;
+        rt_mutex_release(&mutex_robotStarted);
+        if (rs == 1) {
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            batLevel = (MessageBattery*)robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
+            rt_mutex_release(&mutex_robot);
+
+            rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+            monitor.Write(batLevel);
+            rt_mutex_release(&mutex_monitor);
+        }
+    }
+}
+
 /**
  * Write a message in a given queue
  * @param queue Queue identifier

software/raspberry/superviseur-robot/tasks.h

@@ -14,7 +14,6 @@
  * You should have received a copy of the GNU General Public License
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-
 #ifndef __TASKS_H__
 #define __TASKS_H__
 
@@ -76,6 +75,7 @@ private:
     RT_TASK th_openComRobot;
     RT_TASK th_startRobot;
     RT_TASK th_move;
+    RT_TASK th_checkBattery;
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -132,6 +132,11 @@ private:
      */
     void MoveTask(void *arg);
     
+    /**
+     * @brief Thread handling control of the robot.
+     */
+    void CheckBatteryTask(void *arg);
+    
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/