pilote_automatique.c

Maxime Eymery, 07/01/2013 08:53 PM

Download (10.8 KB)

 
1

    
2
#include <stdio.h>
3
#include <conio.h>
4
#include <math.h>
5
#include <windows.h>
6
#include "remote_socket_client.h"
7
#include "gestion.h"
8
#define N 2700
9
//link with ws2_32
10

    
11

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

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

    
52

    
53
int depart;
54
int fin;
55

    
56
int course_trop_longue(float chronoref)
57
{
58
    float tps_actuel;
59
    /*if (depart == 0)
60
    {
61

62
        return 0;
63

64
    }*/
65
    tps_actuel = remote_read('H')/100.0f;
66
    printf("%f \n", tps_actuel);
67
    printf("Temps = %g \n", tps_actuel);
68
    if ( tps_actuel > 40 /*chronoref*2*/ )
69
    {
70
        return 1;
71
    }
72
    return 0;
73
}
74

    
75

    
76
int course_terminee()
77
{
78
    return fin;
79
}
80

    
81

    
82
/** Fonction appelée lorsqu'un évènement est recu **/
83
void f(int v, float chronoref)
84
{
85
    printf ("!!! Event:%04X !!!\n",v);
86
    printf("test2\n");
87
    if (v & 128)
88
    {
89
       depart =1 ;
90
    }
91
    if (v & 2048)
92
    {
93
        fin = 1;
94
    }
95
}
96

    
97

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

    
102
//Initialisation des peripheriques
103
    remote_init(local_periph);
104

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

    
108
/** Mettre Votre Nom **/
109
    remote_nom("??");
110

    
111

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

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

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

    
121
    depart=0;
122
    fin=0;
123

    
124
        do{
125
    float dist;
126
    float err;
127
    float angle;
128

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

    
132
        /**Convertion en metres**/
133
        dist=dist/100.0f;
134

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

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

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

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

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

    
151

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

    
155
    printf ("Fin de la course\n");
156
    htime=remote_read('H');
157
    *chrono=htime/100.0f;
158
        remote_close();
159
        if (course_terminee())
160
        {
161
            return 0; /* renvoi 0 si la course s'est bien terminee*/
162
        }
163
        else
164
        {
165
            return 1;
166
        }
167
}
168

    
169
int recherche(float vitesse, float angle, float distance_mur, float correction_angle, float * temps)
170
{
171
    int v;
172
    int a;
173
    float d_m;
174
    float c_a;
175

    
176

    
177
    FILE* fichier = NULL;
178

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

    
181
    if (fichier != NULL)
182
    {
183

    
184
        while(!feof(fichier))
185
        {
186

    
187
            fscanf(fichier, "%f %f %f %f %f", &v, &a, &d_m, &c_a, &temps);
188
            if(v==vitesse && a==angle && d_m==distance_mur && c_a==correction_angle)
189
            {
190
                fclose(fichier);
191
                return 1;
192
            }
193

    
194

    
195
        }
196

    
197
        fclose(fichier);
198
    }
199
    else
200
    {
201
        printf("erreur ouverture fichier dans recherche \n");
202
        return 2;
203
    }
204

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

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

    
212
    FILE* fichier = NULL;
213

    
214
    fichier = fopen(nom, "r");
215
    if (fichier != NULL)
216
    {
217
            fseek(fichier, 0, SEEK_SET);
218
        while(!feof(fichier) && k != i)
219
        {
220
            fscanf(fichier, "%f %f %f %f %f", v, a, d_m, c, tps);
221
            k++;
222
        }
223
        fclose(fichier);
224
    }
225
    else
226
    {
227
        printf("erreur ouverture fichier dans recherche \n");
228
    }
229
}
230

    
231

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

    
236
    FILE* fichier = NULL;
237
    fichier = fopen(nom, "r+");
238
    if (fichier != NULL)
239
    {
240
            fseek(fichier, 0, SEEK_SET);
241
        while(!feof(fichier) && k != i)
242
        {
243
            k++;
244
        }
245
        fprintf(fichier, "%g %g %g %g %g \n", v, a, d_m, c, tps);
246
        fclose(fichier);
247
    }
248
    else
249
    {
250
        printf("erreur ouverture fichier dans ecriture \n");
251
    }
252
}
253

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

    
258
    fichier = fopen(nom, "r+");
259
    if (fichier != NULL)
260
    {
261

    
262
        fseek(fichier, 0, SEEK_END);
263

    
264

    
265
        fprintf(fichier, "%f %f %f %f %f \n", vitesse, angle, distance_mur, correction_angle, resultat);
266
        fclose(fichier);
267
    }
268
    else
269
    {
270
        printf("erreur ouverture fichier dans ecriture");
271

    
272
    }
273
}
274

    
275
void remplir_aleatoire(float tableau[64][5], char * fichier_source)
276
{
277
    FILE * fic1;
278
    FILE * fic2;
279
    int iter=0, i;
280
    float vi;
281
    float ai;
282
    float di;
283
    float ci;
284
    float tpsi;
285

    
286

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

    
289
    srand(time(0));
290

    
291
    do
292
        {
293

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

    
297

    
298

    
299
                recherche_ligne (i,&vi,&ai,&di,&ci,&tpsi, "resultats.txt");
300
                printf("%f %f %f %f %f \n", vi, ai, di, ci, tpsi);
301
        tableau[iter][0]=vi;
302
        tableau[iter][1]=ai;
303
        tableau[iter][2]=di;
304
        tableau[iter][3]=ci;
305
        tableau[iter][4]=tpsi;
306
        iter++;
307
        printf("iterr : %d \n", iter);
308

    
309
    }while(iter<63);
310

    
311

    
312
    fclose(fic2);
313

    
314

    
315
}
316

    
317
void affiche_tableau(float tab[64][5])
318
{
319
    int i;
320
    i=0;
321

    
322

    
323
    while(i<63)
324
    {
325
        printf("%f %f %f %f %f \n", tab[i][0], tab[i][1], tab[i][2], tab[i][3], tab[i][4]);
326
        i++;
327
    }
328
}
329

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

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

    
342
        float vi, vj;
343
    float ai, aj ;
344
    float di, dj;
345
    float ci, cj;
346
    float tpsi, tpsj, tps_ref=100;
347

    
348
        srand(time(0));
349

    
350
        remplir_aleatoire(tableau, "resultats.txt");
351

    
352
        do
353
        {
354
                iter++;
355
                i=rand()%63;
356
                j=rand()%63;
357
                while (i == j)
358
                {
359
                        j=rand()%63;
360
                }
361
        printf("%d \n", i);
362
                //aller à la ligne i
363
                /*recherche_ligne (i,&vi,&ai,&di,&ci,&tpsi, "recuit.txt");*/
364
                itab[0]=tableau[i][0];
365
                itab[1]=tableau[i][1];
366
                itab[2]=tableau[i][2];
367
                itab[3]=tableau[i][3];
368
                itab[4]=tableau[i][4];
369

    
370

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

    
379
                x[0] = alpha*itab[0]+(1-alpha)*jtab[0];
380
                x[1] = alpha*itab[1]+(1-alpha)*jtab[1];
381
                x[2] = alpha*itab[2]+(1-alpha)*jtab[2];
382
                x[3] = alpha*itab[3]+(1-alpha)*jtab[3];
383

    
384

    
385

    
386
                /*if (itab[4] < jtab[4])
387
                {
388
                        tps_ref=itab[4];
389
                }
390
                else
391
                {
392
                        tps_ref=jtab[4];
393
                }*/
394

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

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

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

    
415
                                            tableau[j][0]=x[0];
416
                                            tableau[j][1]=x[1];
417
                                            tableau[j][2]=x[2];
418
                                            tableau[j][3]=x[3];
419
                                            tableau[j][4]=y;
420

    
421

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

    
429
                                            tableau[i][0]=x[0];
430
                                            tableau[i][1]=x[1];
431
                                            tableau[i][2]=x[2];
432
                                            tableau[i][3]=x[3];
433
                                            tableau[i][4]=y;
434

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

    
445
                                            tableau[j][0]=x[0];
446
                                            tableau[j][1]=x[1];
447
                                            tableau[j][2]=x[2];
448
                                            tableau[j][3]=x[3];
449
                                            tableau[j][4]=y;
450

    
451
                                        printf("je remplace ligne j par x \n");
452
                                }
453
                        }
454
        printf("iter : %d \n", iter);
455
        } while (iter<100);
456
    affiche_tableau(tableau);
457
        printf("vitesse minimum obtenue: %g\n", y);
458
        printf("nombre d'iterations: %d\n", iter);
459
        return 0;
460
}