From 410e50b22df07c139909a1cfe49a3e897748435c Mon Sep 17 00:00:00 2001 From: vitrat Date: Sun, 29 Mar 2020 04:38:24 +0200 Subject: [PATCH 1/2] suppression et ajouts des task.* --- .../raspberry/superviseur-robot/tasks.cpp | 292 ++++++++++++++++-- software/raspberry/superviseur-robot/tasks.h | 41 ++- 2 files changed, 302 insertions(+), 31 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index f17ae21..a9b2347 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -24,8 +24,10 @@ #define PRIORITY_TMOVE 20 #define PRIORITY_TSENDTOMON 22 #define PRIORITY_TRECEIVEFROMMON 25 -#define PRIORITY_TSTARTROBOT 20 -#define PRIORITY_TCAMERA 21 +#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 24 +#define PRIORITY_TSTARTROBOTWITHWATCHDOG 23 +#define PRIORITY_TCAMERA 21 +#define PRIORITY_TVISION 26 /* * Some remarks: @@ -73,6 +75,30 @@ void Tasks::Init() { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_mutex_create(&mutex_killBattery, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_mutex_create(&mutex_killVision, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_mutex_create(&mutex_acquireImage, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_mutex_create(&mutex_searchArena, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_mutex_create(&mutex_drawArena, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_mutex_create(&mutex_getPosition, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Mutexes created successfully" << endl << flush; /**************************************************************************************/ @@ -90,7 +116,19 @@ void Tasks::Init() { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_sem_create(&sem_startRobot, NULL, 0, S_FIFO)) { + if (err = rt_sem_create(&sem_startRobotWithoutWatchdog, NULL, 0, S_FIFO)) { + cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) { + cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_sem_create(&sem_askBattery, NULL, 0, S_FIFO)) { + cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_sem_create(&sem_allowStartCaptureImage, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } @@ -115,7 +153,11 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0)) { + if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } @@ -123,6 +165,14 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_task_create(&th_askBattery, "th_askBattery", 0, PRIORITY_TMOVE, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_create(&th_vision, "th_vision", 0, PRIORITY_TVISION, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Tasks created successfully" << endl << flush; /**************************************************************************************/ @@ -159,7 +209,11 @@ void Tasks::Run() { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) { + if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } @@ -167,7 +221,14 @@ void Tasks::Run() { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - + if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) { + 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; } @@ -265,7 +326,9 @@ void Tasks::ReceiveFromMonTask(void *arg) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { rt_sem_v(&sem_openComRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { - rt_sem_v(&sem_startRobot); + rt_sem_v(&sem_startRobotWithoutWatchdog); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { + rt_sem_v(&sem_startRobotWithWatchdog); } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || @@ -280,6 +343,9 @@ void Tasks::ReceiveFromMonTask(void *arg) { } } + + + /** * @brief Thread opening communication with the robot. */ @@ -313,39 +379,104 @@ void Tasks::OpenComRobot(void *arg) { } } + + + + + /** * @brief Thread starting the communication with the robot. */ -void Tasks::StartRobotTask(void *arg) { +void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - + int killBatteryOk=0; + Message * msgSend; /**************************************************************************************/ /* The task startRobot starts here */ /**************************************************************************************/ - while (1) { + //Boolean to get the battery + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=0; + rt_mutex_release(&mutex_killBattery); + rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE); + cout << "Start robot without watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(robot.StartWithoutWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + cout << "Movement answer: " << msgSend->ToString() << endl << flush; + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon - Message * msgSend; - rt_sem_p(&sem_startRobot, TM_INFINITE); - cout << "Start robot without watchdog ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(robot.StartWithoutWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; - - cout << "Movement answer: " << msgSend->ToString() << endl << flush; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon - - if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 1; - rt_mutex_release(&mutex_robotStarted); + if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + rt_mutex_release(&mutex_robotStarted); + rt_task_set_periodic(NULL, TM_NOW, 500000000); + while (killBatteryOk==0) { + rt_task_wait_period(NULL); + rt_sem_v(&sem_askBattery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=killBattery; + rt_mutex_release(&mutex_killBattery); } } } +/** + * @brief Thread starting the communication with the robot. + *//** + * @brief Thread starting the communication with the robot. + */ + + +void Tasks::StartRobotTaskWithWatchdog(void *arg) { + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + int killBatteryOk=0; + int cpt=1; + Message * msgSend; + /**************************************************************************************/ + /* The task startRobot starts here */ + /**************************************************************************************/ + //Boolean to get the battery + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBattery=0; + rt_mutex_release(&mutex_killBattery); + rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); + cout << "Start robot with watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(robot.StartWithWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + cout << "Movement answer: " << msgSend->ToString() << endl << flush; + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + + if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + rt_mutex_release(&mutex_robotStarted); + rt_task_set_periodic(NULL, TM_NOW, 500000000); + while (killBatteryOk==0) { + cpt++; + if(cpt==2){ + robot.Write(robot.ReloadWD()); + cpt=0; + } + rt_task_wait_period(NULL); + rt_sem_v(&sem_askBattery); + rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); + killBatteryOk=killBattery; + rt_mutex_release(&mutex_killBattery); + } + } +} + + /** * @brief Thread handling control of the robot. */ @@ -360,7 +491,7 @@ void Tasks::MoveTask(void *arg) { /**************************************************************************************/ /* The task starts here */ /**************************************************************************************/ - rt_task_set_periodic(NULL, TM_NOW, 100000000); + rt_task_set_periodic(NULL, TM_NOW, 1000000000); while (1) { rt_task_wait_period(NULL); @@ -383,6 +514,113 @@ void Tasks::MoveTask(void *arg) { } } + + +void Tasks::AskBattery(void *arg){ + Message* p_mess_answer_battery; + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + + /**************************************************************************************/ + /* The task starts here */ + /**************************************************************************************/ + while(1){ + rt_sem_p(&sem_askBattery, TM_INFINITE); + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + p_mess_answer_battery = robot.Write(robot.GetBattery()); + rt_mutex_release(&mutex_robot); + WriteInQueue(&q_messageToMon, p_mess_answer_battery); + cout << endl << flush; + } +} + + +//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) + rt_sem_p(&sem_barrier, TM_INFINITE); + + rt_sem_p(&sem_askBattery, TM_INFINITE); + int killVisionOk=0; + Camera camera; + int acquireImageOk=1; + int searchArenaOk=0; + int drawArenaOk=0; + int getPositionOk=0; + Arena arena; + list positionList; + list::iterator it; + string strPos; + 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); + camera.Open(); + while(killVisionOk==0){ + while(acquireImageOk==1){ + Img img=camera.Grab(); + 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); + rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); + acquireImage=1; + rt_mutex_release(&mutex_acquireImage); + 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(); + //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; +} + + + + + /** * Write a message in a given queue * @param queue Queue identifier diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 27fe329..32de3c9 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -66,6 +66,15 @@ private: ComRobot robot; int robotStarted = 0; int move = MESSAGE_ROBOT_STOP; + bool killBattery = 0; + bool killReceiveFromMon=0; + bool killVision=0; + bool searchArena=0; + bool killSendMon=0; + bool killDetectlostSupRob=0; + bool acquireImage=0; + bool getPosition=0; + bool drawArena=0; /**********************************************************************/ /* Tasks */ @@ -74,8 +83,11 @@ private: RT_TASK th_sendToMon; RT_TASK th_receiveFromMon; RT_TASK th_openComRobot; - RT_TASK th_startRobot; + RT_TASK th_startRobotWithoutWatchdog; + RT_TASK th_startRobotWithWatchdog; RT_TASK th_move; + RT_TASK th_vision; + RT_TASK th_askBattery; /**********************************************************************/ /* Mutex */ @@ -84,6 +96,12 @@ private: RT_MUTEX mutex_robot; RT_MUTEX mutex_robotStarted; RT_MUTEX mutex_move; + RT_MUTEX mutex_killBattery; + RT_MUTEX mutex_killVision; + RT_MUTEX mutex_acquireImage; + RT_MUTEX mutex_searchArena; + RT_MUTEX mutex_drawArena; + RT_MUTEX mutex_getPosition; /**********************************************************************/ /* Semaphores */ @@ -91,8 +109,11 @@ private: RT_SEM sem_barrier; RT_SEM sem_openComRobot; RT_SEM sem_serverOk; - RT_SEM sem_startRobot; - + RT_SEM sem_startRobotWithoutWatchdog; + RT_SEM sem_startRobotWithWatchdog; + RT_SEM sem_askBattery; + RT_SEM sem_allowStartCaptureImage; + /**********************************************************************/ /* Message queues */ /**********************************************************************/ @@ -125,13 +146,25 @@ private: /** * @brief Thread starting the communication with the robot. */ - void StartRobotTask(void *arg); + void StartRobotTaskWithoutWatchdog(void *arg); + + /** + * @brief Thread starting the communication with the robot with watchdog. + */ + void StartRobotTaskWithWatchdog(void *arg); + /** * @brief Thread handling control of the robot. */ void MoveTask(void *arg); + + void AskBattery(void *arg); + + + void Vision(void *arg); + /**********************************************************************/ /* Queue services */ /**********************************************************************/ From 0b6a901df72910a5b5428eef9369590e8c7f5b44 Mon Sep 17 00:00:00 2001 From: vitrat Date: Sun, 29 Mar 2020 04:39:56 +0200 Subject: [PATCH 2/2] private --- .../superviseur-robot/nbproject/private/private.xml | 8 +------- software/simulateur/nbproject/private/private.xml | 4 +--- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 1242b72..aef7ea3 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -6,12 +6,6 @@ - - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/messages.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/tasks.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp - file:/home/etud/dumber/software/raspberry/superviseur-robot/lib/commonitor.h - + diff --git a/software/simulateur/nbproject/private/private.xml b/software/simulateur/nbproject/private/private.xml index 2683170..aef7ea3 100644 --- a/software/simulateur/nbproject/private/private.xml +++ b/software/simulateur/nbproject/private/private.xml @@ -6,8 +6,6 @@ - - file:/home/etud/dumber/software/simulateur/main.cpp - +