diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp index cbc618a..3faf608 100644 --- a/software/raspberry/superviseur-robot/lib/comrobot.cpp +++ b/software/raspberry/superviseur-robot/lib/comrobot.cpp @@ -327,6 +327,7 @@ string ComRobot::MessageToString(Message *msg) { break; case MESSAGE_ROBOT_RELOAD_WD: s += LABEL_ROBOT_RELOAD_WD; + cout << "watchdog" << endl; break; case MESSAGE_ROBOT_BATTERY_GET: s += LABEL_ROBOT_GET_BATTERY; diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 1524e7a..dacb50d 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -106,6 +106,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; /**************************************************************************************/ @@ -139,6 +143,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_TMOVE, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Tasks created successfully" << endl << flush; @@ -188,7 +196,11 @@ void Tasks::Run() { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - + if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } + cout << "Tasks launched" << endl << flush; } @@ -349,19 +361,17 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - int killBattery=0; - Message* p_mess_answer_battery; Message * msgSend; /**************************************************************************************/ /* The task startRobot starts here */ /**************************************************************************************/ - cout << "Start robot without watchdog ("; - //Boolean to get the battery rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); killBatteryBool=0; rt_mutex_release(&mutex_kill_battery); + rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE); + cout << "Start robot without watchdog ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); msgSend = robot.Write(robot.StartWithoutWD()); rt_mutex_release(&mutex_robot); @@ -377,11 +387,7 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { rt_task_set_periodic(NULL, TM_NOW, 500000000); while (killBattery==0) { 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); - WriteInQueue(&q_messageToMon, p_mess_answer_battery); - cout << endl << flush; + rt_sem_v(&sem_askBattery); rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); killBattery=killBatteryBool; rt_mutex_release(&mutex_kill_battery); @@ -396,36 +402,32 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { * @brief Thread starting the communication with the robot. */ + void Tasks::StartRobotTaskWithWatchdog(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - + int killBattery=0; + int cpt=0; + Message * msgSend; /**************************************************************************************/ /* The task startRobot starts here */ /**************************************************************************************/ - Message* p_mess_answer_battery; - Message * msgSend; - int cpt=0; - int killBattery=0; + //Boolean to get the battery + rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); + killBatteryBool=0; + rt_mutex_release(&mutex_kill_battery); + rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); cout << "Start robot with watchdog ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); msgSend = robot.Write(robot.StartWithWD()); rt_mutex_release(&mutex_robot); - - //boolean to ask battery - rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); - killBatteryBool=0; - rt_mutex_release(&mutex_kill_battery); - cout << msgSend->GetID(); - cout << ")" << endl; - + cout << ")" << endl; cout << "Movement answer: " << msgSend->ToString() << endl << flush; WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); robotStarted = 1; rt_mutex_release(&mutex_robotStarted); @@ -433,17 +435,11 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) { while (killBattery==0) { cpt++; if(cpt==2){ - rt_mutex_acquire(&mutex_robot, TM_INFINITE); robot.Write(robot.ReloadWD()); - rt_mutex_release(&mutex_robot); cpt=0; } 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); - WriteInQueue(&q_messageToMon, p_mess_answer_battery); - cout << endl << flush; + rt_sem_v(&sem_askBattery); rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE); killBattery=killBatteryBool; rt_mutex_release(&mutex_kill_battery); @@ -492,6 +488,26 @@ void Tasks::MoveTask(void *arg) { } +void Tasks::AskBattery(void *arg){ + Message* p_mess_answer_battery; + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + + /**************************************************************************************/ + /* The task starts here */ + /**************************************************************************************/ + while(1){ + rt_sem_p(&sem_askBattery, TM_INFINITE); + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + p_mess_answer_battery = robot.Write(robot.GetBattery()); + rt_mutex_release(&mutex_robot); + WriteInQueue(&q_messageToMon, p_mess_answer_battery); + 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 609aeda..cdcd957 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -79,6 +79,7 @@ private: RT_TASK th_startRobotWithoutWatchdog; RT_TASK th_startRobotWithWatchdog; RT_TASK th_move; + RT_TASK th_askBattery; /**********************************************************************/ /* Mutex */ @@ -97,6 +98,7 @@ private: RT_SEM sem_serverOk; RT_SEM sem_startRobotWithoutWatchdog; RT_SEM sem_startRobotWithWatchdog; + RT_SEM sem_askBattery; /**********************************************************************/ /* Message queues */ @@ -144,6 +146,9 @@ private: + + void AskBattery(void *arg); + /**********************************************************************/ /* Queue services */ /**********************************************************************/ diff --git a/software/simulateur/main.cpp b/software/simulateur/main.cpp index 969c35f..71906df 100755 --- a/software/simulateur/main.cpp +++ b/software/simulateur/main.cpp @@ -123,7 +123,8 @@ int main(int argc, char const *argv[]) { print_time(start_time); printf(" >>> I received a message : %s\n", buffer); string s = ""; - int error = simulate_error(); + //int error = simulate_error(); + int error = 0; if (error == 0) { struct timespec t; long int e;