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);
}
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;
/**************************************************************************************/
@ -454,6 +459,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
cout << "Command Open Camera Received" << endl << flush;
//start task Vision
rt_sem_v(&sem_startVision);
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
cout << "Command Close Camera Received" << endl << flush;
@ -848,15 +854,7 @@ 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);
int killVisionOk=0;
Camera camera;
int acquireImageOk=0;
int searchArenaOk=0;
int drawArenaOk=0;
int getPositionOk=0;
Arena arena;
//Img* img;
list<Position> positionList;
@ -867,6 +865,20 @@ void Tasks::Vision(void *arg){
MessageImg msgImg;
Message* msgImgMsg;
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);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=0;
@ -891,9 +903,6 @@ void Tasks::Vision(void *arg){
}
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;
@ -933,14 +942,10 @@ void Tasks::Vision(void *arg){
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;
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVisionOk = killVision;
rt_mutex_release(&mutex_killVision);
}
}

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

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