void connecter(void *arg) {
DMessage *message = d_new_message();
int status;
rt_printf("tconnect : Debut de l'exécution de tconnect\n");
while (1) {
rt_printf("tconnect : Attente du sémaphore semConnecterRobot\n");
rt_sem_p(&semConnecterRobot, TM_INFINITE);
rt_printf("tconnect : Ouverture de la communication avec le robot\n");
status = robot->open_device(robot);
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommRobot = status;
if (status == STATUS_OK) {
cptCommErr = 0;
//robot->start_insecurely(robot);
status = robot->start(robot);
if (status == STATUS_OK) { /* Demarrage du robot */
rt_printf("tconnect : Robot démarrer\n");
rt_sem_v(&semDeplacer);
rt_sem_v(&semRechargerWatchdog);
rt_sem_v(&semVerifierBatterie);
} else { /* Impossible de demarrer le robot, tentative de reinitialisation */
robot->stop(robot);
robot->close_com(robot);
}
}
rt_mutex_release(&mutexEtat);
message->put_state(message, status);
serveur->send(serveur, message);
}
}
void comm_err_handler(int status) {
DMessage *message = d_new_message();
rt_printf("Trop d'erreurs de communication avec le robot => connexion perdue\n");
robot->stop(robot);
robot->close_com(robot);
message->put_state(message, status);
serveur->send(serveur, message);
}
void Verif_Comm (int status) {
int compteur;
DMessage *message;
if (status != STATUS_OK) {
rt_mutex_acquire(&mutexCpt, TM_INFINITE);
cpt_perte_com ++;
compteur = cpt_perte_com;
rt_mutex_release(&mutexCpt);
if (compteur >= 6) {
// La communication est perdue
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommRobot = status;
rt_mutex_release(&mutexEtat);
message = d_new_message();
message->put_state(message, status);
rt_printf("tmove : Envoi message\n");
if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
message->free(message);
}
//rt_sem_v(&semConnectedRobot);
// On réinitialise le compteur
rt_mutex_acquire(&mutexCpt, TM_INFINITE);
cpt_perte_com = 0 ;
rt_mutex_release(&mutexCpt);
compteur = 0 ;
// On reset le robot
//rt_mutex_acquire(&mutexRobot, TM_INFINITE);
robot->close_com(robot); // a verifier
// rt_mutex_release(&mutexRobot);
}
}
}
void connecter(void * arg) {
int status;
DMessage *message;
rt_printf("tconnect : Debut de l'exécution de tconnect\n");
while (1) {
rt_printf("tconnect : Attente du sémarphore semConnecterRobot\n");
rt_sem_p(&semConnecterRobot, TM_INFINITE);
rt_printf("tconnect : Ouverture de la communication avec le robot\n");
status = robot->open_device(robot);
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommRobot = status;
rt_mutex_release(&mutexEtat);
if (status == STATUS_OK) {
status = robot->start_insecurely(robot);
if (status == STATUS_OK){
rt_printf("tconnect : Robot démarrer\n");
rt_sem_v(&semConnectedRobot);
rt_sem_v(&semConnectedRobot);
}
}
message = d_new_message();
message->put_state(message, status);
rt_printf("tconnecter : Envoi message\n");
message->print(message, 100);
if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
message->free(message);
}
}
}
