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@0:9eb40ee5ff41, 2019-05-31 (annotated)

Committer:
Mrlinkblue
Date:
Fri May 31 09:41:29 2019 +0000
Revision:
0:9eb40ee5ff41
Child:
1:8c488662e000
Child:
2:d2e002754654
Version avant projet libre;

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 0:9eb40ee5ff41 9 // Timer
Mrlinkblue 0:9eb40ee5ff41 10 Timer t;
Mrlinkblue 0:9eb40ee5ff41 11 DigitalOut myled1(LED1);
Mrlinkblue 0:9eb40ee5ff41 12 DigitalOut myled2(LED2);
Mrlinkblue 0:9eb40ee5ff41 13 DigitalOut myled3(LED3);
Mrlinkblue 0:9eb40ee5ff41 14
Mrlinkblue 0:9eb40ee5ff41 15 float t1;
Mrlinkblue 0:9eb40ee5ff41 16 float voiture_deltat;
Mrlinkblue 0:9eb40ee5ff41 17
Mrlinkblue 0:9eb40ee5ff41 18 int main() {
Mrlinkblue 0:9eb40ee5ff41 19 // On setup toute la voiture
Mrlinkblue 0:9eb40ee5ff41 20 t.start();
Mrlinkblue 0:9eb40ee5ff41 21 t1 = t.read();
Mrlinkblue 0:9eb40ee5ff41 22
Mrlinkblue 0:9eb40ee5ff41 23 setup_lidar();
Mrlinkblue 0:9eb40ee5ff41 24 setup_mpu9250();
Mrlinkblue 0:9eb40ee5ff41 25 setup_direction();
Mrlinkblue 0:9eb40ee5ff41 26
Mrlinkblue 0:9eb40ee5ff41 27 // Tableau d'angle
Mrlinkblue 0:9eb40ee5ff41 28 for (int i =0 ; i < data_angles.size() ; i++){
Mrlinkblue 0:9eb40ee5ff41 29 data_angles[i]=i;
Mrlinkblue 0:9eb40ee5ff41 30 }
Mrlinkblue 0:9eb40ee5ff41 31
Mrlinkblue 0:9eb40ee5ff41 32 voiture_deltat = t.read() - t1;
Mrlinkblue 0:9eb40ee5ff41 33 t1 = t.read();
Mrlinkblue 0:9eb40ee5ff41 34
Mrlinkblue 0:9eb40ee5ff41 35 int t_angle = 1090;
Mrlinkblue 0:9eb40ee5ff41 36 int t_vitesse = 1480;
Mrlinkblue 0:9eb40ee5ff41 37
Mrlinkblue 0:9eb40ee5ff41 38 // commande des servos
Mrlinkblue 0:9eb40ee5ff41 39 pwm_servo_direction.pulsewidth_us(t_angle);
Mrlinkblue 0:9eb40ee5ff41 40 pwm_servo_vitesse.pulsewidth_us(t_vitesse);
Mrlinkblue 0:9eb40ee5ff41 41
Mrlinkblue 0:9eb40ee5ff41 42 myled1=1;
Mrlinkblue 0:9eb40ee5ff41 43 myled2=1;
Mrlinkblue 0:9eb40ee5ff41 44 myled3=1;
Mrlinkblue 0:9eb40ee5ff41 45
Mrlinkblue 0:9eb40ee5ff41 46 while (1){
Mrlinkblue 0:9eb40ee5ff41 47 // données de la centrale inertiel
Mrlinkblue 0:9eb40ee5ff41 48 recup_mpu9250(voiture_deltat);
Mrlinkblue 0:9eb40ee5ff41 49
Mrlinkblue 0:9eb40ee5ff41 50 // mise à jour l'angle de la voiture
Mrlinkblue 0:9eb40ee5ff41 51 voiture_angle = voiture_angle + gz*voiture_deltat;
Mrlinkblue 0:9eb40ee5ff41 52 // gz et voiture_angle a été vérifié et marche bien
Mrlinkblue 0:9eb40ee5ff41 53 // voiture angle = angle dans le repère du circuit
Mrlinkblue 0:9eb40ee5ff41 54
Mrlinkblue 0:9eb40ee5ff41 55 // mise à jour la vitesse de la voiture
Mrlinkblue 0:9eb40ee5ff41 56 voiture_vitesse = voiture_vitesse + sqrt(ax*ax+ay*ay)*voiture_deltat*g; //(m)
Mrlinkblue 0:9eb40ee5ff41 57
Mrlinkblue 0:9eb40ee5ff41 58 if (voiture_vitesse>1){
Mrlinkblue 0:9eb40ee5ff41 59 myled2=1;
Mrlinkblue 0:9eb40ee5ff41 60 }
Mrlinkblue 0:9eb40ee5ff41 61 else{
Mrlinkblue 0:9eb40ee5ff41 62 myled2=0;
Mrlinkblue 0:9eb40ee5ff41 63 }
Mrlinkblue 0:9eb40ee5ff41 64
Mrlinkblue 0:9eb40ee5ff41 65 if (voiture_vitesse>5){
Mrlinkblue 0:9eb40ee5ff41 66 myled3=1;
Mrlinkblue 0:9eb40ee5ff41 67 }
Mrlinkblue 0:9eb40ee5ff41 68 else{
Mrlinkblue 0:9eb40ee5ff41 69 myled3=0;
Mrlinkblue 0:9eb40ee5ff41 70 }
Mrlinkblue 0:9eb40ee5ff41 71
Mrlinkblue 0:9eb40ee5ff41 72 // data_distances = Radar;
Mrlinkblue 0:9eb40ee5ff41 73 addRadar(Radar,data,prevData);
Mrlinkblue 0:9eb40ee5ff41 74
Mrlinkblue 0:9eb40ee5ff41 75 for (int i =0; i<360; i++){
Mrlinkblue 0:9eb40ee5ff41 76 data_distances[0]=float(Radar[0])/1000.0; // (m)
Mrlinkblue 0:9eb40ee5ff41 77 }
Mrlinkblue 0:9eb40ee5ff41 78
Mrlinkblue 0:9eb40ee5ff41 79 // L'actualisation ne se lance que si on a un tableau complet
Mrlinkblue 0:9eb40ee5ff41 80 if (newRadar){
Mrlinkblue 0:9eb40ee5ff41 81 myled1 =! myled1;
Mrlinkblue 0:9eb40ee5ff41 82 actualisation();
Mrlinkblue 0:9eb40ee5ff41 83 }
Mrlinkblue 0:9eb40ee5ff41 84
Mrlinkblue 0:9eb40ee5ff41 85 calculAngle(voiture_deltat);
Mrlinkblue 0:9eb40ee5ff41 86 calculVitesse(voiture_deltat);
Mrlinkblue 0:9eb40ee5ff41 87 avancer(voiture_deltat);
Mrlinkblue 0:9eb40ee5ff41 88
Mrlinkblue 0:9eb40ee5ff41 89
Mrlinkblue 0:9eb40ee5ff41 90 // converversion vitesse/angle
Mrlinkblue 0:9eb40ee5ff41 91 int t_angle = convers_angle_tempsh(angle_suivre);
Mrlinkblue 0:9eb40ee5ff41 92 int t_vitesse = convers_vitesse_tempsh(vitesse_suivre);
Mrlinkblue 0:9eb40ee5ff41 93
Mrlinkblue 0:9eb40ee5ff41 94 // commande des servos
Mrlinkblue 0:9eb40ee5ff41 95 pwm_servo_direction.pulsewidth_us(t_angle);
Mrlinkblue 0:9eb40ee5ff41 96 pwm_servo_vitesse.pulsewidth_us(t_vitesse);
Mrlinkblue 0:9eb40ee5ff41 97
Mrlinkblue 0:9eb40ee5ff41 98 voiture_deltat = t.read() - t1;
Mrlinkblue 0:9eb40ee5ff41 99 t1 = t.read();
Mrlinkblue 0:9eb40ee5ff41 100 }
Mrlinkblue 0:9eb40ee5ff41 101
Mrlinkblue 0:9eb40ee5ff41 102 }