conceica/temps_reel
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Version_Offcielle

Browse source
This commit is contained in:
Joao Conceicao Nunes 2020-04-08 15:30:53 +02:00
parent 76cdc1f5f4
commit fe6d948857
1 changed files with 23 additions and 71 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -364,12 +364,11 @@ void Tasks::OpenComRobot(void *arg) {
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
} }
} }
/** /**
* @brief Thread starting the communication with the robot. * @brief Thread starting the communication with the robot.
*/ */
void Tasks::StartRobotTask(void *arg) { void Tasks::StartRobotTask(void *arg) {
int cpStart=0; int cpStart = 0;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
@ -378,89 +377,58 @@ void Tasks::StartRobotTask(void *arg) {
/* The task startRobot starts here */ /* The task startRobot starts here */
/**************************************************************************************/ /**************************************************************************************/
while (1) { while (1) {
Message * msgSend; Message * msgSend;
rt_sem_p(&sem_startRobot, TM_INFINITE); rt_sem_p(&sem_startRobot, TM_INFINITE);
cpStart = start; cpStart = start;
if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITHOUT_WD) { if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITHOUT_WD) {
cout << "Start robot without watchdog ("; cout << "Start robot without watchdog (";
} else if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITH_WD) { } else if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITH_WD) {
cout << "Start robot with watchdog ("; cout << "Start robot with watchdog (";
} }
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(new Message((MessageID) cpStart)); msgSend = robot.Write(new Message((MessageID) cpStart));
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
cout << msgSend->GetID(); cout << msgSend->GetID();
cout << ")" << endl; cout << ")" << endl;
cout << "Movement answer: " << msgSend->ToString() << endl << flush; cout << "Movement answer: " << msgSend->ToString() << endl << flush;
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1; robotStarted = 1;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITH_WD){rt_sem_v(&sem_startReloadWD);} if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITH_WD) {
rt_sem_v(&sem_startReloadWD);
}
} }
} }
} }
/** /**
* @brief Thread managing the Robot's coms * @brief Thread managing the Robot's coms
*/ */
void Tasks::GestionComRobotTask(void *arg) { void Tasks::GestionComRobotTask(void *arg) {
int cpt = 0;
int cpt=0;
int state; int state;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_communicationEtablie, TM_INFINITE); rt_sem_p(&sem_communicationEtablie, TM_INFINITE);
rt_task_set_periodic(NULL, TM_NOW, 100000000); rt_task_set_periodic(NULL, TM_NOW, 100000000);
cpt = compteur_gestionCom;
while (1) {
cpt=compteur_gestionCom; rt_task_wait_period(NULL);
if (cpt == 3) {
while(1){ WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
rt_task_wait_period(NULL); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
if (cpt == 3 ){
WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
rt_mutex_acquire(&mutex_robot,TM_INFINITE);
state = robot.Close(); state = robot.Close();
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
if (state < 0) {
if(state<0){
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
} } else {
else{
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
} }
} }
}
}
} }
/** /**
@ -468,25 +436,19 @@ void Tasks::GestionComRobotTask(void *arg) {
*/ */
void Tasks::ReloadWDTask(void *arg) { void Tasks::ReloadWDTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_startReloadWD, TM_INFINITE); rt_sem_p(&sem_startReloadWD, TM_INFINITE);
rt_task_set_periodic(NULL,TM_NOW,1000000000); rt_task_set_periodic(NULL,TM_NOW,1000000000);
while(1){
while(1){
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot,TM_INFINITE); rt_mutex_acquire(&mutex_robot,TM_INFINITE);
robot.Write(new Message(MESSAGE_ROBOT_RELOAD_WD)); robot.Write(new Message(MESSAGE_ROBOT_RELOAD_WD));
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
} }
} }
/** /**
* @brief Thread handling control of the robot. * @brief Thread handling control of the robot.
*/ */
@ -494,16 +456,13 @@ void Tasks::MoveTask(void *arg) {
int rs; int rs;
int cpMove; int cpMove;
Message * msgRcv; Message * msgRcv;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/ /**************************************************************************************/
/* The task starts here */ /* The task starts here */
/**************************************************************************************/ /**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 100000000); rt_task_set_periodic(NULL, TM_NOW, 100000000);
while (1) { while (1) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
cout << "Periodic movement update"; cout << "Periodic movement update";
@ -514,27 +473,20 @@ void Tasks::MoveTask(void *arg) {
rt_mutex_acquire(&mutex_move, TM_INFINITE); rt_mutex_acquire(&mutex_move, TM_INFINITE);
cpMove = move; cpMove = move;
rt_mutex_release(&mutex_move); rt_mutex_release(&mutex_move);
cout << " move: " << cpMove; cout << " move: " << cpMove;
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgRcv = robot.Write(new Message((MessageID) cpMove)); msgRcv = robot.Write(new Message((MessageID) cpMove));
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
if (msgRcv->CompareID(MESSAGE_ANSWER_ACK)) {
if (msgRcv->CompareID(MESSAGE_ANSWER_ACK)){ compteur_gestionCom = 0;
compteur_gestionCom = 0; } else {
compteur_gestionCom += 1;
}
} }
else {
compteur_gestionCom +=1;
}
}
cout << endl << flush; cout << endl << flush;
}
} }
}
/** /**