suppression et ajouts des task.*

This commit is contained in:
Romain Vitrat 2020-03-29 04:38:24 +02:00
parent 89a2bae5c0
commit 410e50b22d
2 changed files with 302 additions and 31 deletions

View file

@ -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<Position> positionList;
list<Position>::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

View file

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