diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index f17ae21..d53aec6 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -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 diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 27fe329..bf8fe06 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -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 */ /**********************************************************************/