INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 13:50:49 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Added Mutex and Thread management for arenaChoice.

Browse source
This commit is contained in:
Yohan Boujon 2024-05-11 11:20:44 +02:00
parent 8f84e6d42e
commit 5904a21007
2 changed files with 65 additions and 19 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -19,6 +19,7 @@
#include <stdexcept>
// Déclaration des priorités des taches
// 99 -> haute, 0 -> basse
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TMOVE 20
@ -28,6 +29,7 @@
#define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 19
#define PRIORITY_TSETCAMERA 18
#define PRIOTITY_TARENA 17
/*
* Some remarks:
@ -88,6 +90,10 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_arenaStatus, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
@ -155,6 +161,10 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_arenaChoice, "th_arenaChoice", 0, PRIOTITY_TARENA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/
@ -216,6 +226,10 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_arenaChoice, (void(*)(void*)) & Tasks::ArenaChoice, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush;
}
@ -344,9 +358,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_cameraStatus);
}
else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
std::cout << "\n\n\nArena asked !!!\n\n\n" << std::endl;
cameraAsked();
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
if(cameraStatus == ArenaStatusEnum::NONE)
cameraStatus = ArenaStatusEnum::SEARCHING;
rt_mutex_release(&mutex_arenaStatus);
}
else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl;
@ -644,21 +659,43 @@ void Tasks::CloseCamera(void * arg)
}
}
void Tasks::cameraAsked()
void Tasks::ArenaChoice(void * arg)
{
Img * img = new Img(cam->Grab());
Arena a = img->SearchArena();
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
Img * img = nullptr;
Arena a;
ArenaStatusEnum as = ArenaStatusEnum::NONE;
std::cout << "\n\n\n" << img->ToString() << "\n\n\n" << std::endl;
if(a.IsEmpty())
while(1)
{
std::cout << "\n\n\nArena null :(\n\n\n" << std::endl;
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
// Check the status of the arena
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
as = arenaStatus;
rt_mutex_release(&mutex_arenaStatus);
if(as == ArenaStatusEnum::SEARCHING)
{
// Gathering last image + closing camera when prompted
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
if(cam->IsOpen())
{
img = new Img(cam->Grab());
cam->Close();
}
rt_mutex_release(&mutex_camera);
// Putting the arena overlay if found
a = img->SearchArena();
if(a.IsEmpty())
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
else {
std::cout << "\n\n\nArena Image sending :)\n\n\n" << std::endl;
img->DrawArena(a);
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msgImg);
rt_mutex_release(&mutex_monitor);
}
}
}
}

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

@ -43,6 +43,12 @@ enum class CameraStatusEnum {
CLOSING
};
enum class ArenaStatusEnum {
NONE,
SEARCHING,
CHOOSEN
};
class Tasks {
public:
/**
@ -75,6 +81,7 @@ private:
int move = MESSAGE_ROBOT_STOP;
bool robotBatteryGet = false;
CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED;
ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE;
Camera* cam = nullptr;
/**********************************************************************/
@ -90,6 +97,7 @@ private:
RT_TASK th_cameraOpen;
RT_TASK th_cameraImage;
RT_TASK th_cameraClose;
RT_TASK th_arenaChoice;
/**********************************************************************/
/* Mutex */
@ -101,6 +109,7 @@ private:
RT_MUTEX mutex_batteryGet;
RT_MUTEX mutex_cameraStatus;
RT_MUTEX mutex_camera;
RT_MUTEX mutex_arenaStatus;
/**********************************************************************/
/* Semaphores */
@ -156,7 +165,7 @@ private:
void OpenCamera(void * arg);
void ImageCamera(void * arg);
void CloseCamera(void * arg);
void ArenaChoice(void * arg);
// Utility functions
@ -178,7 +187,7 @@ private:
*/
Message *ReadInQueue(RT_QUEUE *queue);
void cameraAsked();