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

159
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,80 +865,87 @@ void Tasks::Vision(void *arg){
MessageImg msgImg; MessageImg msgImg;
Message* msgImgMsg; Message* msgImgMsg;
msgImg.SetID(MESSAGE_CAM_IMAGE); msgImg.SetID(MESSAGE_CAM_IMAGE);
//msgPos->SetID(MESSAGE_CAM_POSITION);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE); // Synchronization barrier (waiting that all tasks are starting)
killVision=0; rt_sem_p(&sem_barrier, TM_INFINITE);
rt_mutex_release(&mutex_killVision);
rt_task_set_periodic(NULL, TM_NOW, 100000000); while(1){
camera.Open(); cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
while(killVisionOk==0){ //Wait to get the order to start Vision managed in receiveFromMon
rt_task_wait_period(NULL); rt_sem_p(&sem_startVision, TM_INFINITE);
while(acquireImageOk==1){
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;
rt_mutex_release(&mutex_killVision);
rt_task_set_periodic(NULL, TM_NOW, 100000000);
camera.Open();
while(killVisionOk==0){
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
cout << "open cam" << endl; while(acquireImageOk==1){
Img img=camera.Grab(); rt_task_wait_period(NULL);
cout << "after camera grab" << endl << endl << endl; cout << "open cam" << endl;
msgImg.SetImage(&img); Img img=camera.Grab();
cout << "after set image in msgImg" << endl << endl << endl; cout << "after camera grab" << endl << endl << endl;
if(searchArenaOk==1){ msgImg.SetImage(&img);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); cout << "after set image in msgImg" << endl << endl << endl;
acquireImage=0; if(searchArenaOk==1){
rt_mutex_release(&mutex_acquireImage); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
arena=img.SearchArena(); acquireImage=0;
img.DrawArena(arena); rt_mutex_release(&mutex_acquireImage);
} arena=img.SearchArena();
if(drawArenaOk==1){ img.DrawArena(arena);
img.DrawArena(arena); }
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); if(drawArenaOk==1){
acquireImage=1; img.DrawArena(arena);
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; //On démarre la recherche du robot dans la zone définie par l'arène
//On démarre la recherche du robot dans la zone définie par l'arène positionList=img.SearchRobot(arena);
positionList=img.SearchRobot(arena); //Définitition et assignation de l'itérateur de parcrous de la liste positionList
//Définitition et assignation de l'itérateur de parcrous de la liste positionList it=positionList.begin();
it=positionList.begin(); //Définition d'un messagePosition qui va contenir l'information (x,y)
//Définition d'un messagePosition qui va contenir l'information (x,y) //msgPos->SetPosition(*it);
//msgPos->SetPosition(*it); MessagePosition msgPos(MESSAGE_CAM_POSITION,*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 //Transformation en message classique
msgPosMsg=msgPos.Copy(); msgImgMsg=msgImg.Copy();
//Envoi //Envoi
cout << "envoi au mon" << endl; WriteInQueue(&q_messageToMon,&msgImg);
WriteInQueue(&q_messageToMon,&msgPos);
//Dessis du robot sur l'image
img.DrawRobot(*it);
} }
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
//Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas) acquireImageOk=acquireImage;
msgImg.SetImage(&img); rt_mutex_release(&mutex_acquireImage);
//Transformation en message classique rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
msgImgMsg=msgImg.Copy(); getPositionOk=getPosition;
//Envoi rt_mutex_release(&mutex_getPosition);
WriteInQueue(&q_messageToMon,&msgImg); 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_acquireImage, TM_INFINITE); rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
acquireImageOk=acquireImage; killVisionOk = killVision;
rt_mutex_release(&mutex_acquireImage); rt_mutex_release(&mutex_killVision);
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;
} }

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 */