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

ajout de la vision (attention elle est pas finie) et watchdog ok

Browse source
This commit is contained in:
Romain Vitrat 2020-03-29 04:50:50 +02:00
commit 481213ac20
3 changed files with 175 additions and 68 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -27,6 +27,7 @@
#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22 #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22
#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
#define PRIORITY_TCAMERA 21 #define PRIORITY_TCAMERA 21
#define PRIORITY_TVISION 26
/* /*
@ -79,27 +80,26 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_mutex_create(&mutex_killBattery, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_killVision, NULL)) { if (err = rt_mutex_create(&mutex_killVision, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_mutex_create(&mutex_searchArena, NULL)) { if (err = rt_mutex_create(&mutex_searchArena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_mutex_create(&mutex_drawArena, NULL)) { if (err = rt_mutex_create(&mutex_drawArena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_mutex_create(&mutex_getPosition, NULL)) { if (err = rt_mutex_create(&mutex_getPosition, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_mutex_create(&mutex_killSendMon, NULL)) { if (err = rt_mutex_create(&mutex_killSendMon, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -114,16 +114,13 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
/*if (err = rt_mutex_create(&mutex_robot_on, NULL)) { /*if (err = rt_mutex_create(&mutex_robot_on, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
}*/ }*/
cout << "Mutexes created successfully" << endl << flush; cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -149,7 +146,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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);
}
if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) { if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -192,7 +192,14 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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);
}
if (err = rt_task_create(&th_vision, "th_vision", 0, PRIORITY_TVISION, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush; cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -241,7 +248,14 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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);
}
if (err = rt_task_start(&th_vision, (void(*)(void*)) & Tasks::Vision, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush; cout << "Tasks launched" << endl << flush;
} }
@ -387,7 +401,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
cout << "Command Start Robot with Watchdog Received" << endl << flush; cout << "Command Start Robot with Watchdog Received" << endl << flush;
//start task robot with watchdog rt_sem_v(&sem_startRobotWithoutWatchdog);
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) { } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
cout << "Command Search Arena Received" << endl << flush; cout << "Command Search Arena Received" << endl << flush;
@ -430,7 +444,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1; acquireImage=1;
rt_mutex_release(&mutex_acquireImage); rt_mutex_release(&mutex_acquireImage);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@ -453,6 +466,9 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_sem_v(&sem_allowStartReceive); rt_sem_v(&sem_allowStartReceive);
} }
/** /**
* @brief Thread opening communication with the robot. * @brief Thread opening communication with the robot.
*/ */
@ -486,29 +502,31 @@ void Tasks::OpenComRobot(void *arg) {
} }
} }
/** /**
* @brief Thread starting the communication with the robot. * @brief Thread starting the communication with the robot.
*//** *//**
* @brief Thread starting the communication with the robot. * @brief Thread starting the communication with the robot.
*/ */
void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
int killBattery=0; int killBatteryOk=0;
Message * msgSend;
/**************************************************************************************/ /**************************************************************************************/
/* The task startRobot starts here */ /* The task startRobot starts here */
/**************************************************************************************/ /**************************************************************************************/
Message* p_mess_answer_battery;
Message * msgSend;
rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
cout << "Start robot without watchdog (";
//Boolean to get the battery //Boolean to get the battery
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=0; killBatteryOk=0;
rt_mutex_release(&mutex_killBattery); rt_mutex_release(&mutex_killBattery);
rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithoutWD()); msgSend = robot.Write(robot.StartWithoutWD());
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
@ -522,17 +540,11 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
robotStarted = 1; robotStarted = 1;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
rt_task_set_periodic(NULL, TM_NOW, 500000000); rt_task_set_periodic(NULL, TM_NOW, 500000000);
while (killBattery==0) { while (killBatteryOk==0) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_sem_v(&sem_askBattery);
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_mutex_acquire(&mutex_killBattery, TM_INFINITE); rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=killBattery; killBatteryOk=killBattery;
rt_mutex_release(&mutex_killBattery); rt_mutex_release(&mutex_killBattery);
} }
} }
@ -544,64 +556,52 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
*//** *//**
* @brief Thread starting the communication with the robot. * @brief Thread starting the communication with the robot.
*/ */
void Tasks::StartRobotTaskWithWatchdog(void *arg) { void Tasks::StartRobotTaskWithWatchdog(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
int killBatteryOk=0;
int cpt=1;
Message * msgSend;
/**************************************************************************************/ /**************************************************************************************/
/* The task startRobot starts here */ /* The task startRobot starts here */
/**************************************************************************************/ /**************************************************************************************/
Message* p_mess_answer_battery; //Boolean to get the battery
Message * msgSend; rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
int cpt=1; killBattery=0;
int err; rt_mutex_release(&mutex_killBattery);
rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
cout << "Start robot with watchdog ("; cout << "Start robot with watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithWD()); msgSend = robot.Write(robot.StartWithWD());
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
//boolean to ask battery
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=0;
rt_mutex_release(&mutex_killBattery);
cout << msgSend->GetID(); cout << msgSend->GetID();
cout << ")" << endl; cout << ")" << endl;
cout << "Movement answer: " << msgSend->ToString() << endl << flush; 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) { if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1; robotStarted = 1;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
rt_task_set_periodic(NULL, TM_NOW, 500000000); rt_task_set_periodic(NULL, TM_NOW, 500000000);
while (1) { while (killBatteryOk==0) {
cpt++; cpt++;
if(cpt%2==0){ if(cpt==2){
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(robot.ReloadWD()); robot.Write(robot.ReloadWD());
rt_mutex_release(&mutex_robot); cpt=0;
} }
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_sem_v(&sem_askBattery);
p_mess_answer_battery = robot.Write(robot.GetBattery()); rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
rt_mutex_release(&mutex_robot); killBatteryOk=killBattery;
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); rt_mutex_release(&mutex_killBattery);
monitor.Write(p_mess_answer_battery);
rt_mutex_release(&mutex_monitor);
cout << endl << flush;
} }
} }
} }
/** /**
* @brief Thread handling control of the robot. * @brief Thread handling control of the robot.
*/ */
@ -616,7 +616,7 @@ void Tasks::MoveTask(void *arg) {
/**************************************************************************************/ /**************************************************************************************/
/* The task starts here */ /* The task starts here */
/**************************************************************************************/ /**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 100000000); rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while (1) { while (1) {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
@ -639,8 +639,110 @@ 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;
}
}
/**
//This task works on a difficult principle that fully make it controllable through
// monitor, accessing booleans variables to set getPosition or searchArena
void Tasks::Vision(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_askBattery, TM_INFINITE);
int killVisionOk=0;
Camera camera;
int acquireImageOk=1;
int searchArenaOk=0;
int drawArenaOk=0;
int getPositionOk=0;
Arena arena;
list<Position> positionList;
list<Position>::iterator it;
string strPos;
MessagePosition* msgPos;
Message* msgPosMsg;
MessageImg* msgImg;
Message* msgImgMsg;
msgPos->SetID(MESSAGE_CAM_POSITION);
msgImg->SetID(MESSAGE_CAM_IMAGE);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=0;
rt_mutex_release(&mutex_killVision);
camera.Open();
while(killVisionOk==0){
while(acquireImageOk==1){
Img img=camera.Grab();
if(searchArenaOk==1){
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=0;
rt_mutex_release(&mutex_acquireImage);
acquireImageOk=acquireImage;
arena=img.SearchArena();
img.DrawArena(arena);
}
if(drawArenaOk==1){
// img.DrawArena(arena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1;
rt_mutex_release(&mutex_acquireImage);
acquireImageOk=acquireImage;
}
if(getPositionOk==1){
//On démarre la recherche du robot dans la zone définie par l'arène
positionList=img.SearchRobot(arena);
//Définitition et assignation de l'itérateur de parcrous de la liste positionList
it=positionList.begin();
//Définition d'un messagePosition qui va contenir l'information (x,y)
msgPos->SetPosition(*it);
//Transformation en message classique
msgPosMsg=msgPos->Copy();
//Envoi
WriteInQueue(&q_messageToMon,msgPos);
//Dessis du robot sur l'image
img.DrawRobot(*it);
}
//Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas)
msgImg->SetImage(&img);
//Transformation en message classique
msgImgMsg=msgImg->Copy();
//Envoi
WriteInQueue(&q_messageToMon,msgImg);
}
}
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0;
rt_mutex_release(&mutex_searchArena);
searchArenaOk=1;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0;
rt_mutex_release(&mutex_getPosition);
getPositionOk=1;
}
/*
* Write a message in a given queue * Write a message in a given queue
* @param queue Queue identifier * @param queue Queue identifier
* @param msg Message to be stored * @param msg Message to be stored

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

@ -77,7 +77,6 @@ private:
bool drawArena=0; bool drawArena=0;
/* Tasks */ /* Tasks */
/**********************************************************************/ /**********************************************************************/
RT_TASK th_server; RT_TASK th_server;
@ -87,6 +86,8 @@ private:
RT_TASK th_startRobotWithoutWatchdog; RT_TASK th_startRobotWithoutWatchdog;
RT_TASK th_startRobotWithWatchdog; RT_TASK th_startRobotWithWatchdog;
RT_TASK th_move; RT_TASK th_move;
RT_TASK th_vision;
RT_TASK th_askBattery;
/**********************************************************************/ /**********************************************************************/
/* Mutex */ /* Mutex */
@ -106,7 +107,6 @@ private:
RT_MUTEX mutex_killDetectLostSupRob; RT_MUTEX mutex_killDetectLostSupRob;
RT_MUTEX mutex_killStartRob; RT_MUTEX mutex_killStartRob;
RT_MUTEX mutex_acquireImage; RT_MUTEX mutex_acquireImage;
/**********************************************************************/ /**********************************************************************/
/* Semaphores */ /* Semaphores */
/**********************************************************************/ /**********************************************************************/
@ -116,6 +116,7 @@ private:
RT_SEM sem_startRobotWithoutWatchdog; RT_SEM sem_startRobotWithoutWatchdog;
RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_allowStartReceive; RT_SEM sem_allowStartReceive;
RT_SEM sem_askBattery;
/**********************************************************************/ /**********************************************************************/
/* Message queues */ /* Message queues */
@ -161,7 +162,13 @@ private:
*/ */
void MoveTask(void *arg); void MoveTask(void *arg);
void AskBattery(void *arg);
void Vision(void *arg);
/**********************************************************************/ /**********************************************************************/
/* Queue services */ /* Queue services */

4
software/simulateur/nbproject/private/private.xml
View file

@ -6,8 +6,6 @@
</data> </data>
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/> <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2"> <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group> <group/>
<file>file:/home/etud/dumber/software/simulateur/main.cpp</file>
</group>
</open-files> </open-files>
</project-private> </project-private>