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

Camera presque finie

Browse source
This commit is contained in:
alejeune 2023-04-03 15:21:01 +02:00
parent 96a3c8fe08
commit a9194dee2d
6 changed files with 160 additions and 41 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.5:22</developmentServer> <developmentServer>xenomai@10.105.1.8: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.8-xenomai-22 Normal file
View file

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

@ -6,6 +6,13 @@
</data> </data>
<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/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
</group>
</open-files> </open-files>
</project-private> </project-private>

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

@ -0,0 +1,39 @@
#Mon Apr 03 15:19:02 CEST 2023
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1679401432930
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1679401432864
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/gdbsudo.sh=c1679401432813
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1679401432832
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1679401457314
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680527409352
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.dep.inc=c1679401432701
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1679401432922
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp=c1679401457320
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1680518630237
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/README.md=c1679401432714
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1679401457337
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/Makefile=c1679401432709
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1679401432820
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/wrapper.c=c1679401457381
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680519967821
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp=c1679404095224
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.properties=c1679401432983
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp=c1679401457304
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1679401432851
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1679401432844
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1679401457346
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1679401432827
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1679401432944
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1679401432840
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.h=c1679401432877
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h=c1679401457331
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.gitignore=c1679401432706
VERSION=1.3
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.h=c1679401432868
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1679401432836
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1679401432848

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

@ -27,6 +27,8 @@
#define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21 #define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 25 #define PRIORITY_TBATTERY 25
#define PRIORITY_TSTARTCAMERA 25
/* /*
* Some remarks: * Some remarks:
@ -74,6 +76,22 @@ 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_cameraStarted, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_robot_pos, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_search_arena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_camera, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush; cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -95,6 +113,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_startCamera, 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;
/**************************************************************************************/ /**************************************************************************************/
@ -120,6 +142,7 @@ 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_move, "th_move", 0, PRIORITY_TMOVE, 0)) { if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -128,6 +151,14 @@ 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_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_startCamera, "th_startCamera", 0, PRIORITY_TSTARTCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush; cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -176,6 +207,14 @@ 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_camera, (void(*)(void*)) & Tasks::CameraTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startCamera, (void(*)(void*)) & Tasks::StartCameraTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush; cout << "Tasks launched" << endl << flush;
} }
@ -240,7 +279,7 @@ void Tasks::SendToMonTask(void* arg) {
while (1) { while (1) {
cout << "wait msg to send" << endl << flush; cout << "wait msg to send" << endl << flush;
msg = ReadInQueue(&q_messageToMon); msg = ReadInQueue(&q_messageToMon);
cout << "Send msg to mon: " << msg->ToString() << endl << flush; //cout << "Send msg to mon: " << msg->ToString() << endl << flush;
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msg); // The message is deleted with the Write monitor.Write(msg); // The message is deleted with the Write
rt_mutex_release(&mutex_monitor); rt_mutex_release(&mutex_monitor);
@ -285,12 +324,31 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_move); rt_mutex_release(&mutex_move);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){ } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
rt_sem_v(&sem_startCamera); rt_sem_v(&sem_startCamera);
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){
rt_sem_v(&sem_startCamera);
} else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){ } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
rt_mutex_acquire(&mutex_answer_arena); rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE);
msg_arena = 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; // A vérifier que le ternaire est utile
cout << "Staut de l'arene" << arenaConfirm << endl << flush;
rt_mutex_release(&mutex_answer_arena); rt_mutex_release(&mutex_answer_arena);
rt_sem_v(&sem_answerSync); rt_sem_v(&sem_answerSync);
}else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1;
rt_mutex_release(&mutex_search_arena);
cout << "Search arena true " << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
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)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 0;
rt_mutex_release(&mutex_robot_pos);
cout << "Robot Pos false" << endl << flush;
} }
delete(msgRcv); // must be deleted manually, no consumer delete(msgRcv); // must be deleted manually, no consumer
} }
} }
@ -370,30 +428,42 @@ void Tasks::StartCameraTask(void *arg) {
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/ /**************************************************************************************/
/* The task Camera Task starts here */ /* The task startRobot starts here */
/**************************************************************************************/ /**************************************************************************************/
while (1) { while (1) {
Message * msgSend; Message * msgSend;
Message * msg;
bool status;
rt_sem_p(&sem_startCamera, TM_INFINITE); rt_sem_p(&sem_startCamera, TM_INFINITE);
cout << "Start Camera "; rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
rt_mutex_acquire(&mutex_camera, TM_INFINITE); rt_mutex_acquire(&mutex_camera, TM_INFINITE);
if(!camera.Open()){ if(!cameraStarted){
msgSend = new Message(MESSAGE_ANSWER_NACK) status = camera.Open();
cout << "Camera Opened" << endl << flush;
}else{
camera.Close();
status = true;
cout << "Camera Closed" << endl << flush;
}
if(!status){
msgSend = new Message(MESSAGE_ANSWER_NACK);
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
cout << msgSend->GetID << endl << flush; cout << msgSend->ToString() << endl << flush;
rt_mutex_release(&mutex_camera); rt_mutex_release(&mutex_camera);
rt_mutex_release(&mutex_cameraStarted);
continue; continue;
} }
msg = new Message(MESSAGE_ANSWER_ACK); msg = new Message(MESSAGE_ANSWER_ACK);
cout << msgSend->GetID << endl << flush; cout << msg->ToString() << endl << flush;
cameraStarted = !cameraStarted;
cout << cameraStarted << endl << flush;
rt_mutex_release(&mutex_camera); rt_mutex_release(&mutex_camera);
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cameraStarted = 1;
rt_mutex_release(&mutex_cameraStarted); rt_mutex_release(&mutex_cameraStarted);
}
cout << "Cam success! " << endl <<flush;
cout << "Fin de start cam" << endl << flush;
} }
} }
@ -415,7 +485,7 @@ void Tasks::MoveTask(void *arg) {
while (1) { while (1) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
cout << "Periodic movement update"; //cout << "Periodic movement update";
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
if (rs == 1) { if (rs == 1) {
@ -430,7 +500,7 @@ void Tasks::MoveTask(void *arg) {
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
} }
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
cout << endl << flush; //cout << endl << flush;
} }
} }
@ -470,7 +540,7 @@ void Tasks::BatteryTask(void *arg) {
/** /**
* @brief task in charge of managing the camera. * @brief task in charge of managing the camera.
*/ */
void CameraTask(void *arg){ void Tasks::CameraTask(void *arg){
int cam; int cam;
std::list<Position> robotPosition; std::list<Position> robotPosition;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
@ -495,7 +565,6 @@ void CameraTask(void *arg){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
if(searchArena){ if(searchArena){
// chopper l'arene // chopper l'arene
cout << "Search arena pls ! " << endl << flush;
if(arena.IsEmpty()){ if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush; cout << "Arena empty" << endl << flush;
// envoi d'un no ack au moniteur // envoi d'un no ack au moniteur
@ -505,45 +574,45 @@ void CameraTask(void *arg){
img.DrawArena(arena); img.DrawArena(arena);
// envoi de l'arene pour vérif // envoi de l'arene pour vérif
cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
} }
} }
rt_mutex_release(&mutex_search_arena);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
if(robotPos){ if(robotPos){
// chopper le robot // chopper le robot
cout << "Pos du robot pls !" ; cout << "Pos du robot pls !" ;
robotPosition = img.SearchRobot(arena); robotPosition = img.SearchRobot(arena);
}
rt_mutex_release(&mutex_robot_pos);
//Traitement du robot //Traitement du robot
if(){ // Robot position != null if(!robotPosition.empty()){ // Robot position != null
img.DrawAllRobots(robotPosition); img.DrawAllRobots(robotPosition);
} }
for (auto position : robotPosition) for (auto position : robotPosition)
{ {
WriteInQueue(&q_messageToMon, position); // Envoi de toutes les positions WriteInQueue(&q_messageToMon, new MessagePosition(MESSAGE_CAM_POSITION,position)); // Envoi de toutes les positions
} }
}
rt_mutex_release(&mutex_robot_pos);
// Envoi de l'image // Envoi de l'image
WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
rt_sem_p(&sem_answerSync, TM_INFINITE); rt_sem_p(&sem_answerSync, TM_INFINITE);
rt_mutex_acquire(&mutex_anwer_arena, TM_INFINITE); rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE);
if(arenaConfirm){ if(arenaConfirm){
// Arena OK // Arena OK
img.DrawArena(arena); // Comment save ou rejeter? img.DrawArena(arena); // Comment save ou rejeter?
//searchArena = 0;
}else{ }else{
// Arena not OK // Arena not OK
searchArena = 0;
// Rien ? // Rien ?
} }
rt_mutex_release(&mutex_answer_arena); rt_mutex_release(&mutex_answer_arena);
rt_mutex_release(&mutex_search_arena);
// Je sais pas si cette partie sert a qqc // Je sais pas si cette partie sert a qqc
} }
} }

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

@ -66,8 +66,10 @@ private:
ComRobot robot; ComRobot robot;
Camera camera; Camera camera;
int robotStarted = 0; int robotStarted = 0;
int robotPos = 0;
int cameraStarted = 0; int cameraStarted = 0;
int arenaConfirm = 0; int arenaConfirm = 0;
int searchArena = 0;
int move = MESSAGE_ROBOT_STOP; int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/ /**********************************************************************/
@ -91,7 +93,9 @@ private:
RT_MUTEX mutex_cameraStarted; RT_MUTEX mutex_cameraStarted;
RT_MUTEX mutex_move; RT_MUTEX mutex_move;
RT_MUTEX mutex_camera; RT_MUTEX mutex_camera;
RT_MUTEX mutex_search_arena;
RT_MUTEX mutex_answer_arena; RT_MUTEX mutex_answer_arena;
RT_MUTEX mutex_robot_pos;
/**********************************************************************/ /**********************************************************************/
/* Semaphores */ /* Semaphores */