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

Added basic position management.

Browse source
This commit is contained in:
Robin Marin-Muller 2024-05-15 18:39:08 +02:00
parent 5b9656c67a
commit ce68e09679
2 changed files with 49 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -17,6 +17,7 @@
#include "tasks.h"
#include <stdexcept>
#include <vector>
// Task priority: higher has the best priority
#define PRIORITY_TSERVER 30
@ -35,6 +36,7 @@
// Their priority is lower.
#define PRIORITY_TSETCAMERA 18
#define PRIOTITY_TARENA 17
#define PRIOTITY_TPOSITION 16
/*
* Some remarks:
@ -46,7 +48,7 @@
* 3- Data flow is probably not optimal
*
* 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
* time for internal buffer to flush
* time for interponal buffer to flush
*
* 5- Same behavior existe for ComMonitor::Write !
*
@ -125,6 +127,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_findPosition, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
// Added Semaphores
if (err = rt_sem_create(&sem_manageCamera, NULL, 0, S_FIFO)) {
@ -408,6 +414,16 @@ void Tasks::ReceiveFromMonTask(void *arg) {
// Calling arenaChoice, unblocking the thread.
rt_sem_v(&sem_arenaChoice);
}
else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
rt_mutex_acquire(&mutex_positionEnabled, TM_INFINITE);
positionEnabled = true;
rt_mutex_release(&mutex_positionEnabled);
}
else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
rt_mutex_acquire(&mutex_positionEnabled, TM_INFINITE);
positionEnabled = false;
rt_mutex_release(&mutex_positionEnabled);
}
else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) {
//?
}
@ -634,6 +650,9 @@ void Tasks::ImageCameraTask(void * arg)
// Variables
CameraStatusEnum cs = CameraStatusEnum::CLOSED;
Arena a = Arena();
std::vector<MessagePosition*> msgPos = {};
MessageImg *msgImg = nullptr;
bool pe = false;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
@ -656,6 +675,10 @@ void Tasks::ImageCameraTask(void * arg)
a = arena;
rt_mutex_release(&mutex_arena);
rt_mutex_acquire(&mutex_positionEnabled, TM_INFINITE);
pe = positionEnabled;
rt_mutex_release(&mutex_positionEnabled);
// Only gather image if opened
if(CameraStatusEnum::OPENED == cs)
{
@ -668,9 +691,24 @@ void Tasks::ImageCameraTask(void * arg)
if(!a.IsEmpty())
img->DrawArena(a);
// Check position
auto listPos = img->SearchRobot(a);
img->DrawAllRobots(listPos);
// Sending the image to the monitor
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
if(!listPos.empty())
{
for(auto l : listPos)
msgPos.emplace_back(new MessagePosition(MESSAGE_CAM_POSITION, l));
}
msgImg= new MessageImg(MESSAGE_CAM_IMAGE, img);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
if(!listPos.empty())
{
for(auto mg : msgPos)
monitor.Write(mg);
msgPos.clear();
}
monitor.Write(msgImg);
rt_mutex_release(&mutex_monitor);
}

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

@ -108,6 +108,8 @@ private:
ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE;
// arena object (overlay of the arena on the img)
Arena arena;
// when gathering position, set to true
bool positionEnabled = false;
/**********************************************************************/
/* Tasks */
@ -143,6 +145,8 @@ private:
RT_MUTEX mutex_arena;
// Locking the variable 'arenaStatus'
RT_MUTEX mutex_arenaStatus;
// Locking the variable 'positionEnabled'
RT_MUTEX mutex_positionEnabled;
/**********************************************************************/
/* Semaphores */
@ -158,6 +162,8 @@ private:
RT_SEM sem_manageCamera;
// Used to call ArenaChoiceTask (non-periodic)
RT_SEM sem_arenaChoice;
// Used to call FindPositionTask (non-periodic)
RT_SEM sem_findPosition;
/**********************************************************************/
/* Message queues */
@ -230,6 +236,9 @@ private:
*/
void ArenaChoiceTask(void * arg);
void FindPositionTask(void * arg);
/************************************************************************/
/* Added functions */
/************************************************************************/