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@7:20d05f0d11a2, 2019-06-08 (annotated)

Committer:
Mrlinkblue
Date:
Sat Jun 08 09:41:48 2019 +0000
Revision:
7:20d05f0d11a2
Parent:
6:9af875ef7b30 
version_8_juin

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 myled2(LED2);
Mrlinkblue 7:20d05f0d11a2 12
Mrlinkblue 6:9af875ef7b30 13
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 7:20d05f0d11a2 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 7:20d05f0d11a2 81 // if (newData){
Mrlinkblue 7:20d05f0d11a2 82 // newData = 0;
Mrlinkblue 7:20d05f0d11a2 83 // addRadar(dataReady, prevData);
Mrlinkblue 7:20d05f0d11a2 84 // for(int k=0; k<4; k++){
Mrlinkblue 7:20d05f0d11a2 85 // prevData[k] = dataReady[k];
Mrlinkblue 7:20d05f0d11a2 86 // }
Mrlinkblue 7:20d05f0d11a2 87 //}
Mrlinkblue 5:73aac5fe9696 88 if (newRadar) {
Mrlinkblue 5:73aac5fe9696 89 cpt++;
Mrlinkblue 5:73aac5fe9696 90 newRadar = false;
Mrlinkblue 7:20d05f0d11a2 91 //myled3 = !myled3 ;
Mrlinkblue 5:73aac5fe9696 92 // Conversion tableau -> vector
Mrlinkblue 5:73aac5fe9696 93 for (int i =0; i<360; i++) {
Mrlinkblue 5:73aac5fe9696 94 data_distances[i]=float(Radar[i])/1000.0; // (m)
Mrlinkblue 3:97827746c632 95 }
Mrlinkblue 7:20d05f0d11a2 96 myled3=!myled3;
Mrlinkblue 5:73aac5fe9696 97 actualisation();
Mrlinkblue 7:20d05f0d11a2 98 //myled3=0;
Mrlinkblue 5:73aac5fe9696 99 if(cpt%100==0) {
Mrlinkblue 5:73aac5fe9696 100 for(int i=0; i<360; i++) {
Mrlinkblue 5:73aac5fe9696 101 pc.printf("%3d %4.3f%\n",i,data_distances[i]);
Mrlinkblue 5:73aac5fe9696 102 }
Mrlinkblue 5:73aac5fe9696 103 }
Mrlinkblue 5:73aac5fe9696 104 t1 = t.read();
Mrlinkblue 3:97827746c632 105 }
Mrlinkblue 5:73aac5fe9696 106
Mrlinkblue 5:73aac5fe9696 107 avancer(voiture_deltat);
Mrlinkblue 5:73aac5fe9696 108
Mrlinkblue 5:73aac5fe9696 109
Mrlinkblue 5:73aac5fe9696 110 // converversion vitesse/angle
Mrlinkblue 5:73aac5fe9696 111 int t_angle = convers_angle_tempsh(voiture_angle_roues);
Mrlinkblue 5:73aac5fe9696 112
Mrlinkblue 5:73aac5fe9696 113 if (voiture_angle_roues==0) {
Mrlinkblue 5:73aac5fe9696 114 myled2=1;
Mrlinkblue 5:73aac5fe9696 115 } else {
Mrlinkblue 5:73aac5fe9696 116 myled2=0;
Mrlinkblue 0:9eb40ee5ff41 117 }
Mrlinkblue 5:73aac5fe9696 118
Mrlinkblue 5:73aac5fe9696 119 int t_vitesse = convers_vitesse_tempsh(voiture_vitesse);
Mrlinkblue 6:9af875ef7b30 120 t_vitesse=1480;
Mrlinkblue 6:9af875ef7b30 121
Mrlinkblue 5:73aac5fe9696 122 // commande des servos
Mrlinkblue 5:73aac5fe9696 123 pwm_servo_direction.pulsewidth_us(t_angle);
Mrlinkblue 5:73aac5fe9696 124 pwm_servo_vitesse.pulsewidth_us(t_vitesse);
Mrlinkblue 5:73aac5fe9696 125
Mrlinkblue 5:73aac5fe9696 126 }
Mrlinkblue 5:73aac5fe9696 127 }
Mrlinkblue 5:73aac5fe9696 128
Mrlinkblue 0:9eb40ee5ff41 129 }