alejeune/Temps-Reel_Lejeune_Robert_Lacroix
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Caméra Fonctionnelle et code aéré

Browse source
This commit is contained in:
Gwenael Robert 2023-04-13 16:56:04 +02:00
parent 79fdcf3bec
commit d6bd146249
5 changed files with 129 additions and 44 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>xenomai@10.105.1.3:22</developmentServer> <developmentServer>xenomai@10.105.1.4: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.4-xenomai-22 Normal file
View file

39
software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.4-xenomai-22 Normal file
View file

@ -0,0 +1,39 @@
#Thu Apr 13 16:52:14 CEST 2023
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/README.md=c1681393207260
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1681393207255
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1681393207258
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.h=c1681393207285
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/.gitignore=c1681393207250
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.h=c1681393672548
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.h=c1681393226953
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1681393207325
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1681393207318
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1681393207323
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.gitignore=c1681393207123
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1681393207281
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1681393207269
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.properties=c1681393207350
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1681393226957
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1681393207264
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1681393226947
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.h=c1681396618984
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/gdbsudo.sh=c1681393207246
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/camera.cpp=c1681393226941
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/.dep.inc=c1681393207120
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1681393207321
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1681393207272
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/tasks.cpp=c1681397524403
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/img.cpp=c1681393226950
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1681393226960
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/superviseur.doxygen=c1681393207355
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/project.xml=c1681393226999
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/base64/base64.h=c1681393207266
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.cpp=c1681393207300
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/main.cpp=c1681393207306
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/messages.h=c1681393207303
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/Makefile=c1681393207126
VERSION=1.3
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1681393207332
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/wrapper.c=c1681393227008
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/lib/comrobot.h=c1681393207291
/home/g_robert/Temps-Reel_Lejeune_Robert_Lacroix/software/raspberry/superviseur-robot/README.md=c1681393207129

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

@ -160,6 +160,10 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_mutex_create(&mutex_answerSync, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush; cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -196,6 +200,9 @@ void Tasks::Init() {
if (err = rt_sem_create(&sem_reloadMessages, NULL, 0, S_FIFO)) { if (err = rt_sem_create(&sem_reloadMessages, NULL, 0, S_FIFO)) {
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_answerSync, 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;
@ -408,11 +415,17 @@ void Tasks::ReceiveFromMonTask(void *arg) {
delete(msgRcv); delete(msgRcv);
exit(-1); exit(-1);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot); rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_sem_v(&sem_startRobot); rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
rt_sem_v(&sem_startRobotWithWatchdog); rt_sem_v(&sem_startRobotWithWatchdog);
} 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) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@ -422,36 +435,54 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_acquire(&mutex_move, TM_INFINITE); rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = msgRcv->GetID(); move = msgRcv->GetID();
rt_mutex_release(&mutex_move); rt_mutex_release(&mutex_move);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){ // Ouverture de la caméra
rt_sem_v(&sem_startCamera); rt_sem_v(&sem_startCamera);
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){ // Fermeture de la caméra
rt_sem_v(&sem_startCamera); rt_sem_v(&sem_startCamera);
}else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
}else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){ // Demande de recherche de l'arène
cout << "Enabling search arena" << endl << flush; cout << "Enabling search arena" << endl << flush;
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1; searchArena = 1; // Activation du flag de recherche de l'arène
rt_mutex_release(&mutex_search_arena); rt_mutex_release(&mutex_search_arena);
rt_mutex_acquire(&mutex_image, TM_INFINITE);
getImage = 0; cout << "Search arena started " << endl << flush;
rt_mutex_release(&mutex_image);
cout << "Search arena true " << endl << flush; } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || // Réponse du moniteur sur la confirmation de l'arène
} else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){ msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE); rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE);
arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0; // A vérifier que le ternaire est utile arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0;
cout << "Statut de l'arene" << arenaConfirm << endl << flush; //cout << "Statut de l'arene " << arenaConfirm << endl << flush;
rt_mutex_release(&mutex_answer_arena); rt_mutex_release(&mutex_answer_arena);
rt_sem_v(&sem_answerSync);
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){ //rt_sem_v(&sem_answerSync);
rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE);
answerSync = 1;
rt_mutex_release(&mutex_answer_arena);
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){ // Recherche de la position du robot
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 1; robotPos = 1;
rt_mutex_release(&mutex_robot_pos); rt_mutex_release(&mutex_robot_pos);
cout << "Robot Pos true " << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){ cout << "Searching robot position " << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){ // Arret de la recherche du robot
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 0; robotPos = 0; //
rt_mutex_release(&mutex_robot_pos); rt_mutex_release(&mutex_robot_pos);
cout << "Robot Pos false" << endl << flush; cout << "Robot Pos false" << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){
}else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){ // Perte de connexion avec le robot
cout << "MONITOR LOST" << endl << flush; cout << "MONITOR LOST" << endl << flush;
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@ -755,15 +786,16 @@ void Tasks::BatteryTask(void *arg) {
void Tasks::CameraTask(void *arg){ void Tasks::CameraTask(void *arg){
int cam; int cam;
int grab; int grab;
Img* image; Img* image; // Image qui sera envoyée au moniteur
Arena arenaSaved,arena ; Arena arenaSaved,arena ; // arenaSaved : Arène sauvegardée par le moniteur, arena: Arène temporaire pour le traitement
std::list<Position> robotPosition; std::list<Position> robotPosition; // Liste des positions de robot à envoyer
MessageImg* imgToSend; MessageImg* imgToSend;
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_task_set_periodic(NULL, TM_NOW, 100*1000*1000); rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000); // Periode de 100 ms
while(1){ while(1){
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
@ -773,9 +805,10 @@ void Tasks::CameraTask(void *arg){
if(!cam){ if(!cam){
continue; continue;
} }
rt_mutex_acquire(&mutex_image, TM_INFINITE);
if(getImage){ if(getImage){
cout << "Traitement de l'image" << endl << flush ; cout << "Statut du traitement " << getImage << endl;
//cout << "Traitement de l'image" << endl << flush ;
rt_mutex_acquire(&mutex_camera, TM_INFINITE); rt_mutex_acquire(&mutex_camera, TM_INFINITE);
// Lancer le traitement périodique // Lancer le traitement périodique
@ -785,13 +818,15 @@ void Tasks::CameraTask(void *arg){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true
if(searchArena){ if(searchArena){
rt_mutex_acquire(&mutex_image, TM_INFINITE);
cout << "Arret du traitment !!!!" << endl << flush;
getImage = 0; getImage = 0;
rt_mutex_release(&mutex_image);
// chopper l'arene // chopper l'arene
if(arena.IsEmpty()){ if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush; cout << "Arena is empty" << endl << flush;
// envoi d'un no ack au moniteur // envoi d'un no ack au moniteur
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
}else{ }else{
cout << "Draw Arena" << endl << flush; cout << "Draw Arena" << endl << flush;
@ -824,24 +859,33 @@ void Tasks::CameraTask(void *arg){
WriteInQueue(&q_messageToMon, imgToSend); WriteInQueue(&q_messageToMon, imgToSend);
} }
cout << "Statut du traitement " << getImage << endl;
cout << "Statut du search " << searchArena << endl;
rt_mutex_release(&mutex_image);
cout << "Avant Réponse ! " << endl << flush;
rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE); rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE);
if(searchArena){ if(searchArena){
rt_sem_p(&sem_answerSync, TM_INFINITE); //rt_sem_p(&sem_answerSync, TM_INFINITE);
if(arenaConfirm){ rt_mutex_acquire(&mutex_answerSync, TM_INFINITE);
// Arena OK if(answerSync){
cout << "Arena Saved" << endl << flush;
arenaSaved = arena; if(arenaConfirm){
}else{ // Arena OK
// Arena not OK cout << "Arena Saved" << endl << flush;
cout << "Arena Dumped" << endl << flush; arenaSaved = arena;
// Rien ? }else{
// Arena not OK
cout << "Arena Dumped" << endl << flush;
// Rien ?
}
rt_mutex_acquire(&mutex_image, TM_INFINITE);
getImage = 1; // changer en booleen pour synchroniser plutot que ça
rt_mutex_release(&mutex_image);
searchArena = 0;
answerSync = 0;
} }
rt_mutex_acquire(&mutex_image, TM_INFINITE); rt_mutex_release(&mutex_answerSync);
getImage = 1; // changer en booleen pour synchroniser plutot que ça
rt_mutex_release(&mutex_image);
searchArena = 0;
} }
rt_mutex_release(&mutex_answer_arena); rt_mutex_release(&mutex_answer_arena);
rt_mutex_release(&mutex_search_arena); rt_mutex_release(&mutex_search_arena);

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

@ -72,6 +72,7 @@ private:
int arenaConfirm = 0; int arenaConfirm = 0;
int searchArena = 0; int searchArena = 0;
int getImage = 0; int getImage = 0;
int answerSync = 0;
int move = MESSAGE_ROBOT_STOP; int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/ /**********************************************************************/
@ -105,6 +106,7 @@ private:
RT_MUTEX mutex_robot_pos; RT_MUTEX mutex_robot_pos;
RT_MUTEX mutex_battery; RT_MUTEX mutex_battery;
RT_MUTEX mutex_image; RT_MUTEX mutex_image;
RT_MUTEX mutex_answerSync;
/**********************************************************************/ /**********************************************************************/