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 batterie(void *arg) {
rt_printf("tconnect : Attente du sémarphore semConnectedRobotbatterie\n");
rt_sem_p(&semConnectedRobot, TM_INFINITE);
int status=1;
int niveau_batterie=0;
DBattery *bat= d_new_battery();
DMessage *message;
rt_printf("tbatterie : Debut de l'éxecution de periodique à 250ms\n");
rt_task_set_periodic(NULL, TM_NOW, 250000000);
while (1) {
rt_task_wait_period(NULL);
rt_printf("tbatterie : Activation périodique\n");
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
status = etatCommMoniteur;
rt_mutex_release(&mutexEtat);
if (status == STATUS_OK) {
rt_mutex_acquire(&mutexRobot, TM_INFINITE);
status=d_robot_get_vbat(robot, &niveau_batterie);
rt_mutex_release(&mutexRobot);
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommRobot=status;
rt_mutex_release(&mutexEtat);
rt_printf("Niveau de la batterie : %d\n", niveau_batterie);
rt_printf("Status : %d\n", status);
if(status == STATUS_OK) {
message=d_new_message();
d_battery_set_level(bat,niveau_batterie);
d_message_put_battery_level(message, bat);
rt_mutex_acquire(&mutexCom, TM_INFINITE);
if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
message->free(message);
}
rt_mutex_release(&mutexCom);
}
}
}
}
void envoyer(void * arg) {
DMessage *msg;
int err;
while (1) {
rt_printf("tenvoyer : Attente d'un message\n");
if ((err = rt_queue_read(&queueMsgGUI, &msg, sizeof (DMessage), TM_INFINITE)) >= 0) {
rt_printf("tenvoyer : envoi d'un message au moniteur\n");
serveur->send(serveur, msg);
msg->free(msg);
} else {
rt_printf("Error msg queue write: %s\n", strerror(-err));
}
}
}
void communiquer(void *arg) {
DMessage *msg = d_new_message();
int var1 = 1;
int num_msg = 0;
rt_printf("tserver : Début de l'exécution de serveur\n");
serveur->open(serveur, "8000");
rt_printf("tserver : Connexion\n");
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommMoniteur = 0;
rt_mutex_release(&mutexEtat);
while (var1 > 0) {
rt_printf("tserver : Attente d'un message\n");
var1 = serveur->receive(serveur, msg);
num_msg++;
if (var1 > 0) {
switch (msg->get_type(msg)) {
case MESSAGE_TYPE_ACTION:
rt_printf("tserver : Le message %d reçu est une action\n",
num_msg);
DAction *action = d_new_action();
action->from_message(action, msg);
switch (action->get_order(action)) {
case ACTION_CONNECT_ROBOT:
rt_printf("tserver : Action connecter robot\n");
rt_sem_v(&semConnecterRobot);
break;
}
break;
case MESSAGE_TYPE_MOVEMENT:
rt_printf("tserver : Le message reçu %d est un mouvement\n",
num_msg);
rt_mutex_acquire(&mutexMove, TM_INFINITE);
move->from_message(move, msg);
move->print(move);
rt_mutex_release(&mutexMove);
break;
}
}
}
}
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);
}
}
}
void communiquer(void *arg) {
DMessage *msg = d_new_message();
DMessage *to_send = d_new_message();
DAction *action = d_new_action();
int var1 = 1;
int num_msg = 0;
while (1) {
rt_printf("tserver : Début de l'exécution de serveur\n");
serveur->open(serveur, "8000");
rt_printf("tserver : Connexion\n");
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommMoniteur = 0;
rt_mutex_release(&mutexEtat);
while (var1 > 0) {
//rt_printf("tserver : Attente d'un message\n");
var1 = serveur->receive(serveur, msg);
num_msg++;
if (var1 > 0) {
switch (msg->get_type(msg)) {
case MESSAGE_TYPE_ACTION:
rt_printf("tserver : Le message %d reçu est une action\n", num_msg);
action->from_message(action, msg);
switch (action->get_order(action)) {
case ACTION_CONNECT_ROBOT:
rt_printf("tserver : Action connecter robot\n");
rt_sem_v(&semConnecterRobot);
break;
case ACTION_FIND_ARENA:
rt_printf("tserver : Demande de detection de l'arene\n");
rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
if (etat_camera == 2) etat_camera = 3;
else etat_camera = 1;
rt_mutex_release(&mutexEtatCamera);
break;
case ACTION_COMPUTE_CONTINUOUSLY_POSITION:
rt_printf("tserver : Demande de detection du robot\n");
rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
if (etat_camera == 1) etat_camera = 3;
else etat_camera = 2;
rt_mutex_release(&mutexEtatCamera);
break;
case ACTION_STOP_COMPUTE_POSITION:
rt_printf("tserver : Fin de detection\n");
rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
etat_camera = 0;
rt_mutex_release(&mutexEtatCamera);
break;
}
break;
case MESSAGE_TYPE_MOVEMENT:
rt_printf("tserver : Le message reçu %d est un mouvement\n", num_msg);
rt_mutex_acquire(&mutexMove, TM_INFINITE);
move->from_message(move, msg);
//move->print(move);
rt_mutex_release(&mutexMove);
break;
case MESSAGE_TYPE_MISSION:
rt_printf("tserver : Nouvelle mission\n", num_msg);
mission_s->from_message(mission_s, msg);
mission_s->print(mission_s);
mission_s->get_position(mission_s, position_dest);
position_dest->print(position_dest);
mission_s->print(mission_s);
rt_sem_v(&semMissionLancee);
break;
case MISSION_TYPE_STOP:
to_send->mission_terminate(to_send, mission_s->get_id(mission_s));
serveur->send(serveur, to_send);
//rt_sem_p(&semMissionLancee, TM_INFINITE);
break;
}
} else {
rt_printf("tserver : Perte de communication avec le moniteur\n");
serveur->close(serveur);
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommMoniteur = 1;
rt_mutex_release(&mutexEtat);
}
}
}
}
void envoyer_image(void *arg) {
DMessage *message = d_new_message();
DMessage *message_position = d_new_message();
DMessage *message_arena = d_new_message();
DImage *image = d_new_image();
DJpegimage *jpegimage = d_new_jpegimage();
char arena_found[] = "arena found";
rt_printf("tstreaming : Envoi de l'image à ~2.5img/s\n");
rt_task_set_periodic(NULL, TM_NOW, 400000000);
while (1) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
camera->get_frame(camera, image);
switch (etat_camera) {
case 0 : /* streaming simple */
rt_mutex_release(&mutexEtatCamera);
break;
case 1 : /* streaming avec arena */
rt_mutex_release(&mutexEtatCamera);
/* détection de l'arène */
arena = (DArena *) image->compute_arena_position(image);
/*check si l'arene a été trouvée*/
if (arena != NULL) {
x_arena = arena->get_x(arena);
y_arena = arena->get_y(arena);
//rt_printf("x = %f; y = %f\n ", x_arena, y_arena);
/*envoie du message confirmant que l'arène a été trouvée*/
message_arena->set(message_arena, ACTION_ARENA_IS_FOUND, strlen(arena_found), arena_found);
serveur->send(serveur, message_arena);
d_imageshop_draw_arena(image, arena);
}
break;
case 2 : /* streaming avec position du robot */
rt_mutex_release(&mutexEtatCamera);
/* détection de la position du robot */
arena = (DArena *) image->compute_arena_position(image);
rt_mutex_acquire(&mutexPosition, TM_INFINITE);
position_robot = (DPosition *) image->compute_robot_position(image, arena);
if (position_robot != NULL) {
x_robot = position_robot->get_x(position_robot);
y_robot = position_robot->get_y(position_robot);
orientation_robot = position_robot->get_orientation(position_robot);
rt_mutex_release(&mutexPosition);
/* rt_printf("\n\n\nx = %f; y = %f, angle = %f\n\n\n ", x_robot, y_robot, orientation_robot); */
/*envoie de la position par message*/
message_position->put_position(message_position, position_robot);
serveur->send(serveur, message_position);
d_imageshop_draw_position(image, position_robot);
}
break;
case 3 : /* streaming avec position de l'arene et du robot */
rt_mutex_release(&mutexEtatCamera);
/* détection de la position du robot et de l'arene*/
arena = (DArena *) image->compute_arena_position(image);
rt_mutex_acquire(&mutexPosition, TM_INFINITE);
position_robot = (DPosition *) image->compute_robot_position(image, arena);
if (position_robot != NULL) {
x_robot = position_robot->get_x(position_robot);
y_robot = position_robot->get_y(position_robot);
/* rt_printf("\n\n\nx = %f; y = %f\n\n\n ", x_robot, y_robot); */
rt_mutex_release(&mutexPosition);
/*envoie de la position par message*/
message_position->put_position(message_position, position_robot);
serveur->send(serveur, message_position);
d_imageshop_draw_position(image, position_robot);
}
/*check si l'arene a été trouvée*/
if (arena != NULL) {
x_arena = arena->get_x(arena);
y_arena = arena->get_y(arena);
/* rt_printf("x = %f; y = %f\n ", x_arena, y_arena); */
/*envoie du message confirmant que l'arène a été trouvée*/
message_arena->set(message_arena, ACTION_ARENA_IS_FOUND, strlen(arena_found), arena_found);
serveur->send(serveur, message_arena);
d_imageshop_draw_arena(image, arena);
}
break;
default :
rt_printf("valeur de etat_camera corrompue\n");
}
/*compression et envoie de l'image*/
jpegimage->compress(jpegimage, image);
//jpegimage->print(jpegimage);
message->put_jpeg_image(message, jpegimage);
serveur->send(serveur, message);
}
}