Robot terminé : Tester les fonctionnalités
This commit is contained in:
parent
4ac28d8427
commit
674485fd95
6 changed files with 107 additions and 25 deletions
|
|
@ -41,7 +41,7 @@
|
|||
</conf>
|
||||
<conf name="Debug__RPI_" type="1">
|
||||
<toolsSet>
|
||||
<developmentServer>pi@10.105.1.7:22</developmentServer>
|
||||
<developmentServer>pi@10.105.1.07:22</developmentServer>
|
||||
<platform>2</platform>
|
||||
</toolsSet>
|
||||
<dbx_gdbdebugger version="1">
|
||||
|
|
|
|||
|
|
@ -7,9 +7,11 @@
|
|||
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
|
||||
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
|
||||
<group>
|
||||
<file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.h</file>
|
||||
<file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.cpp</file>
|
||||
<file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
|
||||
<file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.h</file>
|
||||
<file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h</file>
|
||||
</group>
|
||||
</open-files>
|
||||
</project-private>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,38 @@
|
|||
#Wed Mar 17 12:14:13 CET 2021
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.h=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/superviseur.doxygen=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.xml=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/.gitignore=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.h=c1615975893000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/.gitignore=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.h=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/.dep.inc=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1615800255000
|
||||
VERSION=1.3
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/Makefile=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.h=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.h=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.cpp=c1615979649000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.h=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/README.md=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/README.md=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/main.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/gdbsudo.sh=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1615800255000
|
||||
/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.properties=c1615800255000
|
||||
|
|
@ -26,7 +26,8 @@
|
|||
#define PRIORITY_TRECEIVEFROMMON 25
|
||||
#define PRIORITY_TSTARTROBOT 20
|
||||
#define PRIORITY_TCAMERA 21
|
||||
#define PRIORITY_TBATTERY 31
|
||||
#define PRIORITY_TBATTERY 19
|
||||
#define PRIORITY_TWATCHDOG 31
|
||||
|
||||
|
||||
|
||||
|
|
@ -101,7 +102,10 @@ void Tasks::Init() {
|
|||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_sem_create(&sem_robotStopped, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
cout << "Semaphores created successfully" << endl << flush;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
|
@ -135,6 +139,10 @@ void Tasks::Init() {
|
|||
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
if (err = rt_task_create(&th_watchDog, "th_watchDog", 0, PRIORITY_TWATCHDOG, 0)) {
|
||||
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
cout << "Tasks created successfully" << endl << flush;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
|
@ -196,7 +204,10 @@ void Tasks::Run() {
|
|||
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_task_start(&th_watchDog, (void(*)(void*)) & Tasks::WatchDog, this)) {
|
||||
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
cout << "Tasks launched" << endl << flush;
|
||||
}
|
||||
|
||||
|
|
@ -242,7 +253,10 @@ void Tasks::ServerTask(void *arg) {
|
|||
monitor.AcceptClient(); // Wait the monitor client
|
||||
cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
|
||||
rt_sem_broadcast(&sem_serverOk);
|
||||
rt_sem_p(&sem_restartServer);
|
||||
rt_sem_p(&sem_restartServer, TM_INFINITE);
|
||||
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
|
||||
monitor.Close();
|
||||
rt_mutex_release(&mutex_monitor);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -260,6 +274,7 @@ void Tasks::SendToMonTask(void* arg) {
|
|||
/**************************************************************************************/
|
||||
/* The task sendToMon starts here */
|
||||
/**************************************************************************************/
|
||||
|
||||
rt_sem_p(&sem_serverOk, TM_INFINITE);
|
||||
|
||||
while (1) {
|
||||
|
|
@ -295,10 +310,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
|
||||
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
|
||||
delete(msgRcv);
|
||||
WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
|
||||
WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
|
||||
WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
|
||||
WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
|
||||
rt_sem_v(&sem_restartServer, TM_INFINITE); // A VERIFIER
|
||||
rt_sem_v(&sem_restartServer); // A VERIFIER
|
||||
break;
|
||||
|
||||
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
|
||||
|
|
@ -324,8 +339,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) ||
|
||||
msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) ||
|
||||
msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) ||
|
||||
msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP) ||
|
||||
)
|
||||
msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP))
|
||||
{
|
||||
WriteInQueue(&q_messageControlCam, msgRcv);
|
||||
}
|
||||
|
|
@ -367,7 +381,8 @@ void Tasks::OpenComRobot(void *arg) {
|
|||
msgSend = new Message(MESSAGE_ANSWER_ACK);
|
||||
}
|
||||
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
||||
}else if(msg->CompareID(MESSAGE_ROBOT_COM_CLOSE)){
|
||||
}else if(msg->CompareID(MESSAGE_ROBOT_COM_CLOSE)){
|
||||
rt_sem_p(&sem_robotStopped, TM_INFINITE);
|
||||
cout << "Close serial com (";
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
status = robot.Close();
|
||||
|
|
@ -383,7 +398,7 @@ void Tasks::OpenComRobot(void *arg) {
|
|||
}
|
||||
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
||||
}
|
||||
delete(msg);
|
||||
//delete(msg);
|
||||
//rt_sem_p(&sem_openComRobot, TM_INFINITE);
|
||||
|
||||
}
|
||||
|
|
@ -423,6 +438,7 @@ void Tasks::StartRobotTask(void *arg) {
|
|||
cout << "Start robot with watchdog (";
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
msgSend = robot.Write(robot.StartWithWD());
|
||||
rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
|
||||
rt_mutex_release(&mutex_robot);
|
||||
cout << msgSend->GetID();
|
||||
cout << ")" << endl;
|
||||
|
|
@ -432,22 +448,29 @@ void Tasks::StartRobotTask(void *arg) {
|
|||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
robotStarted = 1;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
|
||||
} else {
|
||||
rt_task_set_periodic(&th_watchDog,TM_NOW,0);
|
||||
}
|
||||
|
||||
} else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
|
||||
rt_task_set_periodic(&th_watchDog,TM_NOW,0);
|
||||
cout << "Stopping Robot (";
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
robotStarted = 0;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
rt_task_sleep(100000000);
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
msgSend = robot.Write(new Message(MESSAGE_ROBOT_RESET));
|
||||
msgSend = robot.Write(robot.Reset());
|
||||
rt_mutex_release(&mutex_robot);
|
||||
rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
||||
move = MESSAGE_ROBOT_STOP;
|
||||
rt_mutex_release(&mutex_move);
|
||||
cout << msgSend->GetID();
|
||||
cout << ")" << endl;
|
||||
cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
||||
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
robotStarted = 0;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
|
||||
rt_sem_v(&sem_robotStopped);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -461,10 +484,12 @@ void Tasks::WatchDog(void *arg) {
|
|||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
rt_task_set_periodic(NULL, TM_NOW, 0);
|
||||
while (1) {
|
||||
cout << "Realoading" << endl << flush;
|
||||
rt_task_wait_period(NULL);
|
||||
Message * msgSend;
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
msgSend = robot.ReloadWD();
|
||||
robot.Write(msgSend);
|
||||
rt_mutex_release(&mutex_robot);
|
||||
}
|
||||
}
|
||||
|
|
@ -476,6 +501,7 @@ void Tasks::MoveTask(void *arg) {
|
|||
int rs;
|
||||
int previousMove = MESSAGE_ROBOT_GO_FORWARD;
|
||||
int cpMove;
|
||||
int nb_error = 0;
|
||||
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
|
|
@ -486,26 +512,43 @@ void Tasks::MoveTask(void *arg) {
|
|||
/**************************************************************************************/
|
||||
rt_task_set_periodic(NULL, TM_NOW, 100000000);
|
||||
|
||||
Message * msgSend;
|
||||
while (1) {
|
||||
rt_task_wait_period(NULL);
|
||||
cout << "Periodic movement update";
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
rs = robotStarted;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
if (rs == 1) {
|
||||
cout << "Periodic movement update";
|
||||
rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
||||
cpMove = move;
|
||||
rt_mutex_release(&mutex_move);
|
||||
if (cpMove != previousMove) {
|
||||
if (cpMove != previousMove || cpMove != MESSAGE_ROBOT_STOP) {
|
||||
cout << " move: " << cpMove;
|
||||
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
robot.Write(new Message((MessageID)cpMove));
|
||||
msgSend = robot.Write(new Message((MessageID)cpMove));
|
||||
rt_mutex_release(&mutex_robot);
|
||||
previousMove = cpMove;
|
||||
if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
|
||||
nb_error = 0;
|
||||
previousMove = cpMove;
|
||||
} else {
|
||||
nb_error++;
|
||||
}
|
||||
if (nb_error > 2) {
|
||||
cout << "ERREUR ROBOT DETECTED";
|
||||
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR));
|
||||
rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
||||
move = MESSAGE_ROBOT_STOP;
|
||||
rt_mutex_release(&mutex_move);
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
robotStarted = 0;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
rt_task_set_periodic(&th_watchDog,TM_NOW,0);
|
||||
}
|
||||
}
|
||||
cout << endl << flush;
|
||||
}
|
||||
cout << endl << flush;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -555,11 +598,11 @@ void Tasks::ReadBattery(void *arg){
|
|||
|
||||
while (1) {
|
||||
rt_task_wait_period(NULL);
|
||||
cout << "Periodic battery get lvl \n";
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
rs = robotStarted;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
if (rs == 1) {
|
||||
cout << "Periodic battery get lvl" << endl << flush;
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
Message *msg = robot.Write( robot.GetBattery() ) ;
|
||||
rt_mutex_release(&mutex_robot);
|
||||
|
|
@ -569,7 +612,5 @@ void Tasks::ReadBattery(void *arg){
|
|||
rt_mutex_release(&mutex_monitor);
|
||||
}
|
||||
}
|
||||
|
||||
cout << endl << flush;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -101,6 +101,7 @@ private:
|
|||
RT_SEM sem_startRobot;
|
||||
RT_SEM sem_watchdogStart;
|
||||
RT_SEM sem_restartServer;
|
||||
RT_SEM sem_robotStopped;
|
||||
|
||||
|
||||
|
||||
|
|
@ -174,7 +175,7 @@ private:
|
|||
* Sends periodically a message to the robot
|
||||
* @return Nothing
|
||||
*/
|
||||
void Watchdog(void *arg);
|
||||
void WatchDog(void *arg);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue