Camera presque finie
This commit is contained in:
alejeune
parent
96a3c8fe08
commit
a9194dee2d
6 changed files with 160 additions and 41 deletions
|
|
@ -41,7 +41,7 @@
|
|||
</conf>
|
||||
<conf name="Debug__RPI_" type="1">
|
||||
<toolsSet>
|
||||
<developmentServer>xenomai@10.105.1.5:22</developmentServer>
|
||||
<developmentServer>xenomai@10.105.1.8:22</developmentServer>
|
||||
<platform>2</platform>
|
||||
</toolsSet>
|
||||
<dbx_gdbdebugger version="1">
|
||||
|
|
|
|||
|
|
@ -6,6 +6,13 @@
|
|||
</data>
|
||||
<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/>
|
||||
<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>
|
||||
</project-private>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -27,6 +27,8 @@
|
|||
#define PRIORITY_TSTARTROBOT 20
|
||||
#define PRIORITY_TCAMERA 21
|
||||
#define PRIORITY_TBATTERY 25
|
||||
#define PRIORITY_TSTARTCAMERA 25
|
||||
|
||||
|
||||
/*
|
||||
* Some remarks:
|
||||
|
|
@ -74,6 +76,22 @@ void Tasks::Init() {
|
|||
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
|
||||
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;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
|
@ -95,6 +113,10 @@ void Tasks::Init() {
|
|||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
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;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
|
@ -120,6 +142,7 @@ void Tasks::Init() {
|
|||
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
|
||||
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
|
|
@ -128,6 +151,14 @@ void Tasks::Init() {
|
|||
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
||||
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;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
|
@ -176,6 +207,14 @@ void Tasks::Run() {
|
|||
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
||||
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;
|
||||
}
|
||||
|
|
@ -240,7 +279,7 @@ void Tasks::SendToMonTask(void* arg) {
|
|||
while (1) {
|
||||
cout << "wait msg to send" << endl << flush;
|
||||
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);
|
||||
monitor.Write(msg); // The message is deleted with the Write
|
||||
rt_mutex_release(&mutex_monitor);
|
||||
|
|
@ -285,12 +324,31 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
rt_mutex_release(&mutex_move);
|
||||
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
|
||||
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)){
|
||||
rt_mutex_acquire(&mutex_answer_arena);
|
||||
msg_arena = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM? 1:0; // A vérifier que le ternaire est utile
|
||||
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 << "Staut de l'arene" << arenaConfirm << endl << flush;
|
||||
rt_mutex_release(&mutex_answer_arena);
|
||||
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
|
||||
}
|
||||
}
|
||||
|
|
@ -365,35 +423,47 @@ void Tasks::StartRobotTask(void *arg) {
|
|||
* @brief Thread starting the communication with the robot.
|
||||
*/
|
||||
void Tasks::StartCameraTask(void *arg) {
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
|
||||
/**************************************************************************************/
|
||||
/* The task Camera Task starts here */
|
||||
/* The task startRobot starts here */
|
||||
/**************************************************************************************/
|
||||
while (1) {
|
||||
|
||||
Message * msgSend;
|
||||
Message * msg;
|
||||
bool status;
|
||||
rt_sem_p(&sem_startCamera, TM_INFINITE);
|
||||
cout << "Start Camera ";
|
||||
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
|
||||
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
|
||||
if(!camera.Open()){
|
||||
msgSend = new Message(MESSAGE_ANSWER_NACK)
|
||||
if(!cameraStarted){
|
||||
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
|
||||
cout << msgSend->GetID << endl << flush;
|
||||
cout << msgSend->ToString() << endl << flush;
|
||||
rt_mutex_release(&mutex_camera);
|
||||
rt_mutex_release(&mutex_cameraStarted);
|
||||
continue;
|
||||
}
|
||||
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_cameraStarted);
|
||||
|
||||
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
|
||||
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
|
||||
cameraStarted = 1;
|
||||
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) {
|
||||
rt_task_wait_period(NULL);
|
||||
cout << "Periodic movement update";
|
||||
//cout << "Periodic movement update";
|
||||
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
||||
rs = robotStarted;
|
||||
if (rs == 1) {
|
||||
|
|
@ -430,7 +500,7 @@ void Tasks::MoveTask(void *arg) {
|
|||
rt_mutex_release(&mutex_robot);
|
||||
}
|
||||
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.
|
||||
*/
|
||||
void CameraTask(void *arg){
|
||||
void Tasks::CameraTask(void *arg){
|
||||
int cam;
|
||||
std::list<Position> robotPosition;
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
|
|
@ -495,7 +565,6 @@ void CameraTask(void *arg){
|
|||
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
|
||||
if(searchArena){
|
||||
// chopper l'arene
|
||||
cout << "Search arena pls ! " << endl << flush;
|
||||
if(arena.IsEmpty()){
|
||||
cout << "Arena empty" << endl << flush;
|
||||
// envoi d'un no ack au moniteur
|
||||
|
|
@ -505,45 +574,45 @@ void CameraTask(void *arg){
|
|||
img.DrawArena(arena);
|
||||
|
||||
// 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){
|
||||
// chopper le robot
|
||||
cout << "Pos du robot pls !" ;
|
||||
robotPosition = img.SearchRobot(arena);
|
||||
|
||||
|
||||
|
||||
//Traitement du robot
|
||||
if(!robotPosition.empty()){ // Robot position != null
|
||||
img.DrawAllRobots(robotPosition);
|
||||
}
|
||||
for (auto position : robotPosition)
|
||||
{
|
||||
WriteInQueue(&q_messageToMon, new MessagePosition(MESSAGE_CAM_POSITION,position)); // Envoi de toutes les positions
|
||||
}
|
||||
|
||||
}
|
||||
rt_mutex_release(&mutex_robot_pos);
|
||||
|
||||
//Traitement du robot
|
||||
if(){ // Robot position != null
|
||||
img.DrawAllRobots(robotPosition);
|
||||
}
|
||||
|
||||
for (auto position : robotPosition)
|
||||
{
|
||||
WriteInQueue(&q_messageToMon, position); // Envoi de toutes les positions
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Envoi de l'image
|
||||
WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
|
||||
|
||||
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){
|
||||
// Arena OK
|
||||
img.DrawArena(arena); // Comment save ou rejeter?
|
||||
//searchArena = 0;
|
||||
}else{
|
||||
// Arena not OK
|
||||
searchArena = 0;
|
||||
// Rien ?
|
||||
}
|
||||
rt_mutex_release(&mutex_answer_arena);
|
||||
rt_mutex_release(&mutex_search_arena);
|
||||
// Je sais pas si cette partie sert a qqc
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -66,8 +66,10 @@ private:
|
|||
ComRobot robot;
|
||||
Camera camera;
|
||||
int robotStarted = 0;
|
||||
int robotPos = 0;
|
||||
int cameraStarted = 0;
|
||||
int arenaConfirm = 0;
|
||||
int searchArena = 0;
|
||||
int move = MESSAGE_ROBOT_STOP;
|
||||
|
||||
/**********************************************************************/
|
||||
|
|
@ -91,7 +93,9 @@ private:
|
|||
RT_MUTEX mutex_cameraStarted;
|
||||
RT_MUTEX mutex_move;
|
||||
RT_MUTEX mutex_camera;
|
||||
RT_MUTEX mutex_search_arena;
|
||||
RT_MUTEX mutex_answer_arena;
|
||||
RT_MUTEX mutex_robot_pos;
|
||||
|
||||
/**********************************************************************/
|
||||
/* Semaphores */
|
||||
|
|
|
|||
Loading…
Reference in a new issue