suppression et ajouts des task.*

software/raspberry/superviseur-robot/tasks.cpp

@@ -24,8 +24,10 @@
 #define PRIORITY_TMOVE 20
 #define PRIORITY_TSENDTOMON 22
 #define PRIORITY_TRECEIVEFROMMON 25
-#define PRIORITY_TSTARTROBOT 20
+#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 24
+#define PRIORITY_TSTARTROBOTWITHWATCHDOG 23
 #define PRIORITY_TCAMERA 21 
+#define PRIORITY_TVISION 26 
 
 /*
  * Some remarks:
@@ -73,6 +75,30 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_killBattery, 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_acquireImage, 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);
+    }
     cout << "Mutexes created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -90,7 +116,19 @@ 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_askBattery, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    if (err = rt_sem_create(&sem_allowStartCaptureImage, NULL, 0, S_FIFO)) {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
@@ -115,7 +153,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 +165,14 @@ 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);
+    }
+   if (err = rt_task_create(&th_vision, "th_vision", 0, PRIORITY_TVISION, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -159,7 +209,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);
     }
@@ -167,7 +221,14 @@ 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);
+    }
+   /* if (err = rt_task_start(&th_vision, (void(*)(void*)) & Tasks::Vision, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }*/
     cout << "Tasks launched" << endl << flush;
 }
 
@@ -265,7 +326,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_START_WITHOUT_WD)) {
-            rt_sem_v(&sem_startRobot);
+            rt_sem_v(&sem_startRobotWithoutWatchdog);
+        } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
+            rt_sem_v(&sem_startRobotWithWatchdog);
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
                 msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
                 msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@@ -280,6 +343,9 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     }
 }
 
+
+
+
 /**
  * @brief Thread opening communication with the robot.
  */
@@ -313,28 +379,34 @@ void Tasks::OpenComRobot(void *arg) {
     }
 }
 
+
+
+
+
+
 /**
  * @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 killBatteryOk=0;
+    Message * msgSend;
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
     /**************************************************************************************/
-    while (1) {
+    //Boolean to get the battery
-
+    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-        Message * msgSend;
+    killBatteryOk=0;
-        rt_sem_p(&sem_startRobot, TM_INFINITE);
+    rt_mutex_release(&mutex_killBattery);
+    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);
     cout << msgSend->GetID();
     cout << ")" << endl;  
-
     cout << "Movement answer: " << msgSend->ToString() << endl << flush;
     WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
 
@@ -342,10 +414,69 @@ void Tasks::StartRobotTask(void *arg) {
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
         robotStarted = 1;
         rt_mutex_release(&mutex_robotStarted);
+        rt_task_set_periodic(NULL, TM_NOW, 500000000);
+        while (killBatteryOk==0) {
+            rt_task_wait_period(NULL);
+            rt_sem_v(&sem_askBattery);
+            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+            killBatteryOk=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);
+    int killBatteryOk=0;
+    int cpt=1;
+    Message * msgSend;
+    /**************************************************************************************/
+    /* The task startRobot starts here                                                    */
+    /**************************************************************************************/
+    //Boolean to get the battery
+    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+    killBattery=0;
+    rt_mutex_release(&mutex_killBattery);
+    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);
+    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 (killBatteryOk==0) {
+            cpt++;
+            if(cpt==2){
+                robot.Write(robot.ReloadWD());
+                cpt=0;
+            }
+            rt_task_wait_period(NULL);
+            rt_sem_v(&sem_askBattery);
+            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+            killBatteryOk=killBattery;
+            rt_mutex_release(&mutex_killBattery);
+        }
+    }
+}
+
+
 /**
  * @brief Thread handling control of the robot.
  */
@@ -360,7 +491,7 @@ void Tasks::MoveTask(void *arg) {
     /**************************************************************************************/
     /* The task starts here                                                               */
     /**************************************************************************************/
-    rt_task_set_periodic(NULL, TM_NOW, 100000000);
+    rt_task_set_periodic(NULL, TM_NOW, 1000000000);
 
     while (1) {
         rt_task_wait_period(NULL);
@@ -383,6 +514,113 @@ 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;
+    }
+}
+
+
+//This task works on a difficult principle that fully make it controllable through 
+// monitor, accessing booleans variables to set getPosition or searchArena
+
+
+
+void Tasks::Vision(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_p(&sem_askBattery, TM_INFINITE);
+    int killVisionOk=0;
+    Camera camera;
+    int acquireImageOk=1;
+    int searchArenaOk=0;
+    int drawArenaOk=0;
+    int getPositionOk=0;
+    Arena arena;
+    list<Position> positionList;
+    list<Position>::iterator it;
+    string strPos;
+    MessagePosition* msgPos;
+    Message* msgPosMsg;
+    MessageImg* msgImg;
+    Message* msgImgMsg;
+    msgPos->SetID(MESSAGE_CAM_POSITION);
+    msgImg->SetID(MESSAGE_CAM_IMAGE);
+    rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
+    killVision=0;
+    rt_mutex_release(&mutex_killVision);
+    camera.Open();
+    while(killVisionOk==0){
+        while(acquireImageOk==1){
+            Img img=camera.Grab();
+            if(searchArenaOk==1){
+                rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=0;
+                rt_mutex_release(&mutex_acquireImage);
+                acquireImageOk=acquireImage;
+                arena=img.SearchArena();
+                img.DrawArena(arena);
+            }
+            if(drawArenaOk==1){
+              //  img.DrawArena(arena);
+                rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=1;
+                rt_mutex_release(&mutex_acquireImage);
+                acquireImageOk=acquireImage;
+            }
+            if(getPositionOk==1){
+                //On démarre la recherche du robot dans la zone définie par l'arène
+                positionList=img.SearchRobot(arena);
+                //Définitition et assignation de l'itérateur de parcrous de la liste positionList
+                it=positionList.begin();
+                //Définition d'un messagePosition qui va contenir l'information (x,y)
+                msgPos->SetPosition(*it);
+                //Transformation en message classique
+                msgPosMsg=msgPos->Copy();
+                //Envoi
+                WriteInQueue(&q_messageToMon,msgPos);
+                //Dessis du robot sur l'image
+                img.DrawRobot(*it);
+            }
+            //Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas)
+            msgImg->SetImage(&img);
+            //Transformation en message classique
+            msgImgMsg=msgImg->Copy();
+            //Envoi
+            WriteInQueue(&q_messageToMon,msgImg);
+        }
+    }
+    rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+        searchArena=0;
+    rt_mutex_release(&mutex_searchArena);
+    searchArenaOk=1;
+    rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+        getPosition=0;
+    rt_mutex_release(&mutex_getPosition);
+    getPositionOk=1;
+}
+
+
+
+
+
 /**
  * Write a message in a given queue
  * @param queue Queue identifier

software/raspberry/superviseur-robot/tasks.h

@@ -66,6 +66,15 @@ 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                                                              */
@@ -74,8 +83,11 @@ private:
     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;
+    RT_TASK th_vision;
+    RT_TASK th_askBattery;
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -84,6 +96,12 @@ private:
     RT_MUTEX mutex_robot;
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_move;
+    RT_MUTEX mutex_killBattery;
+    RT_MUTEX mutex_killVision;
+    RT_MUTEX mutex_acquireImage;
+    RT_MUTEX mutex_searchArena;
+    RT_MUTEX mutex_drawArena;
+    RT_MUTEX mutex_getPosition;
 
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -91,7 +109,10 @@ private:
     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_askBattery;
+    RT_SEM sem_allowStartCaptureImage;
     
     /**********************************************************************/
     /* Message queues                                                     */
@@ -125,13 +146,25 @@ private:
     /**
      * @brief Thread starting the communication with the robot.
      */
-    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);
     
+    
+    void AskBattery(void *arg);
+    
+    
+    void Vision(void *arg);
+    
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/