vitrat/real_time
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

batterie ok

Browse source
This commit is contained in:
Romain Vitrat 2020-03-18 17:13:24 +01:00
parent 7f0a9955db
commit 9b30deb263
2 changed files with 54 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -26,6 +26,8 @@
#define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 31
/*
* Some remarks:
@ -73,6 +75,9 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
@ -94,6 +99,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_askBattery, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/
@ -123,6 +132,10 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_askBattery, "th_askBattery", 0, PRIORITY_TBATTERY, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/
@ -167,6 +180,10 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::Ask_Battery, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush;
}
@ -320,6 +337,7 @@ void Tasks::StartRobotTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_v(&sem_askBattery);
/**************************************************************************************/
/* The task startRobot starts here */
@ -383,6 +401,34 @@ void Tasks::MoveTask(void *arg) {
}
}
void Tasks::Ask_Battery(void *arg){
Message* p_mess_answer_battery;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_askBattery, TM_INFINITE);
/**************************************************************************************/
/* The task starts here */
/**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 10000000000);
while (1) {
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
p_mess_answer_battery = robot.Write(robot.GetBattery());
rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(p_mess_answer_battery);
rt_mutex_release(&mutex_monitor);
cout << endl << flush;
}
}
/**
* Write a message in a given queue
* @param queue Queue identifier

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

@ -76,6 +76,7 @@ private:
RT_TASK th_openComRobot;
RT_TASK th_startRobot;
RT_TASK th_move;
RT_TASK th_askBattery;
/**********************************************************************/
/* Mutex */
@ -84,6 +85,7 @@ private:
RT_MUTEX mutex_robot;
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
RT_MUTEX mutex_askBattery;
/**********************************************************************/
/* Semaphores */
@ -92,6 +94,7 @@ private:
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
RT_SEM sem_askBattery;
/**********************************************************************/
/* Message queues */
@ -132,6 +135,11 @@ private:
*/
void MoveTask(void *arg);
void Ask_Battery(void *arg);
/**********************************************************************/
/* Queue services */
/**********************************************************************/