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

Version_Finale

Browse source
This commit is contained in:
Joao Conceicao Nunes 2020-04-08 15:10:12 +02:00
parent 0552b59b79
commit 76cdc1f5f4
3 changed files with 62 additions and 57 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
software/raspberry/superviseur-robot/nbproject/private/private.xml
View file

@ -9,7 +9,7 @@
<url>tasks.cpp</url> <url>tasks.cpp</url>
<bookmark id="1"> <bookmark id="1">
<name/> <name/>
<line>470</line> <line>471</line>
<key/> <key/>
</bookmark> </bookmark>
</file> </file>

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

@ -26,8 +26,8 @@
#define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21 #define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTROBOT 10 #define PRIORITY_TBATTROBOT 15
#define PRIORITY_TGESTIONCOMROBOT 15 #define PRIORITY_TGESTIONCOMROBOT 19
#define PRIORITY_TRELOADWD 15 #define PRIORITY_TRELOADWD 15
/* /*
@ -97,6 +97,15 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_sem_create(&sem_communicationEtablie, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_startReloadWD, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl << flush; cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -133,11 +142,11 @@ void Tasks::Init() {
if (err = rt_task_create(&th_gestionComRobot, "th_gestionComRobot", 0, PRIORITY_TGESTIONCOMROBOT, 0)) { if (err = rt_task_create(&th_gestionComRobot, "th_gestionComRobot", 0, PRIORITY_TGESTIONCOMROBOT, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
}/* }
if (err = rt_task_create(&th_reloadWD, "th_reloadWD", 0, PRIORITY_TRELOADWD, 0)) { if (err = rt_task_create(&th_reloadWD, "th_reloadWD", 0, PRIORITY_TRELOADWD, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
}*/ }
cout << "Tasks created successfully" << endl << flush; cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -190,11 +199,11 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
/*
if (err = rt_task_start(&th_reloadWD, (void(*)(void*)) & Tasks::ReloadWDTask, this)) { if (err = rt_task_start(&th_reloadWD, (void(*)(void*)) & Tasks::ReloadWDTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
}*/ }
cout << "Tasks launched" << endl << flush; cout << "Tasks launched" << endl << flush;
} }
@ -303,7 +312,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
start = msgRcv -> GetID(); start = msgRcv -> GetID();
rt_sem_v(&sem_startRobot); rt_sem_v(&sem_startRobot);
//rt_sem_v(&sem_startReload);
} }
else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
@ -336,11 +345,11 @@ void Tasks::OpenComRobot(void *arg) {
while (1) { while (1) {
rt_sem_p(&sem_openComRobot, TM_INFINITE); rt_sem_p(&sem_openComRobot, TM_INFINITE);
cout << "Open serial com ("; cout << "Open serial com (";
cout<< "Je suis la avant \n";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
status = robot.Open(); status = robot.Open();
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
cout << "Je suis la apres \n";
cout << status; cout << status;
cout << ")" << endl << flush; cout << ")" << endl << flush;
@ -350,7 +359,7 @@ void Tasks::OpenComRobot(void *arg) {
} else { } else {
msgSend = new Message(MESSAGE_ANSWER_ACK); msgSend = new Message(MESSAGE_ANSWER_ACK);
//rt_sem_v(&sem_communicationEtablie); rt_sem_v(&sem_communicationEtablie);
} }
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
} }
@ -399,7 +408,7 @@ void Tasks::StartRobotTask(void *arg) {
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_reloadWD);} if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITH_WD){rt_sem_v(&sem_startReloadWD);}
@ -408,40 +417,33 @@ void Tasks::StartRobotTask(void *arg) {
} }
/** /**
* @brief Thread analysing Robot's battery * @brief Thread managing the Robot's coms
*/ */
void Tasks::GestionComRobotTask(void *arg) { void Tasks::GestionComRobotTask(void *arg) {
Message * msgSend;
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,1000000000); rt_task_set_periodic(NULL, TM_NOW, 100000000);
cpt=compteur_gestionCom;
while(1){ while(1){
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE); if (cpt == 3 ){
msgSend = robot.Write(new Message (MESSAGE_ROBOT_PING));
rt_mutex_release(&mutex_robot);
/*
if (msgSend->CompareID(MESSAGE_ANSWER_NACK)){
cpt+=1;
if (cpt == 3){
WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
rt_mutex_acquire(&mutex_robot,TM_INFINITE); 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));
} }
@ -450,22 +452,11 @@ void Tasks::GestionComRobotTask(void *arg) {
} }
}
}
else {
cpt = 0;
}
*/
} }
}
@ -477,19 +468,19 @@ void Tasks::GestionComRobotTask(void *arg) {
*/ */
void Tasks::ReloadWDTask(void *arg) { void Tasks::ReloadWDTask(void *arg) {
Message * msgSend;
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_reloadWD, 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);
msgSend= 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);
} }
@ -502,6 +493,7 @@ void Tasks::ReloadWDTask(void *arg) {
void Tasks::MoveTask(void *arg) { void Tasks::MoveTask(void *arg) {
int rs; int rs;
int cpMove; int cpMove;
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)
@ -526,13 +518,23 @@ void Tasks::MoveTask(void *arg) {
cout << " move: " << cpMove; cout << " move: " << cpMove;
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
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)){
compteur_gestionCom = 0;
} }
else {
compteur_gestionCom +=1;
}
}
cout << endl << flush; cout << endl << flush;
} }
}
}
/** /**

5
software/raspberry/superviseur-robot/tasks.h
View file

@ -64,6 +64,8 @@ private:
/**********************************************************************/ /**********************************************************************/
ComMonitor monitor; ComMonitor monitor;
ComRobot robot; ComRobot robot;
int compteur_gestionCom = 0; //Ajouté
int robotStarted = 0; int robotStarted = 0;
int move = MESSAGE_ROBOT_STOP; int move = MESSAGE_ROBOT_STOP;
int start = MESSAGE_ROBOT_START_WITHOUT_WD; //Ajouté int start = MESSAGE_ROBOT_START_WITHOUT_WD; //Ajouté
@ -98,7 +100,8 @@ private:
RT_SEM sem_serverOk; RT_SEM sem_serverOk;
RT_SEM sem_startRobot; RT_SEM sem_startRobot;
RT_SEM sem_communicationEtablie; RT_SEM sem_communicationEtablie;
RT_SEM sem_reloadWD; RT_SEM sem_startReloadWD;
/**********************************************************************/ /**********************************************************************/
/* Message queues */ /* Message queues */