pull master start with WD
This commit is contained in:
commit
b2f42f72f6
2 changed files with 207 additions and 74 deletions
|
@ -28,7 +28,7 @@
|
|||
#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
|
||||
#define PRIORITY_TCAMERA 21
|
||||
#define PRIORITY_DETECTLOSTSUPROB 21
|
||||
|
||||
#define PRIORITY_TVISION 26
|
||||
|
||||
/*
|
||||
* Some remarks:
|
||||
|
@ -80,27 +80,26 @@ void Tasks::Init() {
|
|||
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
|
||||
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)) {
|
||||
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);
|
||||
|
@ -114,22 +113,14 @@ void Tasks::Init() {
|
|||
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_killOpenComRobot, 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);
|
||||
}*/
|
||||
|
||||
|
||||
|
||||
|
||||
cout << "Mutexes created successfully" << endl << flush;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
@ -160,6 +151,11 @@ void Tasks::Init() {
|
|||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
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_restart, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
|
@ -204,7 +200,14 @@ void Tasks::Init() {
|
|||
cerr << "Error task create detectLostSupRob: " << strerror(-err) << endl << flush;
|
||||
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;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
@ -262,8 +265,15 @@ void Tasks::Run() {
|
|||
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;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -373,7 +383,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
|
||||
while(1){
|
||||
//rt_sem_p(&sem_restart,TM_INFINITE);
|
||||
|
||||
//Reinitialize control boolean
|
||||
killReceiveFromMonOk = 0;
|
||||
|
@ -457,8 +466,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
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);
|
||||
|
@ -544,6 +551,9 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Thread opening communication with the robot.
|
||||
*/
|
||||
|
@ -582,29 +592,31 @@ void Tasks::OpenComRobot(void *arg) { //PAS DE SOUCIS AU REDEMARAGE
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Thread starting the communication with the robot.
|
||||
*//**
|
||||
* @brief Thread starting the communication with the robot.
|
||||
*/
|
||||
|
||||
void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
int killBattery=0;
|
||||
int killBatteryOk=0;
|
||||
Message * msgSend;
|
||||
/**************************************************************************************/
|
||||
/* 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
|
||||
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
|
||||
killBattery=0;
|
||||
killBatteryOk=0;
|
||||
rt_mutex_release(&mutex_killBattery);
|
||||
rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
|
||||
cout << "Start robot without watchdog (";
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
msgSend = robot.Write(robot.StartWithoutWD());
|
||||
rt_mutex_release(&mutex_robot);
|
||||
|
@ -618,17 +630,11 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
|
|||
robotStarted = 1;
|
||||
rt_mutex_release(&mutex_robotStarted);
|
||||
rt_task_set_periodic(NULL, TM_NOW, 500000000);
|
||||
while (killBattery==0) {
|
||||
while (killBatteryOk==0) {
|
||||
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;
|
||||
rt_sem_v(&sem_askBattery);
|
||||
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
|
||||
killBattery=killBattery;
|
||||
killBatteryOk=killBattery;
|
||||
rt_mutex_release(&mutex_killBattery);
|
||||
}
|
||||
}
|
||||
|
@ -640,60 +646,54 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
|
|||
*//**
|
||||
* @brief Thread starting the communication with the robot.
|
||||
*/
|
||||
|
||||
void Tasks::StartRobotTaskWithWatchdog(void *arg) {
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
|
||||
int killBatteryOk=0;
|
||||
int cpt=1;
|
||||
Message * msgSend;
|
||||
/**************************************************************************************/
|
||||
/* The task startRobot starts here */
|
||||
/**************************************************************************************/
|
||||
Message* p_mess_answer_battery;
|
||||
Message * msgSend;
|
||||
int cpt=1;
|
||||
int err;
|
||||
|
||||
while(1){
|
||||
|
||||
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
|
||||
//Boolean to get the battery
|
||||
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
|
||||
killBattery=0;
|
||||
rt_mutex_release(&mutex_killBattery);
|
||||
|
||||
cout << msgSend->GetID();
|
||||
cout << ")" << endl;
|
||||
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);
|
||||
|
||||
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) {
|
||||
|
||||
while (killBatteryOk==0) {
|
||||
cpt++;
|
||||
if(cpt%2==0){
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
robot.Write(robot.ReloadWD());
|
||||
rt_mutex_release(&mutex_robot);
|
||||
if(cpt==2){
|
||||
cpt=0;
|
||||
}
|
||||
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;
|
||||
rt_sem_v(&sem_askBattery);
|
||||
|
||||
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
|
||||
killBatteryOk=killBattery;
|
||||
rt_mutex_release(&mutex_killBattery);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -701,7 +701,6 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
|
|||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Thread handling control of the robot.
|
||||
*/
|
||||
|
@ -716,7 +715,7 @@ void Tasks::MoveTask(void *arg) {
|
|||
/**************************************************************************************/
|
||||
/* The task starts here */
|
||||
/**************************************************************************************/
|
||||
rt_task_set_periodic(NULL, TM_NOW, 100000000);
|
||||
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
|
||||
|
||||
while (1) {
|
||||
rt_task_wait_period(NULL);
|
||||
|
@ -739,6 +738,7 @@ void Tasks::MoveTask(void *arg) {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
void Tasks::DetectLostSupRob(void *arg){
|
||||
//counter to detect loss of communication
|
||||
int cpt=0;
|
||||
|
@ -813,7 +813,133 @@ void Tasks::DetectLostSupRob(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);
|
||||
if(p_mess_answer_battery->GetID()==MESSAGE_ROBOT_BATTERY_LEVEL){
|
||||
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);
|
||||
int killVisionOk=0;
|
||||
Camera camera;
|
||||
int acquireImageOk=0;
|
||||
int searchArenaOk=0;
|
||||
int drawArenaOk=0;
|
||||
int getPositionOk=0;
|
||||
Arena arena;
|
||||
//Img* img;
|
||||
list<Position> positionList;
|
||||
list<Position>::iterator it;
|
||||
string strPos;
|
||||
//MessagePosition* msgPos;
|
||||
Message* msgPosMsg;
|
||||
MessageImg msgImg;
|
||||
Message* msgImgMsg;
|
||||
msgImg.SetID(MESSAGE_CAM_IMAGE);
|
||||
//msgPos->SetID(MESSAGE_CAM_POSITION);
|
||||
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
|
||||
killVision=0;
|
||||
rt_mutex_release(&mutex_killVision);
|
||||
rt_task_set_periodic(NULL, TM_NOW, 100000000);
|
||||
camera.Open();
|
||||
while(killVisionOk==0){
|
||||
rt_task_wait_period(NULL);
|
||||
while(acquireImageOk==1){
|
||||
rt_task_wait_period(NULL);
|
||||
cout << "open cam" << endl;
|
||||
Img img=camera.Grab();
|
||||
cout << "after camera grab" << endl << endl << endl;
|
||||
msgImg.SetImage(&img);
|
||||
cout << "after set image in msgImg" << endl << endl << endl;
|
||||
if(searchArenaOk==1){
|
||||
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
|
||||
acquireImage=0;
|
||||
rt_mutex_release(&mutex_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);
|
||||
}
|
||||
if(getPositionOk==1){
|
||||
cout << "in GetPos=1" << endl << endl << endl;
|
||||
//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);
|
||||
MessagePosition msgPos(MESSAGE_CAM_POSITION,*it);
|
||||
//Transformation en message classique
|
||||
msgPosMsg=msgPos.Copy();
|
||||
//Envoi
|
||||
cout << "envoi au mon" << endl;
|
||||
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_acquireImage, TM_INFINITE);
|
||||
acquireImageOk=acquireImage;
|
||||
rt_mutex_release(&mutex_acquireImage);
|
||||
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
|
||||
getPositionOk=getPosition;
|
||||
rt_mutex_release(&mutex_getPosition);
|
||||
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
|
||||
searchArenaOk=searchArena;
|
||||
rt_mutex_release(&mutex_searchArena);
|
||||
rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
|
||||
drawArenaOk=drawArena;
|
||||
rt_mutex_release(&mutex_drawArena);
|
||||
}
|
||||
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
|
||||
searchArena=0;
|
||||
rt_mutex_release(&mutex_searchArena);
|
||||
searchArenaOk=0;
|
||||
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
|
||||
getPosition=0;
|
||||
rt_mutex_release(&mutex_getPosition);
|
||||
getPositionOk=0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Write a message in a given queue
|
||||
* @param queue Queue identifier
|
||||
* @param msg Message to be stored
|
||||
|
|
|
@ -78,7 +78,6 @@ private:
|
|||
bool killOpenComRobot=0;
|
||||
|
||||
|
||||
|
||||
/* Tasks */
|
||||
/**********************************************************************/
|
||||
RT_TASK th_server;
|
||||
|
@ -89,7 +88,8 @@ private:
|
|||
RT_TASK th_startRobotWithWatchdog;
|
||||
RT_TASK th_move;
|
||||
RT_TASK th_detectLostSupRob;
|
||||
|
||||
RT_TASK th_vision;
|
||||
RT_TASK th_askBattery;
|
||||
|
||||
/**********************************************************************/
|
||||
/* Mutex */
|
||||
|
@ -109,8 +109,8 @@ private:
|
|||
RT_MUTEX mutex_killDetectLostSupRob;
|
||||
RT_MUTEX mutex_killStartRob;
|
||||
RT_MUTEX mutex_acquireImage;
|
||||
RT_MUTEX mutex_killOpenComRobot;
|
||||
|
||||
RT_MUTEX mutex_killOpenComRobot;
|
||||
/**********************************************************************/
|
||||
/* Semaphores */
|
||||
/**********************************************************************/
|
||||
|
@ -121,6 +121,7 @@ private:
|
|||
RT_SEM sem_startRobotWithWatchdog;
|
||||
RT_SEM sem_detectLostSupRob;
|
||||
RT_SEM sem_restart;
|
||||
RT_SEM sem_askBattery;
|
||||
|
||||
/**********************************************************************/
|
||||
/* Message queues */
|
||||
|
@ -171,6 +172,12 @@ private:
|
|||
*/
|
||||
void DetectLostSupRob(void *arg);
|
||||
|
||||
|
||||
void AskBattery(void *arg);
|
||||
|
||||
|
||||
|
||||
void Vision(void *arg);
|
||||
/**********************************************************************/
|
||||
/* Queue services */
|
||||
/**********************************************************************/
|
||||
|
|
Loading…
Reference in a new issue