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

vision funciton not fully working but almost

Browse source
This commit is contained in:
Romain Vitrat 2020-03-29 11:19:22 +02:00
parent 481213ac20
commit be868ed6f4
2 changed files with 44 additions and 20 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -154,7 +154,10 @@ void Tasks::Init() {
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);
@ -668,76 +671,96 @@ 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 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;
//MessagePosition* msgPos;
Message* msgPosMsg;
MessageImg* msgImg;
MessageImg msgImg;
Message* msgImgMsg;
msgPos->SetID(MESSAGE_CAM_POSITION);
msgImg->SetID(MESSAGE_CAM_IMAGE);
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);
acquireImageOk=acquireImage;
arena=img.SearchArena();
img.DrawArena(arena);
}
if(drawArenaOk==1){
// img.DrawArena(arena);
img.DrawArena(arena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
acquireImageOk=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);
//msgPos->SetPosition(*it);
MessagePosition msgPos(MESSAGE_CAM_POSITION,*it);
//Transformation en message classique
msgPosMsg=msgPos->Copy();
msgPosMsg=msgPos.Copy();
//Envoi
WriteInQueue(&q_messageToMon,msgPos);
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);
msgImg.SetImage(&img);
//Transformation en message classique
msgImgMsg=msgImg->Copy();
msgImgMsg=msgImg.Copy();
//Envoi
WriteInQueue(&q_messageToMon,msgImg);
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=1;
searchArenaOk=0;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
getPositionOk=1;
getPositionOk=0;
}

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

@ -117,6 +117,7 @@ private:
RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_allowStartReceive;
RT_SEM sem_askBattery;
RT_SEM sem_allowStartCapture;
/**********************************************************************/
/* Message queues */