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

que dalle

Browse source
This commit is contained in:
Romain Vitrat 2020-03-26 15:51:24 +01:00
parent 23fa284ea6
commit 735739b774
1 changed files with 19 additions and 20 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -24,8 +24,8 @@
#define PRIORITY_TMOVE 20 #define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22 #define PRIORITY_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22 #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 24
#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 23
#define PRIORITY_TCAMERA 21 #define PRIORITY_TCAMERA 21
@ -75,10 +75,10 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
/*if (err = rt_mutex_create(&mutex_robot_on, NULL)) { if (err = rt_mutex_create(&mutex_kill_battery, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
}*/ }
cout << "Mutexes created successfully" << endl << flush; cout << "Mutexes created successfully" << endl << flush;
@ -349,13 +349,13 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
int killBattery=0; int killBattery=0;
Message* p_mess_answer_battery;
Message * msgSend;
/**************************************************************************************/ /**************************************************************************************/
/* The task startRobot starts here */ /* The task startRobot starts here */
/**************************************************************************************/ /**************************************************************************************/
Message* p_mess_answer_battery;
Message * msgSend;
rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
cout << "Start robot without watchdog ("; cout << "Start robot without watchdog (";
//Boolean to get the battery //Boolean to get the battery
@ -380,10 +380,8 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
p_mess_answer_battery = robot.Write(robot.GetBattery()); p_mess_answer_battery = robot.Write(robot.GetBattery());
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); WriteInQueue(&q_messageToMon, p_mess_answer_battery);
monitor.Write(p_mess_answer_battery); cout << endl << flush;
rt_mutex_release(&mutex_monitor);
//cout << endl << flush;
rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
killBattery=killBatteryBool; killBattery=killBatteryBool;
rt_mutex_release(&mutex_kill_battery); rt_mutex_release(&mutex_kill_battery);
@ -408,9 +406,8 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
/**************************************************************************************/ /**************************************************************************************/
Message* p_mess_answer_battery; Message* p_mess_answer_battery;
Message * msgSend; Message * msgSend;
int cpt=1; int cpt=0;
int err; int killBattery=0;
rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
cout << "Start robot with watchdog ("; cout << "Start robot with watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithWD()); msgSend = robot.Write(robot.StartWithWD());
@ -433,21 +430,23 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
robotStarted = 1; robotStarted = 1;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
rt_task_set_periodic(NULL, TM_NOW, 500000000); rt_task_set_periodic(NULL, TM_NOW, 500000000);
while (1) { while (killBattery==0) {
cpt++; cpt++;
if(cpt%2==0){ if(cpt==2){
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(robot.ReloadWD()); robot.Write(robot.ReloadWD());
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
cpt=0;
} }
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
p_mess_answer_battery = robot.Write(robot.GetBattery()); p_mess_answer_battery = robot.Write(robot.GetBattery());
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); WriteInQueue(&q_messageToMon, p_mess_answer_battery);
monitor.Write(p_mess_answer_battery);
rt_mutex_release(&mutex_monitor);
cout << endl << flush; cout << endl << flush;
rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
killBattery=killBatteryBool;
rt_mutex_release(&mutex_kill_battery);
} }
} }
} }
@ -482,7 +481,7 @@ void Tasks::MoveTask(void *arg) {
cpMove = move; cpMove = move;
rt_mutex_release(&mutex_move); rt_mutex_release(&mutex_move);
cout << " move: " << cpMove; //cout << " move: " << cpMove;
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(new Message((MessageID)cpMove)); robot.Write(new Message((MessageID)cpMove));