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

Camera bancale

Browse source
This commit is contained in:
alejeune 2023-04-11 12:43:11 +02:00
parent 670fff6a0a
commit fa2322f0bd
4 changed files with 102 additions and 62 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -14,6 +14,7 @@
<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/lib/camera.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/img.h</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
</group>
</open-files>

6
software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22
View file

@ -1,11 +1,11 @@
#Fri Apr 07 11:52:43 CEST 2023
#Tue Apr 11 12:37:03 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=c1680861150700
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1681209407979
/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
@ -15,7 +15,7 @@
/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=c1680860665596
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1681208571544
/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

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

@ -104,6 +104,14 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_battery, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_image, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
@ -347,9 +355,13 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_answer_arena);
rt_sem_v(&sem_answerSync);
}else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
cout << "Enabling search arena" << endl << flush;
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1;
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_POSITION_COMPUTE_START)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
@ -361,6 +373,11 @@ void Tasks::ReceiveFromMonTask(void *arg) {
robotPos = 0;
rt_mutex_release(&mutex_robot_pos);
cout << "Robot Pos false" << 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
@ -454,10 +471,16 @@ void Tasks::StartCameraTask(void *arg) {
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
if(!cameraStarted){
status = camera->Open();
cout << "Camera Opened" << endl << flush;
cout << "Camera Opened" << endl << flush; // Toggle a modifier ?
rt_mutex_acquire(&mutex_image, TM_INFINITE);
getImage = 1;
rt_mutex_release(&mutex_image);
}else{
camera->Close();
status = true;
rt_mutex_acquire(&mutex_image, TM_INFINITE);
getImage = 0;
rt_mutex_release(&mutex_image);
cout << "Camera Closed" << endl << flush;
}
@ -475,6 +498,7 @@ void Tasks::StartCameraTask(void *arg) {
cout << cameraStarted << endl << flush;
rt_mutex_release(&mutex_camera);
rt_mutex_release(&mutex_cameraStarted);
cout << "Cam success! " << endl <<flush;
cout << "Fin de start cam" << endl << flush;
@ -507,7 +531,7 @@ void Tasks::MoveTask(void *arg) {
cpMove = move;
rt_mutex_release(&mutex_move);
cout << " move: " << cpMove;
cout << " move: " << cpMove << endl << flush;
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(new Message((MessageID)cpMove));
@ -534,30 +558,29 @@ void Tasks::BatteryTask(void *arg) {
/**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000);
/*while (1) {
while (1) {
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted;
if (rs == 1) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
Message* batteryReply = robot.Write(robot.GetBattery());
string batteryLevel = batteryReply->ToString();
MessageBattery* batteryReply = (MessageBattery*)robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
rt_mutex_release(&mutex_robot);
WriteInQueue(&q_messageToMon, batteryReply);
}
rt_mutex_release(&mutex_robotStarted);
}*/
}
}
/**
* @brief task in charge of managing the camera.
*/
void Tasks::CameraTask(void *arg){
int cam;
int cam;
int grab;
Img* image;
Arena arenaSaved ;
Arena arenaSaved,arena ;
std::list<Position> robotPosition;
MessageImg* imgToSend;
@ -574,63 +597,75 @@ void Tasks::CameraTask(void *arg){
if(!cam){
continue;
}
if(getImage){
cout << "Traitement de l'image" << endl << flush ;
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
// Lancer le traitement périodique
cout << "Traitement de l'image" << endl << flush ;
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
// Lancer le traitement périodique
Img img = camera->Grab();
image = img.Copy();
rt_mutex_release(&mutex_camera);
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);
getImage = 0;
rt_mutex_release(&mutex_image);
// chopper l'arene
if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush;
// envoi d'un no ack au moniteur
Img img = camera->Grab();
image = img.Copy();
rt_mutex_release(&mutex_camera);
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
Arena 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){
// chopper l'arene
if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush;
// envoi d'un no ack au moniteur
}else{
cout << "Draw Arena" << endl << flush;
image->DrawArena(arena);
// envoi de l'arene pour vérif
}else{
cout << "Draw Arena" << endl << flush;
image->DrawArena(arena);
// envoi de l'arene pour vérif
}
}else {
image->DrawArena(arenaSaved);
}
}else {
image->DrawArena(arenaSaved);
}
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
if(robotPos){
// chopper le robot
cout << "Searching robot positions" << endl << flush ;
robotPosition = image->SearchRobot(arena);
//Traitement du robot
if(!robotPosition.empty()){ // Robot position != null
image->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);
// Envoi de l'image
cout << "Envoi de l'image" << endl << flush ;
imgToSend = new MessageImg(MESSAGE_CAM_IMAGE,image);
WriteInQueue(&q_messageToMon, imgToSend);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
if(robotPos){
// chopper le robot
cout << "Pos du robot pls !" ;
robotPosition = image->SearchRobot(arena);
}
//Traitement du robot
if(!robotPosition.empty()){ // Robot position != null
image->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);
// Envoi de l'image
cout << "Envoi de l'image" << endl << flush ;
imgToSend = new MessageImg(MESSAGE_CAM_IMAGE,image);
WriteInQueue(&q_messageToMon, imgToSend);
rt_sem_p(&sem_answerSync, TM_INFINITE);
rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE);
if(arenaConfirm){
// Arena OK
arenaSaved = arena;
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_mutex_acquire(&mutex_image, TM_INFINITE);
getImage = 1; // changer en booleen pour synchroniser plutot que ça
rt_mutex_release(&mutex_image);
searchArena = 0;
}else{
// Arena not OK
// Rien ?
}
rt_mutex_release(&mutex_answer_arena);
rt_mutex_release(&mutex_search_arena);

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

@ -70,6 +70,7 @@ private:
int cameraStarted = 0;
int arenaConfirm = 0;
int searchArena = 0;
int getImage = 0;
int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/
@ -96,6 +97,9 @@ private:
RT_MUTEX mutex_search_arena;
RT_MUTEX mutex_answer_arena;
RT_MUTEX mutex_robot_pos;
RT_MUTEX mutex_battery;
RT_MUTEX mutex_image;
/**********************************************************************/
/* Semaphores */