void verifier_batterie(void *arg) {
DBattery *batterie = d_new_battery();
int status;
int vbat;
rt_printf("tbattery : Attente du sémaphore semVerifierBatterie\n");
rt_sem_p(&semVerifierBatterie, TM_INFINITE);
rt_printf("tbattery : Debut de l'execution periodique à 1s\n");
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while (1) {
while (cptCommErr < MAX_ECHECS) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_printf("tbattery : Activation périodique\n");
status = robot->get_vbat(robot, &vbat);
if (status == STATUS_OK) {
cptCommErr = 0;
if (vbat == BATTERY_OFF || vbat == BATTERY_LOW || vbat == BATTERY_OK)
batterie->set_level(batterie, vbat);
} else {
cptCommErr++;
rt_printf("tbattery : Erreur de communication avec le robot (%d)\n", cptCommErr);
}
}
comm_err_handler(status);
rt_sem_p(&semVerifierBatterie, TM_INFINITE);
}
}
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 initStruct(void) {
int err;
/* Creation des mutex */
if (err = rt_mutex_create(&mutexEtat, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexMove, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexRobot, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexArena, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexPosition, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexImage, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexMission, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation du semaphore */
if (err = rt_sem_create(&semConnecterRobot, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semGetImage, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semDetectArena, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
}
if (err = rt_sem_create(&semComputePosition, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
}
if (err = rt_sem_create(&semwatchDog, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
}
if (err = rt_sem_create(&semCommunicate, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
}
if (err = rt_sem_create(&semConnect, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
}
/* Creation des taches */
if (err = rt_task_create(&tServeur, NULL, 0, PRIORITY_TSERVEUR, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tconnect, NULL, 0, PRIORITY_TCONNECT, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tmove, NULL, 0, PRIORITY_TMOVE, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tenvoyer, NULL, 0, PRIORITY_TENVOYER, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tbattery, NULL, 0, PRIORITY_TBATTERY, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tcamera, NULL, 0, PRIORITY_TCAMERA, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tverify, NULL, 0, PRIORITY_TVERIFY, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tcomputepos, NULL, 0, PRIORITY_TCOMPUTEPOS, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tarena, NULL, 0, PRIORITY_TARENA, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation des files de messages */
if (err = rt_queue_create(&queueMsgGUI, "toto", MSG_QUEUE_SIZE * sizeof (DMessage), MSG_QUEUE_SIZE, Q_FIFO)) {
rt_printf("Error msg queue create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation des structures globales du projet */
robot = d_new_robot();
move = d_new_movement();
serveur = d_new_server();
camera = d_new_camera();
battery = d_new_battery();
mission = NULL;
position = NULL;
arena = NULL;
image = NULL;
}
