diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml
index 7a24fd0..87ac888 100644
--- a/software/raspberry/superviseur-robot/nbproject/private/private.xml
+++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml
@@ -9,7 +9,7 @@
tasks.cpp
- 470
+ 471
diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index a6e5e43..2da45f3 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -26,8 +26,8 @@
#define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21
-#define PRIORITY_TBATTROBOT 10
-#define PRIORITY_TGESTIONCOMROBOT 15
+#define PRIORITY_TBATTROBOT 15
+#define PRIORITY_TGESTIONCOMROBOT 19
#define PRIORITY_TRELOADWD 15
/*
@@ -97,6 +97,15 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
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;
/**************************************************************************************/
@@ -133,11 +142,11 @@ void Tasks::Init() {
if (err = rt_task_create(&th_gestionComRobot, "th_gestionComRobot", 0, PRIORITY_TGESTIONCOMROBOT, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
- }/*
+ }
if (err = rt_task_create(&th_reloadWD, "th_reloadWD", 0, PRIORITY_TRELOADWD, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
- }*/
+ }
cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/
@@ -190,11 +199,11 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
- /*
+
if (err = rt_task_start(&th_reloadWD, (void(*)(void*)) & Tasks::ReloadWDTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
- }*/
+ }
cout << "Tasks launched" << endl << flush;
}
@@ -303,7 +312,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
start = msgRcv -> GetID();
rt_sem_v(&sem_startRobot);
- //rt_sem_v(&sem_startReload);
+
}
else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
@@ -336,11 +345,11 @@ void Tasks::OpenComRobot(void *arg) {
while (1) {
rt_sem_p(&sem_openComRobot, TM_INFINITE);
cout << "Open serial com (";
- cout<< "Je suis la avant \n";
+
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
status = robot.Open();
rt_mutex_release(&mutex_robot);
- cout << "Je suis la apres \n";
+
cout << status;
cout << ")" << endl << flush;
@@ -350,7 +359,7 @@ void Tasks::OpenComRobot(void *arg) {
} else {
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
}
@@ -399,7 +408,7 @@ void Tasks::StartRobotTask(void *arg) {
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_reloadWD);}
+ if ((MessageID) cpStart == MESSAGE_ROBOT_START_WITH_WD){rt_sem_v(&sem_startReloadWD);}
@@ -408,39 +417,32 @@ void Tasks::StartRobotTask(void *arg) {
}
/**
- * @brief Thread analysing Robot's battery
+ * @brief Thread managing the Robot's coms
*/
void Tasks::GestionComRobotTask(void *arg) {
- Message * msgSend;
+
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_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){
- rt_task_wait_period(NULL);
-
- rt_mutex_acquire(&mutex_robot, TM_INFINITE);
- 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));
-
-
+ 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){
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
@@ -450,49 +452,38 @@ void Tasks::GestionComRobotTask(void *arg) {
}
-
}
+
+
}
- else {
- cpt = 0;
- }
+
+
+
-
- */
-
-
- }
-
-
-
-
-
-
-
}
-
+
/**
* @brief Thread Relaod WD
*/
void Tasks::ReloadWDTask(void *arg) {
- Message * msgSend;
+
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_reloadWD, 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);
- msgSend= robot.Write(new Message(MESSAGE_ROBOT_RELOAD_WD));
+ robot.Write(new Message(MESSAGE_ROBOT_RELOAD_WD));
rt_mutex_release(&mutex_robot);
-
- }
+
+ }
}
@@ -502,6 +493,7 @@ void Tasks::ReloadWDTask(void *arg) {
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)
@@ -526,13 +518,23 @@ void Tasks::MoveTask(void *arg) {
cout << " move: " << cpMove;
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);
+
+ if (msgRcv->CompareID(MESSAGE_ANSWER_ACK)){
+ compteur_gestionCom = 0;
}
+ else {
+ compteur_gestionCom +=1;
+
+ }
+ }
+
+
cout << endl << flush;
+ }
+
}
-}
-
/**
diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h
index bbbe0e1..75bbac3 100644
--- a/software/raspberry/superviseur-robot/tasks.h
+++ b/software/raspberry/superviseur-robot/tasks.h
@@ -64,6 +64,8 @@ private:
/**********************************************************************/
ComMonitor monitor;
ComRobot robot;
+ int compteur_gestionCom = 0; //Ajouté
+
int robotStarted = 0;
int move = MESSAGE_ROBOT_STOP;
int start = MESSAGE_ROBOT_START_WITHOUT_WD; //Ajouté
@@ -98,7 +100,8 @@ private:
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
RT_SEM sem_communicationEtablie;
- RT_SEM sem_reloadWD;
+ RT_SEM sem_startReloadWD;
+
/**********************************************************************/
/* Message queues */