Benoît Roussel / Mbed 2 deprecated test_algo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@5:73aac5fe9696, 2019-06-06 (annotated)

Committer:
Mrlinkblue
Date:
Thu Jun 06 14:04:56 2019 +0000
Revision:
5:73aac5fe9696
Parent:
4:c393c14f4502
Child:
6:9af875ef7b30
programme_6juinmatin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mrlinkblue 0:9eb40ee5ff41 1 #include "mbed.h"
Mrlinkblue 0:9eb40ee5ff41 2 #include "algorithme.h"
Mrlinkblue 0:9eb40ee5ff41 3 #include "lidar.h"
Mrlinkblue 0:9eb40ee5ff41 4 #include "direction.h"
Mrlinkblue 0:9eb40ee5ff41 5 #include "MPU9250_setup.h"
Mrlinkblue 0:9eb40ee5ff41 6 #include "MPU9250.h"
Mrlinkblue 0:9eb40ee5ff41 7
Mrlinkblue 0:9eb40ee5ff41 8 // Définition du timer global de la voiture
Mrlinkblue 5:73aac5fe9696 9 // Timer
Mrlinkblue 5:73aac5fe9696 10
Mrlinkblue 0:9eb40ee5ff41 11 DigitalOut myled1(LED1);
Mrlinkblue 0:9eb40ee5ff41 12 DigitalOut myled2(LED2);
Mrlinkblue 0:9eb40ee5ff41 13 DigitalOut myled3(LED3);
Mrlinkblue 5:73aac5fe9696 14 Serial pc(USBTX,USBRX,115200);
Mrlinkblue 5:73aac5fe9696 15 float tnul = 0.0;
Mrlinkblue 0:9eb40ee5ff41 16 float t1;
Mrlinkblue 0:9eb40ee5ff41 17 float voiture_deltat;
Mrlinkblue 5:73aac5fe9696 18 float deltat_start;
Mrlinkblue 0:9eb40ee5ff41 19
Mrlinkblue 5:73aac5fe9696 20 int main()
Mrlinkblue 5:73aac5fe9696 21 {
Mrlinkblue 5:73aac5fe9696 22 int cpt =0;
Mrlinkblue 5:73aac5fe9696 23 // On setup toute la voiture
Mrlinkblue 5:73aac5fe9696 24 t.start();
Mrlinkblue 5:73aac5fe9696 25
Mrlinkblue 5:73aac5fe9696 26 setup_lidar();
Mrlinkblue 5:73aac5fe9696 27 setup_mpu9250();
Mrlinkblue 5:73aac5fe9696 28 setup_direction();
Mrlinkblue 5:73aac5fe9696 29
Mrlinkblue 5:73aac5fe9696 30 // Tableau d'angle
Mrlinkblue 5:73aac5fe9696 31 for (int i =0 ; i < data_angles.size() ; i++) {
Mrlinkblue 5:73aac5fe9696 32 data_angles[i]=i;
Mrlinkblue 5:73aac5fe9696 33 }
Mrlinkblue 5:73aac5fe9696 34
Mrlinkblue 5:73aac5fe9696 35
Mrlinkblue 0:9eb40ee5ff41 36 int t_angle = 1090;
Mrlinkblue 0:9eb40ee5ff41 37 int t_vitesse = 1480;
Mrlinkblue 5:73aac5fe9696 38
Mrlinkblue 0:9eb40ee5ff41 39 // commande des servos
Mrlinkblue 0:9eb40ee5ff41 40 pwm_servo_direction.pulsewidth_us(t_angle);
Mrlinkblue 0:9eb40ee5ff41 41 pwm_servo_vitesse.pulsewidth_us(t_vitesse);
Mrlinkblue 5:73aac5fe9696 42
Mrlinkblue 3:97827746c632 43 //Signal lumineux de démarrage
Mrlinkblue 0:9eb40ee5ff41 44 myled1=1;
Mrlinkblue 0:9eb40ee5ff41 45 myled2=1;
Mrlinkblue 0:9eb40ee5ff41 46 myled3=1;
Mrlinkblue 3:97827746c632 47 wait_ms(10);
Mrlinkblue 3:97827746c632 48 myled1=0;
Mrlinkblue 3:97827746c632 49 myled2=0;
Mrlinkblue 5:73aac5fe9696 50 myled3=0;
Mrlinkblue 5:73aac5fe9696 51
Mrlinkblue 5:73aac5fe9696 52
Mrlinkblue 5:73aac5fe9696 53
Mrlinkblue 5:73aac5fe9696 54 while (1) {
Mrlinkblue 5:73aac5fe9696 55 recuperer_start(tnul);
Mrlinkblue 5:73aac5fe9696 56 if (start) {
Mrlinkblue 5:73aac5fe9696 57 tstart = t.read();
Mrlinkblue 5:73aac5fe9696 58 deltat_start = t.read() - tstart;
Mrlinkblue 5:73aac5fe9696 59 t1 = t.read();
Mrlinkblue 5:73aac5fe9696 60 voiture_deltat = t.read() - t1;
Mrlinkblue 5:73aac5fe9696 61 }
Mrlinkblue 5:73aac5fe9696 62
Mrlinkblue 5:73aac5fe9696 63 while (1) { //while(start){
Mrlinkblue 5:73aac5fe9696 64 // données de la centrale inertiel
Mrlinkblue 5:73aac5fe9696 65 // data_distances = Radar;
Mrlinkblue 5:73aac5fe9696 66 recuperer_start(deltat_start);
Mrlinkblue 5:73aac5fe9696 67 recup_mpu9250(voiture_deltat); // n'utilise pas voiture_deltat
Mrlinkblue 5:73aac5fe9696 68
Mrlinkblue 5:73aac5fe9696 69 deltat_start = t.read() - tstart;
Mrlinkblue 5:73aac5fe9696 70 voiture_deltat = t.read() - t1;
Mrlinkblue 5:73aac5fe9696 71 // mise à jour l'angle de la voiture
Mrlinkblue 5:73aac5fe9696 72 voiture_angle = voiture_angle + gz*voiture_deltat;
Mrlinkblue 5:73aac5fe9696 73 // gz et voiture_angle a été vérifié et marche bien
Mrlinkblue 5:73aac5fe9696 74 // voiture angle = angle dans le repère du circuit
Mrlinkblue 5:73aac5fe9696 75
Mrlinkblue 5:73aac5fe9696 76 // mise à jour la vitesse de la voiture
Mrlinkblue 5:73aac5fe9696 77 //voiture_vitesse = voiture_vitesse + sqrt(ax*ax+ay*ay)*voiture_deltat*g; //(m)
Mrlinkblue 5:73aac5fe9696 78
Mrlinkblue 5:73aac5fe9696 79
Mrlinkblue 5:73aac5fe9696 80 // L'actualisation ne se lance que si on a un tableau complet
Mrlinkblue 5:73aac5fe9696 81
Mrlinkblue 5:73aac5fe9696 82 if (newRadar) {
Mrlinkblue 5:73aac5fe9696 83 cpt++;
Mrlinkblue 5:73aac5fe9696 84 newRadar = false;
Mrlinkblue 5:73aac5fe9696 85 myled1 = !myled1 ;
Mrlinkblue 5:73aac5fe9696 86 // Conversion tableau -> vector
Mrlinkblue 5:73aac5fe9696 87 for (int i =0; i<360; i++) {
Mrlinkblue 5:73aac5fe9696 88 data_distances[i]=float(Radar[i])/1000.0; // (m)
Mrlinkblue 3:97827746c632 89 }
Mrlinkblue 5:73aac5fe9696 90 actualisation();
Mrlinkblue 5:73aac5fe9696 91 if(cpt%100==0) {
Mrlinkblue 5:73aac5fe9696 92 myled3 = !myled3;
Mrlinkblue 5:73aac5fe9696 93 for(int i=0; i<360; i++) {
Mrlinkblue 5:73aac5fe9696 94 pc.printf("%3d %4.3f%\n",i,data_distances[i]);
Mrlinkblue 5:73aac5fe9696 95 }
Mrlinkblue 5:73aac5fe9696 96 }
Mrlinkblue 5:73aac5fe9696 97 t1 = t.read();
Mrlinkblue 3:97827746c632 98 }
Mrlinkblue 5:73aac5fe9696 99
Mrlinkblue 5:73aac5fe9696 100 avancer(voiture_deltat);
Mrlinkblue 5:73aac5fe9696 101
Mrlinkblue 5:73aac5fe9696 102
Mrlinkblue 5:73aac5fe9696 103 // converversion vitesse/angle
Mrlinkblue 5:73aac5fe9696 104 int t_angle = convers_angle_tempsh(voiture_angle_roues);
Mrlinkblue 5:73aac5fe9696 105
Mrlinkblue 5:73aac5fe9696 106 if (voiture_angle_roues==0) {
Mrlinkblue 5:73aac5fe9696 107 myled2=1;
Mrlinkblue 5:73aac5fe9696 108 } else {
Mrlinkblue 5:73aac5fe9696 109 myled2=0;
Mrlinkblue 0:9eb40ee5ff41 110 }
Mrlinkblue 5:73aac5fe9696 111
Mrlinkblue 5:73aac5fe9696 112 int t_vitesse = convers_vitesse_tempsh(voiture_vitesse);
Mrlinkblue 5:73aac5fe9696 113
Mrlinkblue 5:73aac5fe9696 114 if (1440 <=t_vitesse <= 1480) {
Mrlinkblue 5:73aac5fe9696 115 myled3=0;
Mrlinkblue 5:73aac5fe9696 116 } else {
Mrlinkblue 5:73aac5fe9696 117 myled3=1;
Mrlinkblue 0:9eb40ee5ff41 118 }
Mrlinkblue 5:73aac5fe9696 119
Mrlinkblue 5:73aac5fe9696 120 // commande des servos
Mrlinkblue 5:73aac5fe9696 121 pwm_servo_direction.pulsewidth_us(t_angle);
Mrlinkblue 5:73aac5fe9696 122 pwm_servo_vitesse.pulsewidth_us(t_vitesse);
Mrlinkblue 5:73aac5fe9696 123
Mrlinkblue 5:73aac5fe9696 124 }
Mrlinkblue 5:73aac5fe9696 125 }
Mrlinkblue 5:73aac5fe9696 126
Mrlinkblue 0:9eb40ee5ff41 127 }