vision funciton not fully working but almost

software/raspberry/superviseur-robot/tasks.cpp

@@ -27,6 +27,7 @@
 #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22
 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
 #define PRIORITY_TCAMERA 21
+#define PRIORITY_TVISION 26 
 
 
 /*
@@ -79,27 +80,26 @@ 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_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);
@@ -114,16 +114,13 @@ void Tasks::Init() {
         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;
 
     /**************************************************************************************/
@@ -149,12 +146,18 @@ 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);
+    }
     if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }   
-    
+    if (err = rt_sem_create(&sem_allowStartCapture, 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_allowStartReceive);
@@ -192,7 +195,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;
 
     /**************************************************************************************/
@@ -241,7 +251,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;
 }
 
@@ -387,7 +404,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
             cout << "Command Start Robot with Watchdog Received" << endl << flush;
-            //start task robot with watchdog 
+            rt_sem_v(&sem_startRobotWithoutWatchdog);
 
         } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
             cout << "Command Search Arena Received" << endl << flush;
@@ -430,7 +447,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             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) ||
@@ -453,6 +469,9 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     rt_sem_v(&sem_allowStartReceive);
 }
 
+
+
+
 /**
  * @brief Thread opening communication with the robot.
  */
@@ -486,29 +505,31 @@ void Tasks::OpenComRobot(void *arg) {
     }
 }
 
+
+
+
+
+
 /**
  * @brief Thread starting the communication with the robot.
  *//**
  * @brief Thread starting the communication with the robot.
  */
-
 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;
+    int killBatteryOk=0;
+    Message * msgSend;
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
     /**************************************************************************************/
-    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;
+    killBatteryOk=0;
     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);
@@ -522,17 +543,11 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
         robotStarted = 1;
         rt_mutex_release(&mutex_robotStarted);
         rt_task_set_periodic(NULL, TM_NOW, 500000000);
-        while (killBattery==0) {
+        while (killBatteryOk==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_sem_v(&sem_askBattery);
             rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-            killBattery=killBattery;
+            killBatteryOk=killBattery;
             rt_mutex_release(&mutex_killBattery);
         }
     }
@@ -544,64 +559,52 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
  *//**
  * @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                                                    */
     /**************************************************************************************/
-    Message* p_mess_answer_battery;
-    Message * msgSend;
-    int cpt=1;
-    int err;
+    //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);
-    
-    //boolean to ask battery
-    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-    killBattery=0;
-    rt_mutex_release(&mutex_killBattery);
-    
     cout << msgSend->GetID();
-    cout << ")" << endl;
-        
+    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) {
+        while (killBatteryOk==0) {
             cpt++;
-            if(cpt%2==0){
-                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            if(cpt==2){
                 robot.Write(robot.ReloadWD());
-                rt_mutex_release(&mutex_robot);
+                cpt=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_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.
  */
@@ -616,7 +619,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);
@@ -639,8 +642,130 @@ 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);
+    int killVisionOk=0;
+    Camera camera;
+    int acquireImageOk=0;
+    int searchArenaOk=0;
+    int drawArenaOk=0;
+    int getPositionOk=0;
+    Arena arena;
+    //Img* img;
+    list<Position> positionList;
+    list<Position>::iterator it;
+    string strPos;
+    //MessagePosition* msgPos;
+    Message* msgPosMsg;
+    MessageImg msgImg;
+    Message* msgImgMsg;
+    msgImg.SetID(MESSAGE_CAM_IMAGE);
+    //msgPos->SetID(MESSAGE_CAM_POSITION);
+    rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
+    killVision=0;
+    rt_mutex_release(&mutex_killVision);
+    rt_task_set_periodic(NULL, TM_NOW, 100000000);
+    camera.Open();  
+    while(killVisionOk==0){
+        rt_task_wait_period(NULL);
+        while(acquireImageOk==1){
+            rt_task_wait_period(NULL);
+            cout << "open cam" << endl;
+            Img img=camera.Grab();
+            cout << "after camera grab" << endl << endl << endl;
+            msgImg.SetImage(&img);
+            cout << "after set image in msgImg" << endl << endl << endl;
+            if(searchArenaOk==1){
+                rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=0;
+                rt_mutex_release(&mutex_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);
+            }
+            if(getPositionOk==1){
+                cout << "in GetPos=1" << endl << endl << endl;
+                //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);
+                MessagePosition msgPos(MESSAGE_CAM_POSITION,*it);
+                //Transformation en message classique
+                msgPosMsg=msgPos.Copy();
+                //Envoi
+                cout << "envoi au mon" << endl;
+                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_acquireImage, TM_INFINITE);
+            acquireImageOk=acquireImage;
+        rt_mutex_release(&mutex_acquireImage);
+        rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+            getPositionOk=getPosition;
+        rt_mutex_release(&mutex_getPosition);
+        rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+            searchArenaOk=searchArena;
+        rt_mutex_release(&mutex_searchArena);
+        rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
+            drawArenaOk=drawArena;
+        rt_mutex_release(&mutex_drawArena);
+    }
+    rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+        searchArena=0;
+    rt_mutex_release(&mutex_searchArena);
+    searchArenaOk=0;
+    rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+        getPosition=0;
+    rt_mutex_release(&mutex_getPosition);
+    getPositionOk=0;
+}
+
+
+
+/*
  * Write a message in a given queue
  * @param queue Queue identifier
  * @param msg Message to be stored

software/raspberry/superviseur-robot/tasks.h

@@ -77,7 +77,6 @@ private:
     bool drawArena=0;
     
     
-    
     /* Tasks                                                              */
     /**********************************************************************/
     RT_TASK th_server;
@@ -87,6 +86,8 @@ private:
     RT_TASK th_startRobotWithoutWatchdog;
     RT_TASK th_startRobotWithWatchdog;
     RT_TASK th_move;
+    RT_TASK th_vision;
+    RT_TASK th_askBattery;
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -106,7 +107,6 @@ private:
     RT_MUTEX mutex_killDetectLostSupRob;
     RT_MUTEX mutex_killStartRob;
     RT_MUTEX mutex_acquireImage;
-    
     /**********************************************************************/
     /* Semaphores                                                         */
     /**********************************************************************/
@@ -116,6 +116,8 @@ private:
     RT_SEM sem_startRobotWithoutWatchdog;
     RT_SEM sem_startRobotWithWatchdog;
     RT_SEM sem_allowStartReceive;
+    RT_SEM sem_askBattery;
+    RT_SEM sem_allowStartCapture;
     
     /**********************************************************************/
     /* Message queues                                                     */
@@ -161,7 +163,13 @@ private:
      */
     void MoveTask(void *arg);
     
-
+    
+    
+    void AskBattery(void *arg);
+    
+    
+    
+    void Vision(void *arg);
     
     /**********************************************************************/
     /* Queue services                                                     */

software/simulateur/nbproject/private/private.xml

@@ -6,8 +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/simulateur/main.cpp</file>
-        </group>
+        <group/>
     </open-files>
 </project-private>