alartigu/Temps_Reel_Robot
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Merge branch 'master' of https://git.etud.insa-toulouse.fr/alartigu/Temps_Reel_Robot

Browse source
This commit is contained in:
Leonie Gallois 2021-03-17 10:05:03 +01:00
commit 4ac28d8427
2 changed files with 110 additions and 37 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -285,32 +285,53 @@ void Tasks::ReceiveFromMonTask(void *arg) {
/**************************************************************************************/
/* The task receiveFromMon starts here */
/**************************************************************************************/
while(1){
rt_sem_p(&sem_serverOk, TM_INFINITE);
cout << "Received message from monitor activated" << endl << flush;
while (1) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
while (1) {
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
delete(msgRcv);
exit(-1);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
//rt_sem_v(&sem_openComRobot);
WriteInQueue(&q_messageComRobot, msgRcv);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_sem_v(&sem_startRobot);
} 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)) {
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
delete(msgRcv);
WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
rt_sem_v(&sem_restartServer, TM_INFINITE); // A VERIFIER
break;
rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
WriteInQueue(&q_messageComRobot, msgRcv);
//rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD) || msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
WriteInQueue(&q_messageControlRobot, msgRcv);
//rt_sem_v(&sem_startRobot);
}
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);
}
else if (msgRcv->CompareID(MESSAGE_CAM_OPEN) ||
msgRcv->CompareID(MESSAGE_CAM_CLOSE) ||
msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA) ||
msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) ||
msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) ||
msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) ||
msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP) ||
)
{
WriteInQueue(&q_messageControlCam, msgRcv);
}
delete(msgRcv); // mus be deleted manually, no consumer
}
delete(msgRcv); // mus be deleted manually, no consumer
}
}
@ -381,23 +402,70 @@ void Tasks::StartRobotTask(void *arg) {
/**************************************************************************************/
while (1) {
Message * tmp;
Message * msgSend;
rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithoutWD());
rt_mutex_release(&mutex_robot);
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) {
tmp = ReadInQueue(&q_messageControlRobot);
if (tmp -> CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)){
cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithoutWD());
rt_mutex_release(&mutex_robot);
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);
}
} else if (tmp -> CompareID(MESSAGE_ROBOT_START_WITH_WD)){
cout << "Start robot with watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithWD());
rt_mutex_release(&mutex_robot);
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(&th_watchDog,TM_NOW,1000000000);
}
} else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
rt_task_set_periodic(&th_watchDog,TM_NOW,0);
cout << "Stopping Robot (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(new Message(MESSAGE_ROBOT_RESET));
rt_mutex_release(&mutex_robot);
cout << msgSend->GetID();
cout << ")" << endl;
cout << "Movement answer: " << msgSend->ToString() << endl << flush;
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1;
robotStarted = 0;
rt_mutex_release(&mutex_robotStarted);
}
}
}
}
void Tasks::WatchDog(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
rt_task_set_periodic(NULL, TM_NOW, 0);
while (1) {
rt_task_wait_period(NULL);
Message * msgSend;
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.ReloadWD();
rt_mutex_release(&mutex_robot);
}
}
@ -434,7 +502,6 @@ void Tasks::MoveTask(void *arg) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(new Message((MessageID)cpMove));
rt_mutex_release(&mutex_robot);
previousMove = cpMove;
}
}

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

@ -99,6 +99,8 @@ private:
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
RT_SEM sem_watchdogStart;
RT_SEM sem_restartServer;
@ -107,7 +109,6 @@ private:
/**********************************************************************/
int MSG_QUEUE_SIZE;
RT_QUEUE q_messageToMon;
RT_QUEUE q_messageComRobot;
RT_QUEUE q_messageControlRobot;
RT_QUEUE q_messageControlCam;
@ -168,7 +169,12 @@ private:
* @return Nothing
*/
void ReadBattery(void *arg);
/**
* Sends periodically a message to the robot
* @return Nothing
*/
void Watchdog(void *arg);
};