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 name="Debug__RPI_" type="1">
<toolsSet>
<developmentServer>xenomai@10.105.1.3:22</developmentServer>
<developmentServer>xenomai@10.105.1.4:22</developmentServer>
<platform>2</platform>
</toolsSet>
<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;
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;
/**************************************************************************************/
@ -196,6 +200,9 @@ void Tasks::Init() {
if (err = rt_sem_create(&sem_reloadMessages, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
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;
@ -408,11 +415,17 @@ void Tasks::ReceiveFromMonTask(void *arg) {
delete(msgRcv);
exit(-1);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
rt_sem_v(&sem_startRobotWithWatchdog);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@ -422,36 +435,54 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = msgRcv->GetID();
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);
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){ // Fermeture de la caméra
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;
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_acquire(&mutex_image, TM_INFINITE);
getImage = 0;
rt_mutex_release(&mutex_image);
cout << "Search arena true " << endl << flush;
} else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
cout << "Search arena started " << endl << flush;
} else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || // Réponse du moniteur sur la confirmation de l'arène
msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
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
cout << "Statut de l'arene" << arenaConfirm << endl << flush;
arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0;
//cout << "Statut de l'arene " << arenaConfirm << endl << flush;
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);
robotPos = 1;
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);
robotPos = 0;
robotPos = 0; //
rt_mutex_release(&mutex_robot_pos);
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;
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@ -755,15 +786,16 @@ void Tasks::BatteryTask(void *arg) {
void Tasks::CameraTask(void *arg){
int cam;
int grab;
Img* image;
Arena arenaSaved,arena ;
std::list<Position> robotPosition;
Img* image; // Image qui sera envoyée au moniteur
Arena arenaSaved,arena ; // arenaSaved : Arène sauvegardée par le moniteur, arena: Arène temporaire pour le traitement
std::list<Position> robotPosition; // Liste des positions de robot à envoyer
MessageImg* imgToSend;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
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){
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
@ -773,9 +805,10 @@ void Tasks::CameraTask(void *arg){
if(!cam){
continue;
}
rt_mutex_acquire(&mutex_image, TM_INFINITE);
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);
// Lancer le traitement périodique
@ -785,13 +818,15 @@ void Tasks::CameraTask(void *arg){
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
if(searchArena){
rt_mutex_acquire(&mutex_image, TM_INFINITE);
cout << "Arret du traitment !!!!" << endl << flush;
getImage = 0;
rt_mutex_release(&mutex_image);
// chopper l'arene
if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush;
cout << "Arena is empty" << endl << flush;
// envoi d'un no ack au moniteur
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
}else{
cout << "Draw Arena" << endl << flush;
@ -824,24 +859,33 @@ void Tasks::CameraTask(void *arg){
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);
if(searchArena){
rt_sem_p(&sem_answerSync, TM_INFINITE);
if(arenaConfirm){
// Arena OK
cout << "Arena Saved" << endl << flush;
arenaSaved = arena;
}else{
// Arena not OK
cout << "Arena Dumped" << endl << flush;
// Rien ?
//rt_sem_p(&sem_answerSync, TM_INFINITE);
rt_mutex_acquire(&mutex_answerSync, TM_INFINITE);
if(answerSync){
if(arenaConfirm){
// Arena OK
cout << "Arena Saved" << endl << flush;
arenaSaved = arena;
}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);
getImage = 1; // changer en booleen pour synchroniser plutot que ça
rt_mutex_release(&mutex_image);
searchArena = 0;
rt_mutex_release(&mutex_answerSync);
}
rt_mutex_release(&mutex_answer_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 searchArena = 0;
int getImage = 0;
int answerSync = 0;
int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/
@ -105,6 +106,7 @@ private:
RT_MUTEX mutex_robot_pos;
RT_MUTEX mutex_battery;
RT_MUTEX mutex_image;
RT_MUTEX mutex_answerSync;
/**********************************************************************/