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 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 mission(void *arg){ float x_dest, y_dest, x_robot, y_robot, angle_robot; int turnAngle; float x, y; int busy; int status, statuscom; DMessage *to_send = d_new_message(); float dist; int range = MISSION_ACCEPTABLE_RANGE; float angle; int sens; while (1) { rt_printf("tmission : Attente du sémaphore semMissionLancee\n"); rt_sem_p(&semMissionLancee, TM_INFINITE); /*le sémaphore est libèré lorsque l'on reçoit un message de type mission_start*/ rt_printf("tmission : Debut de l'exécution de tmission\n"); rt_mutex_acquire(&mutexPosition, TM_INFINITE); x_robot = position_robot->get_x(position_robot); y_robot = position_robot->get_y(position_robot); angle_robot = position_robot->get_orientation(position_robot); rt_mutex_release(&mutexPosition); x_dest = position_dest->get_x(position_dest); y_dest = position_dest->get_y(position_dest); x = x_dest - x_robot; y = y_dest - y_robot; dist = sqrt((double)(x*x + y*y)); angle = (int)atan2(fabs((double)y), fabs((double)x)) * 180.0 / M_PI; rt_printf("angle de base calculé = %f\n", angle); rt_printf("distance a parcourir = %f\n", dist); rt_printf("xdest, ydest = [%d,%d]\n", (int)x_dest, (int)y_dest); /*calcul de l'angle et du sens de rotation*/ if (x_robot <= x_dest) { /*on est soit dans beta1 ou beta3*/ if (y_robot <= y_dest) { /*on est dans beta4*/ angle = angle; sens = 1; rt_printf("beta4\n"); } else { /*on est dans beta2*/ angle = angle; sens = 0; rt_printf("beta2\n"); } } else { /*on est soit dans beta2 ou beta4*/ if (y_robot <= y_dest) { /*on est dans beta3*/ angle = 180 - angle; sens = 0; rt_printf("beta3\n"); } else { /*on est dans beta1*/ angle = 180 - angle; sens = 1; rt_printf("beta1\n"); } } rt_printf("------------------------\n"); rt_printf("On doit tourner de %d°\n", (int)fabs((double)angle)); rt_printf("------------------------\n"); /* turnAngle = (int)(angle_robot * 180.0 / M_PI); while (abs(turnAngle) > 3) { rt_printf("angle_robot = %d\n", turnAngle); // statuscom = etatCommRobot; // while(statuscom != STATUS_OK){ //on remet le robot dans sa position initiale (angle 0°) //while ((robot->turn(robot, (int)(angle_robot * 180.0 / M_PI), ANTI_HORAIRE)) != STATUS_OK); robot->turn(robot, turnAngle, ANTI_HORAIRE); //statuscom = etatCommRobot; //} /*on attend que le robot ait fini de tourné* do { if ((robot->is_busy(robot, &busy)) != STATUS_OK) busy = 1; rt_printf("On tourne -> 0\n"); rt_task_sleep(100000000); } while (busy); rt_mutex_acquire(&mutexPosition, TM_INFINITE); rt_printf("arrive a %d\n", turnAngle = (int)(position_robot->get_orientation(position_robot) * 180.0 / M_PI)); rt_mutex_release(&mutexPosition); } //on tourne selon l'angle et le sens déterminé while (robot->turn(robot, (int)fabs((double)angle), sens) != STATUS_OK); do{ rt_printf("On tourne -> %d\n", (int)fabs((double)angle)); while((robot->is_busy(robot, &busy)) != STATUS_OK); } while (busy); rt_printf("On a atteind %d\n", angle); rt_mutex_acquire(&mutexPosition, TM_INFINITE); angle_robot = position_robot->get_orientation(position_robot); rt_mutex_release(&mutexPosition); rt_printf("angle_robot apres rotation = %d\n", (int)( angle_robot * 180.0 / M_PI)); // rt_task_sleep(100000000000); // rt_mutex_release(&mutexMove); while(robot->move(robot, (int)dist) != STATUS_OK); do{ robot->is_busy(robot, &busy); } while (busy); rt_printf("Mission : Mission terminee\n"); to_send->mission_terminate(to_send, mission_s->get_id(mission_s)); serveur->send(serveur, to_send);*/ } }
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); } }