Add new tasks

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

@@ -6,12 +6,6 @@
     </data>
     <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/etud/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
-            <file>file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
-            <file>file:/home/etud/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
-            <file>file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</file>
-            <file>file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/commonitor.h</file>
-        </group>
+        <group/>
     </open-files>
 </project-private>

software/raspberry/superviseur-robot/tasks.cpp

@@ -24,9 +24,11 @@
 #define PRIORITY_TMOVE 20
 #define PRIORITY_TSENDTOMON 22
 #define PRIORITY_TRECEIVEFROMMON 25
-#define PRIORITY_TSTARTROBOT 20
+#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22
+#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
 #define PRIORITY_TCAMERA 21
 
+
 /*
  * Some remarks:
  * 1- This program is mostly a template. It shows you how to create tasks, semaphore
@@ -73,6 +75,55 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_killReceiveFromMon, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if (err = rt_mutex_create(&mutex_killVision, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if (err = rt_mutex_create(&mutex_searchArena, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
+    if (err = rt_mutex_create(&mutex_drawArena, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if (err = rt_mutex_create(&mutex_getPosition, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
+    if (err = rt_mutex_create(&mutex_killSendMon, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }    
+
+    if (err = rt_mutex_create(&mutex_killDetectLostSupRob, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    } 
+    
+    if (err = rt_mutex_create(&mutex_acquireImage, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }     
+
+    
+    
+    
+    /*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;
 
     /**************************************************************************************/
@@ -90,10 +141,24 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_sem_create(&sem_startRobot, NULL, 0, S_FIFO)) {
+    if (err = rt_sem_create(&sem_startRobotWithoutWatchdog, NULL, 0, S_FIFO)) {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
+    if (err = rt_sem_create(&sem_allow_StartReceive, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }   
+    
+    
+    //Initialization for some specific sem: Allow to run the first time
+    rt_sem_v(&sem_allow_StartReceive);
+    
     cout << "Semaphores created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -115,7 +180,11 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0)) {
+    if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
@@ -123,6 +192,7 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -159,7 +229,11 @@ void Tasks::Run() {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) {
+    if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
@@ -244,28 +318,119 @@ void Tasks::SendToMonTask(void* arg) {
  */
 void Tasks::ReceiveFromMonTask(void *arg) {
     Message *msgRcv;
+    bool killReceiveFromMonOk=0;
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
     
+    //Wait twin task to die if not first round
+    rt_sem_p(&sem_allow_StartReceive, TM_INFINITE);
+    
+    //Reinitialize control boolean 
+    rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
+        killReceiveFromMon=0;
+    rt_mutex_release(&mutex_killReceiveFromMon);
+    
     /**************************************************************************************/
     /* The task receiveFromMon starts here                                                */
     /**************************************************************************************/
     rt_sem_p(&sem_serverOk, TM_INFINITE);
     cout << "Received message from monitor activated" << endl << flush;
-
-    while (1) {
+    while (!killReceiveFromMonOk) {
         msgRcv = monitor.Read();
         cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
 
         if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
             delete(msgRcv);
-            exit(-1);
+            cout << "Connection to monitor lost" << endl;
+            monitor.Close();
+            
+            rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
+                killReceiveFromMon=1;
+            rt_mutex_release(&mutex_killReceiveFromMon);
+            
+            rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
+                killVision=1;
+            rt_mutex_release(&mutex_killVision);
+            
+            rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
+                killSendMon=1;
+            rt_mutex_release(&mutex_killSendMon);           
+             
+            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+                killBattery=1;
+            rt_mutex_release(&mutex_killBattery);
+
+            rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+                killDetectlostSupRob=1;
+            rt_mutex_release(&mutex_killDetectLostSupRob);
+            
+            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=0;
+            rt_mutex_release(&mutex_acquireImage);            
+            
+            //exit(-1);
+        
+        
+        } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
+            cout << "Command Open Camera Received" << endl << flush;
+            //start task Vision
+            
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
+            cout << "Command Open Communication with Robot Received" << endl << flush;
             rt_sem_v(&sem_openComRobot);
+            
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
-            rt_sem_v(&sem_startRobot);
+            cout << "Command Start Robot without Watchdog Received" << endl << flush;
+            rt_sem_v(&sem_startRobotWithoutWatchdog);
+            
+        } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
+            cout << "Command Start Robot with Watchdog Received" << endl << flush;
+            //start task robot with watchdog 
+
+        } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
+            cout << "Command Search Arena Received" << endl << flush;
+            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+                searchArena=1;
+            rt_mutex_release(&mutex_searchArena);
+            
+        } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
+            cout << "Command Get Root Position Received" << endl << flush;
+            rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+                getPosition=1;
+            rt_mutex_release(&mutex_getPosition);            
+
+        } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
+            cout << "Command Stop Getting Robot Position Received" << endl << flush;
+            rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+                getPosition=0;
+            rt_mutex_release(&mutex_getPosition); 
+
+        } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
+            cout << "Command Confirm Arena Received" << endl << flush;
+            rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
+                drawArena=1;
+            rt_mutex_release(&mutex_drawArena);
+            
+            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+                searchArena=0;
+            rt_mutex_release(&mutex_searchArena);
+           
+            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=1;
+            rt_mutex_release(&mutex_acquireImage);            
+ 
+        } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
+            cout << "Command Infirm Arena Received" << endl << flush;
+            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+                searchArena=0;
+            rt_mutex_release(&mutex_searchArena);
+            
+            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=1;
+            rt_mutex_release(&mutex_acquireImage);
+            
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
                 msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
                 msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@@ -276,8 +441,16 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             move = msgRcv->GetID();
             rt_mutex_release(&mutex_move);
         }
-        delete(msgRcv); // mus be deleted manually, no consumer
+        delete(msgRcv); // must be deleted manually, no consumer
+        
+        //Update loop condition
+        rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
+        killReceiveFromMonOk=killReceiveFromMon;
+        rt_mutex_release(&mutex_killReceiveFromMon);
+        
     }
+    
+    rt_sem_v(&sem_allow_StartReceive);
 }
 
 /**
@@ -314,38 +487,121 @@ void Tasks::OpenComRobot(void *arg) {
 }
 
 /**
+ * @brief Thread starting the communication with the robot.
+ *//**
  * @brief Thread starting the communication with the robot.
  */
-void Tasks::StartRobotTask(void *arg) {
+
+void Tasks::StartRobotTaskWithoutWatchdog(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;
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
     /**************************************************************************************/
-    while (1) {
+    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_killBattery, TM_INFINITE);
+    killBattery=0;
+    rt_mutex_release(&mutex_killBattery);
+    rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+    msgSend = robot.Write(robot.StartWithoutWD());
+    rt_mutex_release(&mutex_robot);
+    cout << msgSend->GetID();
+    cout << ")" << endl;  
+    cout << "Movement answer: " << msgSend->ToString() << endl << flush;
+    WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
 
-        Message * msgSend;
-        rt_sem_p(&sem_startRobot, TM_INFINITE);
-        cout << "Start robot without watchdog (";
-        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-        msgSend = robot.Write(robot.StartWithoutWD());
-        rt_mutex_release(&mutex_robot);
-        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);
+    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) {
+            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_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+            killBattery=killBattery;
+            rt_mutex_release(&mutex_killBattery);
         }
     }
 }
 
+
+/**
+ * @brief Thread starting the communication with the robot.
+ *//**
+ * @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);
+   
+    /**************************************************************************************/
+    /* The task startRobot starts here                                                    */
+    /**************************************************************************************/
+    Message* p_mess_answer_battery;
+    Message * msgSend;
+    int cpt=1;
+    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_killBattery, TM_INFINITE);
+    killBattery=0;
+    rt_mutex_release(&mutex_killBattery);
+    
+    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) {
+            cpt++;
+            if(cpt%2==0){
+                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+                robot.Write(robot.ReloadWD());
+                rt_mutex_release(&mutex_robot);
+            }
+            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;
+        }
+    }
+}
+
+
+
+
 /**
  * @brief Thread handling control of the robot.
  */
@@ -383,6 +639,7 @@ void Tasks::MoveTask(void *arg) {
     }
 }
 
+
 /**
  * Write a message in a given queue
  * @param queue Queue identifier

software/raspberry/superviseur-robot/tasks.h

@@ -66,15 +66,26 @@ private:
     ComRobot robot;
     int robotStarted = 0;
     int move = MESSAGE_ROBOT_STOP;
+    bool killBattery = 0;
+    bool killReceiveFromMon=0;
+    bool killVision=0;
+    bool searchArena=0;
+    bool killSendMon=0;
+    bool killDetectlostSupRob=0;
+    bool acquireImage=0;
+    bool getPosition=0;
+    bool drawArena=0;
+    
+    
     
-    /**********************************************************************/
     /* Tasks                                                              */
     /**********************************************************************/
     RT_TASK th_server;
     RT_TASK th_sendToMon;
     RT_TASK th_receiveFromMon;
     RT_TASK th_openComRobot;
-    RT_TASK th_startRobot;
+    RT_TASK th_startRobotWithoutWatchdog;
+    RT_TASK th_startRobotWithWatchdog;
     RT_TASK th_move;
     
     /**********************************************************************/
@@ -84,15 +95,28 @@ private:
     RT_MUTEX mutex_robot;
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_move;
-
+    RT_MUTEX mutex_killBattery;
+    RT_MUTEX mutex_killReceiveFromMon;
+    RT_MUTEX mutex_killVision;
+    RT_MUTEX mutex_searchArena;
+    RT_MUTEX mutex_drawArena;
+    RT_MUTEX mutex_getPosition;
+    RT_MUTEX mutex_killSendMon;
+    RT_MUTEX mutex_killStartRobWD;
+    RT_MUTEX mutex_killDetectLostSupRob;
+    RT_MUTEX mutex_killStartRob;
+    RT_MUTEX mutex_acquireImage;
+    
     /**********************************************************************/
     /* Semaphores                                                         */
     /**********************************************************************/
     RT_SEM sem_barrier;
     RT_SEM sem_openComRobot;
     RT_SEM sem_serverOk;
-    RT_SEM sem_startRobot;
-
+    RT_SEM sem_startRobotWithoutWatchdog;
+    RT_SEM sem_startRobotWithWatchdog;
+    RT_SEM sem_allow_StartReceive;
+    
     /**********************************************************************/
     /* Message queues                                                     */
     /**********************************************************************/
@@ -123,15 +147,22 @@ private:
     void OpenComRobot(void *arg);
 
     /**
-     * @brief Thread starting the communication with the robot.
+     * @brief Thread starting the communication with the robot without watchdog.
      */
-    void StartRobotTask(void *arg);
+    void StartRobotTaskWithoutWatchdog(void *arg);
+    
+    /**
+     * @brief Thread starting the communication with the robot with watchdog.
+     */
+    void StartRobotTaskWithWatchdog(void *arg);
     
     /**
      * @brief Thread handling control of the robot.
      */
     void MoveTask(void *arg);
     
+
+    
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/