diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 39a10ef..e685a6d 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -7,7 +7,18 @@ + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/comrobot.cpp + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/camera.cpp + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/messages.cpp file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/tasks.h + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/main.cpp + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/img.cpp + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/img.h + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/tasks.cpp + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/camera.h + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/commonitor.cpp + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/messages.h + file:/home/romainv/Documents/temps_reel/dumber1/software/raspberry/superviseur-robot/lib/comrobot.h diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index dacb50d..8834e16 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -75,12 +75,11 @@ void Tasks::Init() { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_mutex_create(&mutex_kill_battery, NULL)) { + if (err = rt_mutex_create(&mutex_killBattery, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - cout << "Mutexes created successfully" << endl << flush; /**************************************************************************************/ @@ -367,9 +366,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { /* The task startRobot starts here */ /**************************************************************************************/ //Boolean to get the battery - rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); - killBatteryBool=0; - rt_mutex_release(&mutex_kill_battery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=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); @@ -388,9 +387,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { while (killBattery==0) { rt_task_wait_period(NULL); rt_sem_v(&sem_askBattery); - rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); - killBattery=killBatteryBool; - rt_mutex_release(&mutex_kill_battery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=killBattery; + rt_mutex_release(&mutex_killBattery); } } } @@ -414,9 +413,9 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) { /* The task startRobot starts here */ /**************************************************************************************/ //Boolean to get the battery - rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); - killBatteryBool=0; - rt_mutex_release(&mutex_kill_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); @@ -440,9 +439,9 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) { } rt_task_wait_period(NULL); rt_sem_v(&sem_askBattery); - rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); - killBattery=killBatteryBool; - rt_mutex_release(&mutex_kill_battery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=killBattery; + rt_mutex_release(&mutex_killBattery); } } } @@ -508,6 +507,60 @@ void Tasks::AskBattery(void *arg){ } +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; + int acquireImageOk=1; + int searchArenaOk=0; + int drawArenaOk=0; + int getPositionOk=0; + Arena arena; + list positionList; + list::iterator it; + string strPos; + MessagePosition msgPos; + rt_mutex_acquire(&mutex_killVision, TM_INFINITE); + killVision=0; + rt_mutex_release(&mutex_killVision); + camera.Open(); + while(killVisionOk==0){ + while(acquireImageOk==1){ + 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){ + positionList=img.SearchRobot(arena); + it=positionList.begin(); + //it++; + //msgPos(MESSAGE_CAM_POSITION,*it); + //WriteInQueue(&q_messageToMon,msgPos);r + //img.DrawRobot(position.); + } + } + + } + +} + + + + /** * Write a message in a given queue * @param queue Queue identifier diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index cdcd957..ea20166 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -36,7 +36,7 @@ using namespace std; -class Tasks { +class Tasks{ public: /** * @brief Initializes main structures (semaphores, tasks, mutex, etc.) @@ -64,9 +64,21 @@ private: /**********************************************************************/ ComMonitor monitor; ComRobot robot; - int robotStarted = 0; - int killBatteryBool = 0; - int killRobotWithoutWd = 0; + Camera camera; + Img img; + + bool robotStarted = 0; + + //variables de fermeture batterie : + bool killBattery; + + //variables de fermeture caméra : + bool killVision; + bool acquireImage; + bool searchArena; + bool drawArena; + bool getPosition; + int move = MESSAGE_ROBOT_STOP; /**********************************************************************/ @@ -80,6 +92,7 @@ private: RT_TASK th_startRobotWithWatchdog; RT_TASK th_move; RT_TASK th_askBattery; + RT_TASK th_vision; /**********************************************************************/ /* Mutex */ @@ -88,7 +101,13 @@ private: RT_MUTEX mutex_robot; RT_MUTEX mutex_robotStarted; RT_MUTEX mutex_move; - RT_MUTEX mutex_kill_battery; + 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 */ @@ -99,6 +118,7 @@ private: RT_SEM sem_startRobotWithoutWatchdog; RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_askBattery; + RT_SEM sem_allowStartCaptureImage; /**********************************************************************/ /* Message queues */ @@ -149,6 +169,9 @@ private: void AskBattery(void *arg); + + void Vision(void *arg); + /**********************************************************************/ /* Queue services */ /**********************************************************************/ diff --git a/software/simulateur/nbproject/private/private.xml b/software/simulateur/nbproject/private/private.xml index aef7ea3..304d47a 100755 --- a/software/simulateur/nbproject/private/private.xml +++ b/software/simulateur/nbproject/private/private.xml @@ -6,6 +6,8 @@ - + + file:/home/romainv/Documents/temps_reel/dumber1/software/simulateur/main.cpp +