alartigu/Temps_Reel_Robot
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Robot terminé : Tester les fonctionnalités

Browse source
This commit is contained in:
Leonie Gallois 2021-03-17 12:17:35 +01:00
parent 4ac28d8427
commit 674485fd95
6 changed files with 107 additions and 25 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -41,7 +41,7 @@
</conf> </conf>
<conf name="Debug__RPI_" type="1"> <conf name="Debug__RPI_" type="1">
<toolsSet> <toolsSet>
<developmentServer>pi@10.105.1.7:22</developmentServer> <developmentServer>pi@10.105.1.07:22</developmentServer>
<platform>2</platform> <platform>2</platform>
</toolsSet> </toolsSet>
<dbx_gdbdebugger version="1"> <dbx_gdbdebugger version="1">

0
software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.07-pi-22 Normal file
View file

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

@ -7,9 +7,11 @@
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/> <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"> <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group> <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/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/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/tasks.h</file>
<file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h</file>
</group> </group>
</open-files> </open-files>
</project-private> </project-private>

38
software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.07-pi-22 Normal file
View file

@ -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

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

@ -26,7 +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_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; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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; cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -135,6 +139,10 @@ void Tasks::Init() {
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_watchDog, "th_watchDog", 0, PRIORITY_TWATCHDOG, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush; cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -196,7 +204,10 @@ 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_watchDog, (void(*)(void*)) & Tasks::WatchDog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush; cout << "Tasks launched" << endl << flush;
} }
@ -242,7 +253,10 @@ void Tasks::ServerTask(void *arg) {
monitor.AcceptClient(); // Wait the monitor client monitor.AcceptClient(); // Wait the monitor client
cout << "Rock'n'Roll baby, client accepted!" << endl << flush; cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
rt_sem_broadcast(&sem_serverOk); 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 */ /* The task sendToMon starts here */
/**************************************************************************************/ /**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE); rt_sem_p(&sem_serverOk, TM_INFINITE);
while (1) { while (1) {
@ -295,10 +310,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
delete(msgRcv); delete(msgRcv);
WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET)); 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)); 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; break;
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) { } 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_CONFIRM) ||
msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) ||
msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) || 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); WriteInQueue(&q_messageControlCam, msgRcv);
} }
@ -367,7 +381,8 @@ void Tasks::OpenComRobot(void *arg) {
msgSend = new Message(MESSAGE_ANSWER_ACK); msgSend = new Message(MESSAGE_ANSWER_ACK);
} }
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon 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 ("; cout << "Close serial com (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
status = robot.Close(); status = robot.Close();
@ -383,7 +398,7 @@ 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
} }
delete(msg); //delete(msg);
//rt_sem_p(&sem_openComRobot, TM_INFINITE); //rt_sem_p(&sem_openComRobot, TM_INFINITE);
} }
@ -423,6 +438,7 @@ void Tasks::StartRobotTask(void *arg) {
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(robot.StartWithWD()); msgSend = robot.Write(robot.StartWithWD());
rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
cout << msgSend->GetID(); cout << msgSend->GetID();
cout << ")" << endl; cout << ")" << endl;
@ -432,22 +448,29 @@ 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);
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)){ } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
rt_task_set_periodic(&th_watchDog,TM_NOW,0); rt_task_set_periodic(&th_watchDog,TM_NOW,0);
cout << "Stopping Robot ("; 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); 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_release(&mutex_robot);
rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = MESSAGE_ROBOT_STOP;
rt_mutex_release(&mutex_move);
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
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 0; rt_sem_v(&sem_robotStopped);
rt_mutex_release(&mutex_robotStarted);
} }
@ -461,10 +484,12 @@ void Tasks::WatchDog(void *arg) {
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
rt_task_set_periodic(NULL, TM_NOW, 0); rt_task_set_periodic(NULL, TM_NOW, 0);
while (1) { while (1) {
cout << "Realoading" << endl << flush;
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
Message * msgSend; Message * msgSend;
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.ReloadWD(); msgSend = robot.ReloadWD();
robot.Write(msgSend);
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
} }
} }
@ -476,6 +501,7 @@ void Tasks::MoveTask(void *arg) {
int rs; int rs;
int previousMove = MESSAGE_ROBOT_GO_FORWARD; int previousMove = MESSAGE_ROBOT_GO_FORWARD;
int cpMove; int cpMove;
int nb_error = 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)
@ -486,26 +512,43 @@ void Tasks::MoveTask(void *arg) {
/**************************************************************************************/ /**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 100000000); rt_task_set_periodic(NULL, TM_NOW, 100000000);
Message * msgSend;
while (1) { while (1) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
cout << "Periodic movement update";
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
if (rs == 1) { if (rs == 1) {
cout << "Periodic movement update";
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);
if (cpMove != previousMove) { if (cpMove != previousMove || cpMove != MESSAGE_ROBOT_STOP) {
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)); msgSend = robot.Write(new Message((MessageID)cpMove));
rt_mutex_release(&mutex_robot); 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) { while (1) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
cout << "Periodic battery get lvl \n";
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
if (rs == 1) { if (rs == 1) {
cout << "Periodic battery get lvl" << endl << flush;
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
Message *msg = robot.Write( robot.GetBattery() ) ; Message *msg = robot.Write( robot.GetBattery() ) ;
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
@ -569,7 +612,5 @@ void Tasks::ReadBattery(void *arg){
rt_mutex_release(&mutex_monitor); rt_mutex_release(&mutex_monitor);
} }
} }
cout << endl << flush;
} }
} }

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

@ -101,6 +101,7 @@ private:
RT_SEM sem_startRobot; RT_SEM sem_startRobot;
RT_SEM sem_watchdogStart; RT_SEM sem_watchdogStart;
RT_SEM sem_restartServer; RT_SEM sem_restartServer;
RT_SEM sem_robotStopped;
@ -174,7 +175,7 @@ private:
* Sends periodically a message to the robot * Sends periodically a message to the robot
* @return Nothing * @return Nothing
*/ */
void Watchdog(void *arg); void WatchDog(void *arg);
}; };