diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index e54befb..0a24fc0 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -146,6 +146,10 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_task_create(&th_vision, "th_vision", 0, PRIORITY_TMOVE, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Tasks created successfully" << endl << flush; @@ -199,6 +203,10 @@ void Tasks::Run() { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_task_start(&th_vision, (void(*)(void*)) & Tasks::Vision, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Tasks launched" << endl << flush; } @@ -507,6 +515,9 @@ void Tasks::AskBattery(void *arg){ } +//This task works on a difficult principle that fully make it controllable through +// monitor, accessing booleans variables to set getPosition or searchArena + void Tasks::Vision(void *arg){ cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) @@ -521,7 +532,12 @@ void Tasks::Vision(void *arg){ list positionList; list::iterator it; string strPos; - MessagePosition msgPos; + MessagePosition* msgPos; + Message* msgPosMsg; + MessageImg* msgImg; + Message* msgImgMsg; + msgPos->SetID(MESSAGE_CAM_POSITION); + msgImg->SetID(MESSAGE_CAM_IMAGE); rt_mutex_acquire(&mutex_killVision, TM_INFINITE); killVision=0; rt_mutex_release(&mutex_killVision); @@ -545,17 +561,35 @@ void Tasks::Vision(void *arg){ acquireImageOk=acquireImage; } if(getPositionOk==1){ + //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(); - //it++; - //msgPos(MESSAGE_CAM_POSITION,*it); - //WriteInQueue(&q_messageToMon,msgPos);r - //img.DrawRobot(position.); + //Définition d'un messagePosition qui va contenir l'information (x,y) + msgPos->SetPosition(*it); + //Transformation en message classique + msgPosMsg=msgPos->Copy(); + //Envoi + 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 + msgImgMsg=msgImg->Copy(); + //Envoi + WriteInQueue(&q_messageToMon,msgImg); } - } - + rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); + searchArena=0; + rt_mutex_release(&mutex_searchArena); + searchArenaOk=1; + rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); + getPosition=0; + rt_mutex_release(&mutex_getPosition); + getPositionOk=1; } diff --git a/software/simulateur/main.cpp b/software/simulateur/main.cpp index 71906df..2f9a5a2 100755 --- a/software/simulateur/main.cpp +++ b/software/simulateur/main.cpp @@ -135,7 +135,7 @@ int main(int argc, char const *argv[]) { break; case LABEL_ROBOT_START_WITH_WD: s += LABEL_ROBOT_OK; - cout << ">>> I start without watchdog" << endl; + cout << ">>> I start with watchdog" << endl; break; case LABEL_ROBOT_MOVE: switch (buffer[2]) {