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

Create Task ReceiveFromMon

Browse source
This commit is contained in:
Raphael Benistant 2020-03-26 18:14:09 +01:00
parent 23fa284ea6
commit 436819d9f3
3 changed files with 193 additions and 17 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
software/raspberry/superviseur-robot/main.cpp
View file

@ -36,6 +36,8 @@ int main(int argc, char **argv) {
cout<<"# DE STIJL PROJECT #"<<endl;
cout<<"#################################"<<endl;
//realease Sem just the first time
tasks.Init();
tasks.Run();
tasks.Join();

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

@ -75,6 +75,59 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_killReceiveFromMon, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_killVision, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_searchArena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_drawArena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_getPosition, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_killSendMon, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_killStartRobWD, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_killDetectLostSupRob, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_killStartRob, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_acquireImage, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
/*if (err = rt_mutex_create(&mutex_robot_on, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -106,6 +159,16 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_allow_StartReceive, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
//Initialization for some specific sem: Allow to run the first time
rt_sem_v(&sem_allow_StartReceive);
cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/
@ -265,18 +328,27 @@ void Tasks::SendToMonTask(void* arg) {
*/
void Tasks::ReceiveFromMonTask(void *arg) {
Message *msgRcv;
bool Ok=1;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
//Wait twin task to die if not first round
rt_sem_p(&sem_allow_StartReceive, TM_INFINITE);
//Reinitialize control boolean
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMon=0;
rt_mutex_release(&mutex_killReceiveFromMon);
/**************************************************************************************/
/* The task receiveFromMon starts here */
/**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Received message from monitor activated" << endl << flush;
while (1) {
while (Ok) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
@ -284,14 +356,87 @@ void Tasks::ReceiveFromMonTask(void *arg) {
delete(msgRcv);
cout << "Connection to monitor lost" << endl;
monitor.Close();
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMon=1;
rt_mutex_release(&mutex_killReceiveFromMon);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=1;
rt_mutex_release(&mutex_killVision);
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
killSendMon=1;
rt_mutex_release(&mutex_killSendMon);
rt_mutex_acquire(&mutex_killStartRobWD, TM_INFINITE);
killStartRobWD=1;
rt_mutex_release(&mutex_killStartRobWD);
rt_mutex_acquire(&mutex_killStartRob, TM_INFINITE);
killStartRob=1;
rt_mutex_release(&mutex_killStartRob);
rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
killDetectlostSupRob=1;
rt_mutex_release(&mutex_killDetectLostSupRob);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=0;
rt_mutex_release(&mutex_acquireImage);
//exit(-1);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
//start task Vision
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_sem_v(&sem_startRobotWithoutWatchdog);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
rt_sem_v(&sem_startRobotWithWatchdog);
//start task robot with watchdog
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=1;
rt_mutex_release(&mutex_searchArena);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=1;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
drawArena=1;
rt_mutex_release(&mutex_drawArena);
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0;
rt_mutex_release(&mutex_searchArena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0;
rt_mutex_release(&mutex_searchArena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@ -302,8 +447,15 @@ void Tasks::ReceiveFromMonTask(void *arg) {
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
}
delete(msgRcv); // mus be deleted manually, no consumer
delete(msgRcv); // must be deleted manually, no consumer
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
Ok=!killReceiveFromMon;
rt_mutex_release(&mutex_killReceiveFromMon);
}
rt_sem_v(&sem_allow_StartReceive);
}
/**
@ -359,9 +511,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
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_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=0;
rt_mutex_release(&mutex_killBattery);
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithoutWD());
rt_mutex_release(&mutex_robot);
@ -384,9 +536,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
monitor.Write(p_mess_answer_battery);
rt_mutex_release(&mutex_monitor);
//cout << endl << flush;
rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
killBattery=killBatteryBool;
rt_mutex_release(&mutex_kill_battery);
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=killBattery;
rt_mutex_release(&mutex_killBattery);
}
}
}
@ -417,9 +569,9 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
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);
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=0;
rt_mutex_release(&mutex_killBattery);
cout << msgSend->GetID();
cout << ")" << endl;

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

@ -65,9 +65,20 @@ private:
ComMonitor monitor;
ComRobot robot;
int robotStarted = 0;
int killBatteryBool = 0;
int killRobotWithoutWd = 0;
int killBattery = 0;
int move = MESSAGE_ROBOT_STOP;
bool killReceiveFromMon=0;
bool killVision=0;
bool searchArena=0;
bool killSendMon=0;
bool killStartRobWD=0;
bool killStartRob=0;
bool killDetectlostSupRob=0;
bool acquireImage=0;
bool getPosition=0;
bool drawArena=0;
/**********************************************************************/
/* Tasks */
@ -87,8 +98,18 @@ private:
RT_MUTEX mutex_robot;
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
RT_MUTEX mutex_kill_battery;
RT_MUTEX mutex_killBattery;
RT_MUTEX mutex_killReceiveFromMon;
RT_MUTEX mutex_killVision;
RT_MUTEX mutex_searchArena;
RT_MUTEX mutex_drawArena;
RT_MUTEX mutex_getPosition;
RT_MUTEX mutex_killSendMon;
RT_MUTEX mutex_killStartRobWD;
RT_MUTEX mutex_killDetectLostSupRob;
RT_MUTEX mutex_killStartRob;
RT_MUTEX mutex_acquireImage;
/**********************************************************************/
/* Semaphores */
/**********************************************************************/
@ -97,7 +118,8 @@ private:
RT_SEM sem_serverOk;
RT_SEM sem_startRobotWithoutWatchdog;
RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_allow_StartReceive;
/**********************************************************************/
/* Message queues */
/**********************************************************************/