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
}
}
/**
* @brief Thread starting the communication with the robot.
*/
void Tasks::StartRobotTask(void *arg) {
int cpStart=0;
int cpStart = 0;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
@ -378,89 +377,58 @@ void Tasks::StartRobotTask(void *arg) {
/* The task startRobot starts here */
/**************************************************************************************/
while (1) {
Message * msgSend;
rt_sem_p(&sem_startRobot, TM_INFINITE);
cpStart = start;
if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITHOUT_WD) {
cout << "Start robot without watchdog (";
} else if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITH_WD) {
cout << "Start robot with watchdog (";
}
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(new Message((MessageID) cpStart));
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 ((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
*/
void Tasks::GestionComRobotTask(void *arg) {
int cpt=0;
int cpt = 0;
int state;
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_communicationEtablie, TM_INFINITE);
rt_task_set_periodic(NULL, TM_NOW, 100000000);
cpt=compteur_gestionCom;
while(1){
rt_task_wait_period(NULL);
if (cpt == 3 ){
WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
rt_mutex_acquire(&mutex_robot,TM_INFINITE);
cpt = compteur_gestionCom;
while (1) {
rt_task_wait_period(NULL);
if (cpt == 3) {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
state = robot.Close();
rt_mutex_release(&mutex_robot);
if(state<0){
if (state < 0) {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
}
else{
} else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
}
}
}
}
}
/**
@ -468,25 +436,19 @@ void Tasks::GestionComRobotTask(void *arg) {
*/
void Tasks::ReloadWDTask(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_startReloadWD, TM_INFINITE);
rt_task_set_periodic(NULL,TM_NOW,1000000000);
while(1){
while(1){
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot,TM_INFINITE);
robot.Write(new Message(MESSAGE_ROBOT_RELOAD_WD));
rt_mutex_release(&mutex_robot);
}
}
/**
* @brief Thread handling control of the robot.
*/
@ -494,16 +456,13 @@ void Tasks::MoveTask(void *arg) {
int rs;
int cpMove;
Message * msgRcv;
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 */
/**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 100000000);
while (1) {
rt_task_wait_period(NULL);
cout << "Periodic movement update";
@ -514,27 +473,20 @@ void Tasks::MoveTask(void *arg) {
rt_mutex_acquire(&mutex_move, TM_INFINITE);
cpMove = move;
rt_mutex_release(&mutex_move);
cout << " move: " << cpMove;
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgRcv = robot.Write(new Message((MessageID) cpMove));
rt_mutex_release(&mutex_robot);
if (msgRcv->CompareID(MESSAGE_ANSWER_ACK)){
compteur_gestionCom = 0;
if (msgRcv->CompareID(MESSAGE_ANSWER_ACK)) {
compteur_gestionCom = 0;
} else {
compteur_gestionCom += 1;
}
}
else {
compteur_gestionCom +=1;
}
}
cout << endl << flush;
}
}
}
/**