Battery is now fully working.

software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.144-xenomai-22

@@ -1,4 +1,4 @@
-#Tue Mar 26 15:23:55 CET 2024
+#Tue Mar 26 16:47:57 CET 2024
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/main.cpp=c1711457904172
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1711457904197
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1711458297850
@@ -8,13 +8,13 @@
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1711458297867
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1711457904114
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1711457904187
-/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1711461453052
+/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1711467801732
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/README.md=c1711457904021
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1711458720366
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1711457904182
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1711457904238
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1711457904124
-/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1711463022931
+/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1711468071294
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1711457904151
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/img.h=c1711457904162
 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/.dep.inc=c1711458948692

software/raspberry/superviseur-robot/tasks.cpp

@@ -74,6 +74,10 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_batteryGet, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Mutexes created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -274,7 +278,9 @@ void Tasks::ReceiveFromMonTask(void *arg) {
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
             rt_sem_v(&sem_openComRobot);
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_BATTERY_GET)) {
-            printf("Battery handler\n\r");
+            rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE);
+            robotBatteryGet = true;
+            rt_mutex_release(&mutex_batteryGet);
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
             rt_sem_v(&sem_startRobot);
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
@@ -375,7 +381,7 @@ void Tasks::MoveTask(void *arg) {
 
     while (1) {
         rt_task_wait_period(NULL);
-        cout << "Periodic movement update";
+        //cout << "Periodic movement update";
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
         rs = robotStarted;
         rt_mutex_release(&mutex_robotStarted);
@@ -390,7 +396,7 @@ void Tasks::MoveTask(void *arg) {
             robot.Write(new Message((MessageID) cpMove));
             rt_mutex_release(&mutex_robot);
         }
-        cout << endl << flush;
+        //cout << endl << flush;
     }
 }
 
@@ -426,13 +432,12 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
     return msg;
 }
 
-
-
 /* OUR CODE */
 
 void Tasks::BatteryStatusTask(void * arg) {
     // Variables
     int rs(0);
+    bool getbatterytask(false);
 
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
@@ -440,22 +445,28 @@ void Tasks::BatteryStatusTask(void * arg) {
 
 
     /* The task starts here */
-    rt_task_set_periodic(NULL, TM_NOW, 50000000);
+    rt_task_set_periodic(NULL, TM_NOW, 500000000);
 
     while (1) {
         rt_task_wait_period(NULL);
-        std::cout << "Periodic battery level update\n\r" << std::flush;
+
+        // Verify that the robot has started
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
         rs = robotStarted;
         rt_mutex_release(&mutex_robotStarted);
-        if (rs =! 1)
-            continue;
 
+        rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE);
+        getbatterytask = robotBatteryGet;
+
+        if ((rs != 0) && getbatterytask) {
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
             Message* msg = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
-        std::cout << "Message Received: " << msg->ToString() << std::endl;
-        
             rt_mutex_release(&mutex_robot);
-        cout << endl << flush;
+            std::cout << "Message Received: " << msg->ToString() << std::endl;
+            robotBatteryGet = false;
+            getbatterytask = false;
+
+        }
+        rt_mutex_release(&mutex_batteryGet);
     }
 }

software/raspberry/superviseur-robot/tasks.h

@@ -66,7 +66,7 @@ private:
     ComRobot robot;
     int robotStarted = 0;
     int move = MESSAGE_ROBOT_STOP;
-    int robotBattery = 0;
+    bool robotBatteryGet = false;
 
     /**********************************************************************/
     /* Tasks                                                              */
@@ -86,6 +86,7 @@ private:
     RT_MUTEX mutex_robot;
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_move;
+    RT_MUTEX mutex_batteryGet;
 
     /**********************************************************************/
     /* Semaphores                                                         */