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

Fixed timing issue with battery

Browse source
This commit is contained in:
Robin Marin-Muller 2024-05-15 20:15:47 +02:00
parent d89d36d040
commit 0121dc5707
2 changed files with 66 additions and 32 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -127,12 +127,12 @@ 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)) {
// Added Semaphores
if (err = rt_sem_create(&sem_instantBattery, 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)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -171,6 +171,10 @@ void Tasks::Init() {
exit(EXIT_FAILURE);
}
// Added tasks
if (err = rt_task_create(&th_batteryPeriodic, "th_batteryPeriodic", 0, PRIORITY_TBATTERY, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_battery, "th_battery", 0, PRIORITY_TBATTERY, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -234,7 +238,11 @@ void Tasks::Run() {
}
// Added Tasks
if (err = rt_task_start(&th_battery, (void(*)(void*)) & Tasks::BatteryStatusTask, this)) {
if (err = rt_task_start(&th_batteryPeriodic, (void(*)(void*)) & Tasks::BatteryPeriodicTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_battery, (void(*)(void*)) & Tasks::BatteryTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
@ -364,8 +372,11 @@ void Tasks::ReceiveFromMonTask(void *arg) {
// Battery
else if (msgRcv->CompareID(MESSAGE_ROBOT_BATTERY_GET)) {
rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE);
robotBatteryGet = true;
batteryEnabled = true;
rt_mutex_release(&mutex_batteryGet);
// Calling BatteryTask, unblocking the thread.
rt_sem_v(&sem_instantBattery);
}
// Camera
else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
@ -570,10 +581,10 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
/* Added Tasks */
/************************************************************************/
void Tasks::BatteryStatusTask(void * arg) {
void Tasks::BatteryPeriodicTask(void * arg) {
// Variables
int rs(0);
bool getbatterytask(false);
bool be(false);
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
@ -585,27 +596,24 @@ void Tasks::BatteryStatusTask(void * arg) {
while (1) {
rt_task_wait_period(NULL);
// Verify that the robot has started
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
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;
this->SendBattery(rs,be);
}
rt_mutex_release(&mutex_batteryGet);
}
void Tasks::BatteryTask(void * arg) {
// Variables
int rs(0);
bool be(false);
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
while(1)
{
// Called when semaphore arenChoice incremented
rt_sem_p(&sem_instantBattery, TM_INFINITE);
this->SendBattery(rs,be);
}
}
@ -622,7 +630,7 @@ void Tasks::ManageCameraTask(void * arg)
while(1)
{
// Called when semaphore arenChoice incremented
// Called when semaphore manageCamera incremented
rt_sem_p(&sem_manageCamera, TM_INFINITE);
// Check the status of the camera and store it in a local variable
@ -812,6 +820,27 @@ void Tasks::ArenaChoiceTask(void * arg)
/* Added functions */
/************************************************************************/
void Tasks::SendBattery(int& rs, bool& be)
{
// Verify that the robot has started
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted;
rt_mutex_release(&mutex_robotStarted);
// Checking if battery should be gathered
rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE);
be = batteryEnabled;
rt_mutex_release(&mutex_batteryGet);
if ((rs != 0) && be) {
// 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);
}
}
MessageID Tasks::OpenCamera()
{
cout << "Called " << __PRETTY_FUNCTION__ << endl << flush;

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

@ -99,7 +99,7 @@ private:
/************************************************************************/
// when gathering battery, set to true
bool robotBatteryGet = false;
bool batteryEnabled = false;
// current status of the camera
CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED;
// camera object
@ -120,6 +120,8 @@ private:
RT_TASK th_openComRobot;
RT_TASK th_startRobot;
RT_TASK th_move;
RT_TASK th_batteryPeriodic;
RT_TASK th_battery;
RT_TASK th_cameraManage;
RT_TASK th_cameraImage;
@ -158,12 +160,12 @@ private:
// Added semaphores
// Used when we want to show the battery once.
RT_SEM sem_instantBattery;
// Used to call ManageCameraTask (non-periodic)
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 */
@ -214,7 +216,8 @@ private:
* Task managing the battery. When prompted will get the battery
* from the robot and resend it to the monitor.
*/
void BatteryStatusTask(void * arg);
void BatteryPeriodicTask(void * arg);
void BatteryTask(void * arg);
/**
* @brief Task managing the camera opening and closing. Will check what
@ -243,6 +246,8 @@ private:
/* Added functions */
/************************************************************************/
void SendBattery(int& rs, bool& be);
/**
* @brief Thread safe. Will open the camera and change the status if succeeded.
*