benistan/real_time
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Final Project and Vision

Browse source
This commit is contained in:
Raphael Benistant 2020-04-10 18:07:47 +02:00
parent 2cd32fe997
commit 0027f2e4f9
2 changed files with 83 additions and 77 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

43
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -157,6 +157,11 @@ void Tasks::Init() {
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_sem_create(&sem_startVision, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl << flush; cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -454,6 +459,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) { } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
cout << "Command Open Camera Received" << endl << flush; cout << "Command Open Camera Received" << endl << flush;
//start task Vision //start task Vision
rt_sem_v(&sem_startVision);
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) { } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
cout << "Command Close Camera Received" << endl << flush; cout << "Command Close Camera Received" << endl << flush;
@ -848,15 +854,7 @@ void Tasks::AskBattery(void *arg){
void Tasks::Vision(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);
int killVisionOk=0;
Camera camera; Camera camera;
int acquireImageOk=0;
int searchArenaOk=0;
int drawArenaOk=0;
int getPositionOk=0;
Arena arena; Arena arena;
//Img* img; //Img* img;
list<Position> positionList; list<Position> positionList;
@ -867,6 +865,20 @@ void Tasks::Vision(void *arg){
MessageImg msgImg; MessageImg msgImg;
Message* msgImgMsg; Message* msgImgMsg;
msgImg.SetID(MESSAGE_CAM_IMAGE); msgImg.SetID(MESSAGE_CAM_IMAGE);
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
while(1){
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
//Wait to get the order to start Vision managed in receiveFromMon
rt_sem_p(&sem_startVision, TM_INFINITE);
int killVisionOk=0;
int acquireImageOk=0;
int searchArenaOk=0;
int drawArenaOk=0;
int getPositionOk=0;
//msgPos->SetID(MESSAGE_CAM_POSITION); //msgPos->SetID(MESSAGE_CAM_POSITION);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE); rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=0; killVision=0;
@ -891,9 +903,6 @@ void Tasks::Vision(void *arg){
} }
if(drawArenaOk==1){ if(drawArenaOk==1){
img.DrawArena(arena); img.DrawArena(arena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
} }
if(getPositionOk==1){ if(getPositionOk==1){
cout << "in GetPos=1" << endl << endl << endl; cout << "in GetPos=1" << endl << endl << endl;
@ -933,14 +942,10 @@ void Tasks::Vision(void *arg){
drawArenaOk=drawArena; drawArenaOk=drawArena;
rt_mutex_release(&mutex_drawArena); rt_mutex_release(&mutex_drawArena);
} }
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
searchArena=0; killVisionOk = killVision;
rt_mutex_release(&mutex_searchArena); rt_mutex_release(&mutex_killVision);
searchArenaOk=0; }
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
getPositionOk=0;
} }

1
software/raspberry/superviseur-robot/tasks.h
View file

@ -121,6 +121,7 @@ private:
RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_detectLostSupRob; RT_SEM sem_detectLostSupRob;
RT_SEM sem_askBattery; RT_SEM sem_askBattery;
RT_SEM sem_startVision;
/**********************************************************************/ /**********************************************************************/
/* Message queues */ /* Message queues */