diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.144-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.144-xenomai-22 index 0816957..070c991 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.144-xenomai-22 +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.144-xenomai-22 @@ -1,4 +1,4 @@ -#Tue Mar 26 15:23:55 CET 2024 +#Tue Mar 26 16:47:57 CET 2024 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/main.cpp=c1711457904172 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1711457904197 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1711458297850 @@ -8,13 +8,13 @@ /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1711458297867 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1711457904114 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1711457904187 -/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1711461453052 +/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1711467801732 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/README.md=c1711457904021 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1711458720366 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1711457904182 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1711457904238 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1711457904124 -/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1711463022931 +/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1711468071294 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1711457904151 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/img.h=c1711457904162 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/.dep.inc=c1711458948692 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 7b2a4eb..b3d7fe7 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -74,6 +74,10 @@ void Tasks::Init() { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_mutex_create(&mutex_batteryGet, NULL)) { + cerr << "Error mutex create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Mutexes created successfully" << endl << flush; /**************************************************************************************/ @@ -201,7 +205,7 @@ void Tasks::Join() { */ void Tasks::ServerTask(void *arg) { int status; - + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are started) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -228,7 +232,7 @@ void Tasks::ServerTask(void *arg) { */ void Tasks::SendToMonTask(void* arg) { Message *msg; - + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -253,11 +257,11 @@ void Tasks::SendToMonTask(void* arg) { */ void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; - + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - + /**************************************************************************************/ /* The task receiveFromMon starts here */ /**************************************************************************************/ @@ -274,7 +278,9 @@ void Tasks::ReceiveFromMonTask(void *arg) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { rt_sem_v(&sem_openComRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_BATTERY_GET)) { - printf("Battery handler\n\r"); + rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE); + robotBatteryGet = true; + rt_mutex_release(&mutex_batteryGet); } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { rt_sem_v(&sem_startRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || @@ -301,7 +307,7 @@ void Tasks::OpenComRobot(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); - + /**************************************************************************************/ /* The task openComRobot starts here */ /**************************************************************************************/ @@ -331,7 +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); - + /**************************************************************************************/ /* The task startRobot starts here */ /**************************************************************************************/ @@ -347,7 +353,7 @@ void Tasks::StartRobotTask(void *arg) { cout << ")" << endl; cout << "Movement answer: " << msgSend->ToString() << endl << flush; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); @@ -363,11 +369,11 @@ void Tasks::StartRobotTask(void *arg) { void Tasks::MoveTask(void *arg) { int rs; int cpMove; - + 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 */ /**************************************************************************************/ @@ -375,7 +381,7 @@ void Tasks::MoveTask(void *arg) { while (1) { rt_task_wait_period(NULL); - cout << "Periodic movement update"; + //cout << "Periodic movement update"; rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; rt_mutex_release(&mutex_robotStarted); @@ -383,14 +389,14 @@ void Tasks::MoveTask(void *arg) { rt_mutex_acquire(&mutex_move, TM_INFINITE); cpMove = move; rt_mutex_release(&mutex_move); - + cout << " move: " << cpMove; - + rt_mutex_acquire(&mutex_robot, TM_INFINITE); - robot.Write(new Message((MessageID)cpMove)); + robot.Write(new Message((MessageID) cpMove)); rt_mutex_release(&mutex_robot); } - cout << endl << flush; + //cout << endl << flush; } } @@ -426,36 +432,41 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) { return msg; } - - /* OUR CODE */ void Tasks::BatteryStatusTask(void * arg) { // Variables int rs(0); - + bool getbatterytask(false); + 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 */ - rt_task_set_periodic(NULL, TM_NOW, 50000000); - - while (1) { + rt_task_set_periodic(NULL, TM_NOW, 500000000); + + while (1) { rt_task_wait_period(NULL); - std::cout << "Periodic battery level update\n\r" << std::flush; + + // Verify that the robot has started rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; rt_mutex_release(&mutex_robotStarted); - if (rs =! 1) - continue; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - Message* msg = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET)); - std::cout << "Message Received: " << msg->ToString() << std::endl; - - rt_mutex_release(&mutex_robot); - cout << endl << flush; + rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE); + getbatterytask = robotBatteryGet; + + if ((rs != 0) && getbatterytask) { + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + Message* msg = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET)); + rt_mutex_release(&mutex_robot); + std::cout << "Message Received: " << msg->ToString() << std::endl; + robotBatteryGet = false; + getbatterytask = false; + + } + rt_mutex_release(&mutex_batteryGet); } } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index b1768d2..c4523f6 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -66,7 +66,7 @@ private: ComRobot robot; int robotStarted = 0; int move = MESSAGE_ROBOT_STOP; - int robotBattery = 0; + bool robotBatteryGet = false; /**********************************************************************/ /* Tasks */ @@ -86,6 +86,7 @@ private: RT_MUTEX mutex_robot; RT_MUTEX mutex_robotStarted; RT_MUTEX mutex_move; + RT_MUTEX mutex_batteryGet; /**********************************************************************/ /* Semaphores */