Projet

Général

Profil

Véhicule autonome » pilote_automatique.c

Anonyme, 01/07/2013 20:53

 

#include <stdio.h>
#include <conio.h>
#include <math.h>
#include <windows.h>
#include "remote_socket_client.h"
#include "gestion.h"
#define N 2700
//link with ws2_32


/** INFORMATIONS SUR LES PERIPHERIQUES ET COMMANDES
---------------------------------------------------
'V' : Vitesse du moteur (0-50)
'D' : Angle de braquage des roues avant en 1/10° de degré (10 -> 1∞)
'T' : Azimut de la tourelle du télémètre en 1/10°
'R' : Lecture de l'Azimuth de la tourelle en 1/10°
'U' : Distance mesure par le télémètre en 1/100 de mètre (en cm)
'X' : Position absolue X en cm
'Y' : Position absolue Y en cm
'Z' : Position absolue Z en cm
'N' : Numéro de la voiture (en fonction de l'ordre de connection)
'E' : Lecture des evènements (cf Annexe 2)
'H' : Donne le temps de course actuel
'S' : Temps du tour précédent
'I' : Defini le nom du vehicule lettre par lettre
'M' : Mode de course :
8 bits de poids fort: 1 Attente, 2 course, 3 essais libres)
8 bits de poids faible : numero de piste
'C' : Informations sur le dernier capteur touché :
8 bits de poids faible : numéro du capteur
8 bits de poids fort : couleur ('R','J' ou 'V')
'K' : Téléportation de la voiture sur le troncon de piste N
(correspondant au capteur vert numero N).
Attention à n'utiliser que pour des tests, les scores sont invalidés !

Bit 0 : Point de passage Vert, remis à zéro lors de la lecture du périphérique 'C'
1 : Point de passage Jaune, remis à zéro lors de la lecture du périphérique 'C'
2 : Point de passage Rouge, remis à zéro lors de la lecture du périphérique 'C'
3-6 : non utilisés
7 : Point de passage course (vert), remis à zéro lors de la lecture du périphérique 'C'
8 : La piste à changé , remis à zéro lors de la lecture du périphérique 'M'
9 : Le mode de course a changé , remis à zéro lors de la lecture du périphérique 'M'
10 : Début de la course , remis à zéro au changement du mode de course.
11 : Le dernier point de passage est atteint la course est finie , remis à zéro au changement du mode de course.
12 : La voiture est sortie de la piste.
13 : Utilisation de la fonction de téléportation. Classement Invalidé. Remis à zero au changement de piste ou du mode de course.
14 : Faux départ -> destruction de la voiture , remise à zéro au changement du mode de course.
15 : Collision avec le sol, Remise à zéro au changement de piste.
**/


int depart;
int fin;

int course_trop_longue(float chronoref)
{
float tps_actuel;
/*if (depart == 0)
{

return 0;

}*/
tps_actuel = remote_read('H')/100.0f;
printf("%f \n", tps_actuel);
printf("Temps = %g \n", tps_actuel);
if ( tps_actuel > 40 /*chronoref*2*/ )
{
return 1;
}
return 0;
}


int course_terminee()
{
return fin;
}


/** Fonction appelée lorsqu'un évènement est recu **/
void f(int v, float chronoref)
{
printf ("!!! Event:%04X !!!\n",v);
printf("test2\n");
if (v & 128)
{
depart =1 ;
}
if (v & 2048)
{
fin = 1;
}
}


int parcours (float vitesse, float angle, float distance_mur, float coefficient, float * chrono, float chronoref)
{
int htime=0;

//Initialisation des peripheriques
remote_init(local_periph);

/** Définition de la fonction de réception des évènements **/
event_callback = &f;

/** Mettre Votre Nom **/
remote_nom("??");


/**Telemetre à gauche 70°**/
remote_write('T',700);

/**Laisse le temps à la voiture de se stabiliser**/
Sleep(2000);

/**Demarre la voiture avec une vitesse de 20**/
remote_write('V',vitesse);

depart=0;
fin=0;

do{
float dist;
float err;
float angle;

/**Lecture de la distance au mur **/
dist=remote_read('U');

/**Convertion en metres**/
dist=dist/100.0f;

/**Calcul de l'erreur de distance par rapport à 5.5m du mur**/
err=dist-distance_mur;

/** Calcul de l'angle à appliquer avec une Correction proportionnelle de gain de 11 **/
angle = err*coefficient;

/** Conversion de l'angle en 1/10° **/
angle=angle*10;

/** Definition de l'angle de braquage des roues**/
remote_write('D',angle);

/**Tempo 100ms **/
Sleep(100);
printf ("Distance :%3.2f Erreur: %3.2f Angle:%3.2f \n",dist,err,angle);


/** Attente fin de course ou touche 'Q' appuyée**/
}while(!course_terminee() && !course_trop_longue(chronoref));

printf ("Fin de la course\n");
htime=remote_read('H');
*chrono=htime/100.0f;
remote_close();
if (course_terminee())
{
return 0; /* renvoi 0 si la course s'est bien terminee*/
}
else
{
return 1;
}
}

int recherche(float vitesse, float angle, float distance_mur, float correction_angle, float * temps)
{
int v;
int a;
float d_m;
float c_a;


FILE* fichier = NULL;

fichier = fopen("resultats.txt", "r");

if (fichier != NULL)
{

while(!feof(fichier))
{

fscanf(fichier, "%f %f %f %f %f", &v, &a, &d_m, &c_a, &temps);
if(v==vitesse && a==angle && d_m==distance_mur && c_a==correction_angle)
{
fclose(fichier);
return 1;
}


}

fclose(fichier);
}
else
{
printf("erreur ouverture fichier dans recherche \n");
return 2;
}

return 0; //retourne 1 s'il trouve quelque chose
}

void recherche_ligne (int i, float * v, float * a, float * d_m, float * c, float * tps, char * nom)
{
int k=0;

FILE* fichier = NULL;

fichier = fopen(nom, "r");
if (fichier != NULL)
{
fseek(fichier, 0, SEEK_SET);
while(!feof(fichier) && k != i)
{
fscanf(fichier, "%f %f %f %f %f", v, a, d_m, c, tps);
k++;
}
fclose(fichier);
}
else
{
printf("erreur ouverture fichier dans recherche \n");
}
}


void ecriture_ligne (int i, float v, float a, float d_m, float c, float tps, char * nom)
{
int k=0;

FILE* fichier = NULL;
fichier = fopen(nom, "r+");
if (fichier != NULL)
{
fseek(fichier, 0, SEEK_SET);
while(!feof(fichier) && k != i)
{
k++;
}
fprintf(fichier, "%g %g %g %g %g \n", v, a, d_m, c, tps);
fclose(fichier);
}
else
{
printf("erreur ouverture fichier dans ecriture \n");
}
}

void ecriture(float vitesse, float angle, float resultat, float distance_mur, float correction_angle, char * nom) //ecriture ligne mais ecrit a la fin
{
FILE* fichier;

fichier = fopen(nom, "r+");
if (fichier != NULL)
{

fseek(fichier, 0, SEEK_END);


fprintf(fichier, "%f %f %f %f %f \n", vitesse, angle, distance_mur, correction_angle, resultat);
fclose(fichier);
}
else
{
printf("erreur ouverture fichier dans ecriture");

}
}

void remplir_aleatoire(float tableau[64][5], char * fichier_source)
{
FILE * fic1;
FILE * fic2;
int iter=0, i;
float vi;
float ai;
float di;
float ci;
float tpsi;


fic2=fopen(fichier_source, "r+");

srand(time(0));

do
{

i=rand()%N;
printf("%d \n", i);



recherche_ligne (i,&vi,&ai,&di,&ci,&tpsi, "resultats.txt");
printf("%f %f %f %f %f \n", vi, ai, di, ci, tpsi);
tableau[iter][0]=vi;
tableau[iter][1]=ai;
tableau[iter][2]=di;
tableau[iter][3]=ci;
tableau[iter][4]=tpsi;
iter++;
printf("iterr : %d \n", iter);

}while(iter<63);


fclose(fic2);


}

void affiche_tableau(float tab[64][5])
{
int i;
i=0;


while(i<63)
{
printf("%f %f %f %f %f \n", tab[i][0], tab[i][1], tab[i][2], tab[i][3], tab[i][4]);
i++;
}
}

/** Fonction Principale **/
int main()
{
//float tab [5];
float tableau[64][5];
int i,j,iter=0,k; //k : pour savoir si course se termine bien

float itab[5],jtab[5];
float x[3], y;
float alpha=0.3;
//float eps = pow(10,-6);

float vi, vj;
float ai, aj ;
float di, dj;
float ci, cj;
float tpsi, tpsj, tps_ref=100;

srand(time(0));

remplir_aleatoire(tableau, "resultats.txt");

do
{
iter++;
i=rand()%63;
j=rand()%63;
while (i == j)
{
j=rand()%63;
}
printf("%d \n", i);
//aller à la ligne i
/*recherche_ligne (i,&vi,&ai,&di,&ci,&tpsi, "recuit.txt");*/
itab[0]=tableau[i][0];
itab[1]=tableau[i][1];
itab[2]=tableau[i][2];
itab[3]=tableau[i][3];
itab[4]=tableau[i][4];


//aller à la ligne j
/*recherche_ligne (j,&vj,&aj,&dj,&cj,&tpsj, "recuit.txt");*/
jtab[0]=tableau[j][0];
jtab[1]=tableau[j][1];
jtab[2]=tableau[j][2];
jtab[3]=tableau[j][3];
jtab[4]=tableau[i][4];

x[0] = alpha*itab[0]+(1-alpha)*jtab[0];
x[1] = alpha*itab[1]+(1-alpha)*jtab[1];
x[2] = alpha*itab[2]+(1-alpha)*jtab[2];
x[3] = alpha*itab[3]+(1-alpha)*jtab[3];



/*if (itab[4] < jtab[4])
{
tps_ref=itab[4];
}
else
{
tps_ref=jtab[4];
}*/

if(recherche(x[0], x[1], x[2], x[3], &tps_ref) != 1) //voiture n a pas fait avec ces parametres
{

printf("je prend pour parametres : %f %f %f %f \n", x[0], x[1], x[2], x[3]);
k = parcours (x[0],x[1],x[2],x[3],&y,tps_ref);
if (k != 0)
{
printf("la course c est mal passe \n");
}
ecriture(x[0], x[1], tps_ref, x[2], x[3], "resultats.txt");
}
// sinon on ecrit quand meme mais avec le temps recupere dans la recherche

if (y <= itab[4])
{
if (itab[4] <= jtab[4])
{
//remplacer la ligne j par x dans le fichier source
/*ecriture_ligne (j,x[0],x[1],x[2],x[3],y, "recuit.txt");*/

tableau[j][0]=x[0];
tableau[j][1]=x[1];
tableau[j][2]=x[2];
tableau[j][3]=x[3];
tableau[j][4]=y;


printf("je remplace ligne j par x \n");
}
else
{
//remplacer la ligne i par x dans le fichier source
/*ecriture_ligne (i,x[0],x[1],x[2],x[3],y, "recuit.txt");*/

tableau[i][0]=x[0];
tableau[i][1]=x[1];
tableau[i][2]=x[2];
tableau[i][3]=x[3];
tableau[i][4]=y;

printf("je remplace ligne i par x \n");
}
}
else
{
if (y <= jtab[4])
{
//remplacer la ligne j par x dans le fichier source
/*ecriture_ligne (j,x[0],x[1],x[2],x[3],y, "recuit.txt");*/

tableau[j][0]=x[0];
tableau[j][1]=x[1];
tableau[j][2]=x[2];
tableau[j][3]=x[3];
tableau[j][4]=y;

printf("je remplace ligne j par x \n");
}
}
printf("iter : %d \n", iter);
} while (iter<100);
affiche_tableau(tableau);
printf("vitesse minimum obtenue: %g\n", y);
printf("nombre d'iterations: %d\n", iter);
return 0;
}
    (1-1/1)