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

Connection with Robot Impossible

Browse source
This commit is contained in:
Raphael Benistant 2020-03-29 09:08:54 +02:00
parent 07c5afc82a
commit 4826ac1aeb
2 changed files with 271 additions and 291 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -20,7 +20,7 @@
// Déclaration des priorités des taches
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TOPENCOMROBOT 50
#define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25
@ -155,37 +155,16 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_allowStartDetectLostSupRob, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_allowSendToMon, NULL, 0, S_FIFO)) {
if (err = rt_sem_create(&sem_restart, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_allowOpenComRobot, 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_allowStartReceive);
rt_sem_v(&sem_allowStartDetectLostSupRob);
rt_sem_v(&sem_restart);
rt_sem_v(&sem_allowOpenComRobot);
}
cout << "Semaphores created successfully" << endl << flush;
@ -193,36 +172,36 @@ void Tasks::Init() {
/* Tasks creation */
/**************************************************************************************/
if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create server: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create sendtoMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create receiveFromMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create openComRobot: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create startRobotWithoutWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create startRobotWithWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create move: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_detectLostSupRob, "th_detectLostSupRob", 0, PRIORITY_DETECTLOSTSUPROB, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
cerr << "Error task create detectLostSupRob: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
@ -243,41 +222,43 @@ void Tasks::Init() {
* @brief Démarrage des tâches
*/
void Tasks::Run() {
cout << "Coucou" << endl << flush;
rt_task_set_priority(NULL, T_LOPRIO);
int err;
if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start server: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start sendToMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start receiveFromMon: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start openComRobot: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start startRobotWithoutWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start startRobotWithWatchdog: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start move: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
cerr << "Error task start detectLostSupRob: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
@ -298,6 +279,9 @@ void Tasks::Stop() {
void Tasks::Join() {
cout << "Tasks synchronized" << endl << flush;
rt_sem_broadcast(&sem_barrier);
//Initialization for some specific sem: Allow to run the first time
rt_sem_broadcast(&sem_restart);
pause();
}
@ -311,21 +295,27 @@ void Tasks::ServerTask(void *arg) {
// Synchronization barrier (waiting that all tasks are started)
rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/
/* The task server starts here */
/**************************************************************************************/
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
status = monitor.Open(SERVER_PORT);
rt_mutex_release(&mutex_monitor);
while(1){
//First time it's okay after it's managed by receiveFromMon
//rt_sem_p(&sem_restart, TM_INFINITE);
cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
/**************************************************************************************/
/* The task server starts here */
/**************************************************************************************/
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
status = monitor.Open(SERVER_PORT);
rt_mutex_release(&mutex_monitor);
if (status < 0) throw std::runtime_error {
"Unable to start server on port " + std::to_string(SERVER_PORT)
};
monitor.AcceptClient(); // Wait the monitor client
cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
rt_sem_broadcast(&sem_serverOk);
cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
if (status < 0) throw std::runtime_error {
"Unable to start server on port " + std::to_string(SERVER_PORT)
};
monitor.AcceptClient(); // Wait the monitor client
cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
rt_sem_broadcast(&sem_serverOk);
}
}
/**
@ -339,34 +329,33 @@ void Tasks::SendToMonTask(void* arg) {
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_allowSendToMon, TM_INFINITE);
//Initialize the loop condition
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
killSendMon=0; // The message is deleted with the Write
rt_mutex_release(&mutex_killSendMon);
while(1){
//rt_sem_p(&sem_restart, TM_INFINITE);
/**************************************************************************************/
/* The task sendToMon starts here */
/**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
while (!kill_sendToMonOk) {
cout << "wait msg to send" << endl << flush;
msg = ReadInQueue(&q_messageToMon);
cout << "Send msg to mon: " << msg->ToString() << endl << flush;
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msg); // The message is deleted with the Write
rt_mutex_release(&mutex_monitor);
//Initialize the loop condition
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
kill_sendToMonOk=killSendMon; // The message is deleted with the Write
rt_mutex_release(&mutex_killSendMon);
killSendMon=0; // The message is deleted with the Write
rt_mutex_release(&mutex_killSendMon);
/**************************************************************************************/
/* The task sendToMon starts here */
/**************************************************************************************/
rt_sem_p(&sem_serverOk, TM_INFINITE);
while (!kill_sendToMonOk) {
cout << "wait msg to send" << endl << flush;
msg = ReadInQueue(&q_messageToMon);
cout << "Send msg to mon: " << msg->ToString() << endl << flush;
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msg); // The message is deleted with the Write
rt_mutex_release(&mutex_monitor);
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
kill_sendToMonOk=killSendMon; // The message is deleted with the Write
rt_mutex_release(&mutex_killSendMon);
}
}
cout << "Task SendToMon dies" << endl << flush;
rt_sem_v(&sem_allowSendToMon);
}
/**
@ -380,139 +369,151 @@ void Tasks::ReceiveFromMonTask(void *arg) {
// 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_allowStartReceive, TM_INFINITE);
while(1){
//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 (!killReceiveFromMonOk) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
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_killBattery, TM_INFINITE);
killBattery=1;
rt_mutex_release(&mutex_killBattery);
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);
//Restart all the process
//Tasks::Run();
//exit(-1);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
cout << "Command Open Camera Received" << endl << flush;
//start task Vision
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
cout << "Command Open Communication with Robot Received" << endl << flush;
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
cout << "Command Start Robot without Watchdog Received" << endl << flush;
rt_sem_v(&sem_startRobotWithoutWatchdog);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
cout << "Command Start Robot with Watchdog Received" << endl << flush;
//start task robot with watchdog
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
cout << "Command Search Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=1;
rt_mutex_release(&mutex_searchArena);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
cout << "Command Get Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=1;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
cout << "Command Stop Getting Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
cout << "Command Confirm Arena Received" << endl << flush;
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)) {
cout << "Command Infirm Arena Received" << endl << flush;
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) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
}
delete(msgRcv); // must be deleted manually, no consumer
//Update loop condition
//rt_sem_p(&sem_restart,TM_INFINITE);
//Reinitialize control boolean
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMonOk=killReceiveFromMon;
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 (!killReceiveFromMonOk) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
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_killBattery, TM_INFINITE);
killBattery=1;
rt_mutex_release(&mutex_killBattery);
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);
//Restart all the process
Tasks::Join();
rt_sem_v(&sem_restart);
//exit(-1);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
cout << "Command Open Camera Received" << endl << flush;
//start task Vision
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
cout << "Command Close Camera Received" << endl << flush;
//Trigger killVision
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=1;
rt_mutex_release(&mutex_killVision);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
cout << "Command Open Communication with Robot Received" << endl << flush;
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
cout << "Command Start Robot without Watchdog Received" << endl << flush;
rt_sem_v(&sem_startRobotWithoutWatchdog);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
cout << "Command Start Robot with Watchdog Received" << endl << flush;
//start task robot with watchdog
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
cout << "Command Search Arena Received" << endl << flush;
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=1;
rt_mutex_release(&mutex_searchArena);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
cout << "Command Get Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=1;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
cout << "Command Stop Getting Robot Position Received" << endl << flush;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
cout << "Command Confirm Arena Received" << endl << flush;
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)) {
cout << "Command Infirm Arena Received" << endl << flush;
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) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
}
delete(msgRcv); // must be deleted manually, no consumer
//Update loop condition
rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
killReceiveFromMonOk=killReceiveFromMon;
rt_mutex_release(&mutex_killReceiveFromMon);
}
}
cout << "Task ReceiveFromMon dies" << __PRETTY_FUNCTION__ << endl << flush;
rt_sem_v(&sem_allowStartReceive);
}
/**
@ -526,47 +527,32 @@ 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);
/*rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE);
rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
killOpenComRobot = 0;
rt_mutex_release(&mutex_killOpenComRobot);*/
/**************************************************************************************/
/* The task openComRobot starts here */
/**************************************************************************************/
//while (!killOpenComRobot) {
while (1) {
rt_sem_p(&sem_openComRobot, TM_INFINITE);
cout << "Open serial com (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
status = robot.Open();
rt_mutex_release(&mutex_robot);
cout << status;
cout << ")" << endl << flush;
Message * msgSend;
if (status < 0) {
cout << "J'ai raté la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_NACK);
} else {
cout << "J'ai réussi la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_ACK);
/**************************************************************************************/
/* The task openComRobot starts here */
/**************************************************************************************/
while (1) {
rt_sem_p(&sem_openComRobot, TM_INFINITE);
cout << "Open serial com (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
status = robot.Open();
rt_mutex_release(&mutex_robot);
cout << status;
cout << ")" << endl << flush;
Message * msgSend;
if (status < 0) {
msgSend = new Message(MESSAGE_ANSWER_NACK);
} else {
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
//Trigger Detection of Communication Loss with Robot
rt_sem_v(&sem_detectLostSupRob);
}
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
//Trigger Detection of Communication Loss with Robot
rt_sem_v(&sem_detectLostSupRob);
/*rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
killOpenComRobotOk = killOpenComRobot;
rt_mutex_release(&mutex_killOpenComRobot);*/
}
rt_sem_v(&sem_allowOpenComRobot);
}
/**
@ -619,9 +605,6 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
rt_mutex_release(&mutex_killBattery);
}
}
//Trigger New Opening of the Communication with the Robot
//rt_sem_v(&sem_openComRobot);
}
@ -643,44 +626,50 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
Message * msgSend;
int cpt=1;
int err;
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_killBattery, TM_INFINITE);
killBattery=0;
rt_mutex_release(&mutex_killBattery);
while(1){
cout << msgSend->GetID();
cout << ")" << endl;
rt_sem_p(&sem_restart, TM_INFINITE);
cout << "Movement answer: " << msgSend->ToString() << endl << flush;
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
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);
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1;
rt_mutex_release(&mutex_robotStarted);
rt_task_set_periodic(NULL, TM_NOW, 500000000);
while (1) {
cpt++;
if(cpt%2==0){
//boolean to ask battery
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=0;
rt_mutex_release(&mutex_killBattery);
cout << msgSend->GetID();
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);
rt_task_set_periodic(NULL, TM_NOW, 500000000);
while (1) {
cpt++;
if(cpt%2==0){
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(robot.ReloadWD());
rt_mutex_release(&mutex_robot);
}
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(robot.ReloadWD());
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;
}
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;
}
}
}
@ -731,9 +720,6 @@ void Tasks::DetectLostSupRob(void *arg){
bool kill_detectLostSupRobOk=0;
Message* msgSend;
//garanties twin task is dead if applicable
rt_sem_p(&sem_allowStartDetectLostSupRob, TM_INFINITE);
//Wait the Communication with the Robot to be Set
rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
@ -780,8 +766,6 @@ void Tasks::DetectLostSupRob(void *arg){
kill_detectLostSupRobOk=killDetectLostSupRob;
rt_mutex_release(&mutex_killDetectLostSupRob);
}
rt_sem_v(&sem_allowStartDetectLostSupRob);
}
/**

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

@ -119,12 +119,8 @@ private:
RT_SEM sem_serverOk;
RT_SEM sem_startRobotWithoutWatchdog;
RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_allowStartReceive;
RT_SEM sem_allowStartDetectLostSupRob;
RT_SEM sem_detectLostSupRob;
RT_SEM sem_restart;
RT_SEM sem_allowSendToMon;
RT_SEM sem_allowOpenComRobot;
/**********************************************************************/
/* Message queues */