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 comments. *maybe* fixed issue with overlay not showing when choosing arena.

Browse source
This commit is contained in:
Yohan Boujon 2024-05-14 21:44:18 +02:00
parent 0792fc301d
commit fc2a05ab13
2 changed files with 123 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -24,9 +24,14 @@
#define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20
// Added Priorities
// Camera is the most important as it has the most demanding timing request (10ms)
#define PRIORITY_TCAMERA 21
// Battery has a timing request of 500ms which is not important
#define PRIORITY_TBATTERY 19
// These priorities are for "Semaphored" tasks, not periodic.
// Their priority is lower.
#define PRIORITY_TSETCAMERA 18
#define PRIOTITY_TARENA 17
@ -347,6 +352,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
}
// Added messages
// Battery
else if (msgRcv->CompareID(MESSAGE_ROBOT_BATTERY_GET)) {
@ -549,9 +555,9 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
return msg;
}
/* ------------ */
/************************************************************************/
/* Added Tasks */
/* ------------ */
/************************************************************************/
void Tasks::BatteryStatusTask(void * arg) {
// Variables
@ -563,7 +569,7 @@ void Tasks::BatteryStatusTask(void * arg) {
rt_sem_p(&sem_barrier, TM_INFINITE);
/* The task starts here */
// Periodic task set to 500ms
rt_task_set_periodic(NULL, TM_NOW, 500000000);
while (1) {
@ -574,14 +580,17 @@ void Tasks::BatteryStatusTask(void * arg) {
rs = robotStarted;
rt_mutex_release(&mutex_robotStarted);
// Checking if battery should be gathered
rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE);
getbatterytask = robotBatteryGet;
if ((rs != 0) && getbatterytask) {
// Acquire the battery level and send it to the message queue
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
Message* msg = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
rt_mutex_release(&mutex_robot);
WriteInQueue(&q_messageToMon, msg);
// Resetting the battery status
robotBatteryGet = false;
getbatterytask = false;
}
@ -606,22 +615,25 @@ void Tasks::ManageCameraTask(void * arg)
rt_sem_p(&sem_manageCamera, TM_INFINITE);
// Verify that the robot has started
// (maybe deleted next, because not useful) -> Camera could be started without it
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted;
rt_mutex_release(&mutex_robotStarted);
// Maybe will be deleted -> Camera could be started without the robot
if(rs != 0)
{
// Check the status of the camera
// Check the status of the camera and store it in a local variable
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
// Opening the camera
if(CameraStatusEnum::OPENING == cs)
{
const MessageID tempMessage = this->OpenCamera();
WriteInQueue(&q_messageToMon, new Message(tempMessage));
}
// Closing the camera
if(CameraStatusEnum::CLOSING == cs)
{
this->CloseCamera();
@ -637,24 +649,33 @@ void Tasks::ImageCameraTask(void * arg)
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
// Periodic task set to 10ms
rt_task_set_periodic(NULL, TM_NOW, 100000000);
while(1)
{
rt_task_wait_period(NULL);
// Check the status of the camera
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
const CameraStatusEnum cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
// Only gather image if opened
if(CameraStatusEnum::OPENED == cs)
{
// Gathering an image from the camera
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
Img * img = new Img(cam->Grab());
rt_mutex_release(&mutex_camera);
// If arena has been found, draw overlay
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
if(!arena.IsEmpty())
img->DrawArena(arena);
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
// Sending the image to the monitor
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msgImg);
@ -688,34 +709,46 @@ void Tasks::ArenaChoiceTask(void * arg)
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
const CameraStatusEnum cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
// Gathering last image + closing camera when prompted
std::cout << "\n\n" << (int)cs << "\n\n" << std::endl;
// Only search if camera is already opened
if(CameraStatusEnum::OPENED == cs)
{
// Prepare to close the camera
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cameraStatus = CameraStatusEnum::CLOSING;
rt_mutex_release(&mutex_cameraStatus);
// Gathering last image
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
img = new Img(cam->Grab());
rt_mutex_release(&mutex_camera);
// Closing camera when prompted
this->CloseCamera();
}
// Putting the arena overlay if found
a = img->SearchArena();
}
if(a.IsEmpty())
{
// Debug
std::cout << "\n\n\nArena is empty\n\n\n" << std::endl;
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
}
// If arena is not empty show the arena
else {
// Debug
std::cout << "\n\n\nArena detected\n\n\n" << std::endl;
// Adding overlay on the image
img->DrawArena(a);
// Sending it to the writing queue
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msgImg);
rt_mutex_release(&mutex_monitor);
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
}
// Finished searching for an arena
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
arenaStatus = ArenaStatusEnum::SEARCHED;
rt_mutex_release(&mutex_arenaStatus);
@ -724,7 +757,7 @@ void Tasks::ArenaChoiceTask(void * arg)
// ARENA_CONFIRM / INFIRM
else if(as == ArenaStatusEnum::CONFIRM || as == ArenaStatusEnum::INFIRM)
{
// Store arena to internal buffer + change status
// Store arena to internal buffer (only if confirmed)
if(as == ArenaStatusEnum::CONFIRM)
{
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
@ -734,12 +767,13 @@ void Tasks::ArenaChoiceTask(void * arg)
// empty the temporary arena
a = Arena();
// Re-open the camera
// Re-open the camera (showing overlay if arena is stored)
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cameraStatus = CameraStatusEnum::OPENING;
rt_mutex_release(&mutex_cameraStatus);
this->OpenCamera();
// Change the status of the arena to default value
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
arenaStatus = ArenaStatusEnum::NONE;
rt_mutex_release(&mutex_arenaStatus);
@ -747,9 +781,9 @@ void Tasks::ArenaChoiceTask(void * arg)
}
}
/* --------------- */
/************************************************************************/
/* Added functions */
/* --------------- */
/************************************************************************/
MessageID Tasks::OpenCamera()
{
@ -758,7 +792,8 @@ MessageID Tasks::OpenCamera()
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
const bool isOpened = cam->Open();
rt_mutex_release(&mutex_camera);
// Changing status
// Changing status if succeeded
if(isOpened)
{
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);

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

@ -36,6 +36,12 @@
using namespace std;
/**
* @brief The monitor can change the shared data 'cameraStatus' to
* either 'OPENING' or 'CLOSING'. ManageCamera() will be awake afterwards.
* Then the variable can switch state to open or close the camera.
* Default value should be 'CLOSED'.
*/
enum class CameraStatusEnum {
CLOSED,
OPENING,
@ -43,6 +49,12 @@ enum class CameraStatusEnum {
CLOSING
};
/**
* @ref CameraStatusEnum "Same principle"
* @brief The arena can change the shared data 'arenaStatus' to
* either 'SEARCHING', 'CONFIRM' or 'INFIRM'. The other status will be
* changed by the task ArenaChoice().
*/
enum class ArenaStatusEnum {
NONE,
SEARCHING,
@ -82,10 +94,20 @@ private:
ComRobot robot;
int robotStarted = 0;
int move = MESSAGE_ROBOT_STOP;
/************************************************************************/
/* Added variables */
/************************************************************************/
// when gathering battery, set to true
bool robotBatteryGet = false;
// current status of the camera
CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED;
Camera* cam = new Camera(sm, 10);;
// camera object
Camera* cam = new Camera(sm, 10);
// current status of the arena
ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE;
// arena object (overlay of the arena on the img)
Arena arena;
/**********************************************************************/
@ -109,10 +131,18 @@ private:
RT_MUTEX mutex_robot;
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
// Added mutexes
// Locking the variable 'robotBatteryGet'
RT_MUTEX mutex_batteryGet;
// Locking the variable 'cameraStatus'
RT_MUTEX mutex_cameraStatus;
// Locking the variable 'cam'
RT_MUTEX mutex_camera;
// Locking the variable 'arena'
RT_MUTEX mutex_arena;
// Locking the variable 'arenaStatus'
RT_MUTEX mutex_arenaStatus;
/**********************************************************************/
@ -122,6 +152,9 @@ private:
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
// Added semaphores
// Used to call ManageCameraTask (non-periodic)
RT_SEM sem_manageCamera;
// Used to call ArenaChoiceTask (non-periodic)
@ -167,20 +200,51 @@ private:
void MoveTask(void *arg);
/* ------------ */
/************************************************************************/
/* Added Tasks */
/* ------------ */
/************************************************************************/
/**
* @brief [periodic: 500ms]
* Task managing the battery. When prompted will get the battery
* from the robot and resend it to the monitor.
*/
void BatteryStatusTask(void * arg);
/**
* @brief Task managing the camera opening and closing. Will check what
* state the camera has and change depending on what the monitor asks.
*/
void ManageCameraTask(void * arg);
/**
* @brief [periodic: 10ms]
* Task Managing the image received by the camera. Will send each frame
* if camera's state is opened.
*/
void ImageCameraTask(void * arg);
/**
* @brief Task managing the Arena.
* Will search for a camera and overlay if found.
* Will store the arena and then show it afterwards.
*/
void ArenaChoiceTask(void * arg);
/* --------------- */
/************************************************************************/
/* Added functions */
/* --------------- */
/************************************************************************/
/**
* @brief Thread safe. Will open the camera and change the status if succeeded.
*
* @return MessageID returns MESSAGE_ANSWER_ACK if successful, NACK if not.
*/
MessageID OpenCamera();
/**
* @brief Thread safe. Will close the camera and change the status.
*/
void CloseCamera();