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@1:8c488662e000, 2019-05-31 (annotated)

Committer:
Mrlinkblue
Date:
Fri May 31 11:01:18 2019 +0000
Revision:
1:8c488662e000
Parent:
0:9eb40ee5ff41 
Marche pas - plus de lidar

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 1:8c488662e000 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 1:8c488662e000 38 // Initialisation de la 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 while (1){
Mrlinkblue 0:9eb40ee5ff41 43 // données de la centrale inertiel
Mrlinkblue 1:8c488662e000 44 recup_mpu9250();
Mrlinkblue 0:9eb40ee5ff41 45
Mrlinkblue 0:9eb40ee5ff41 46 // mise à jour l'angle de la voiture
Mrlinkblue 0:9eb40ee5ff41 47 voiture_angle = voiture_angle + gz*voiture_deltat;
Mrlinkblue 0:9eb40ee5ff41 48 // gz et voiture_angle a été vérifié et marche bien
Mrlinkblue 0:9eb40ee5ff41 49 // voiture angle = angle dans le repère du circuit
Mrlinkblue 0:9eb40ee5ff41 50
Mrlinkblue 0:9eb40ee5ff41 51 // mise à jour la vitesse de la voiture
Mrlinkblue 1:8c488662e000 52 //voiture_vitesse = voiture_vitesse + sqrt(ax*ax+ay*ay)*voiture_deltat*g; //(m)
Mrlinkblue 0:9eb40ee5ff41 53
Mrlinkblue 0:9eb40ee5ff41 54 // data_distances = Radar;
Mrlinkblue 0:9eb40ee5ff41 55 addRadar(Radar,data,prevData);
Mrlinkblue 0:9eb40ee5ff41 56
Mrlinkblue 0:9eb40ee5ff41 57 for (int i =0; i<360; i++){
Mrlinkblue 0:9eb40ee5ff41 58 data_distances[0]=float(Radar[0])/1000.0; // (m)
Mrlinkblue 0:9eb40ee5ff41 59 }
Mrlinkblue 0:9eb40ee5ff41 60
Mrlinkblue 0:9eb40ee5ff41 61 // L'actualisation ne se lance que si on a un tableau complet
Mrlinkblue 0:9eb40ee5ff41 62 if (newRadar){
Mrlinkblue 1:8c488662e000 63 myled2 = myled1;
Mrlinkblue 0:9eb40ee5ff41 64 myled1 =! myled1;
Mrlinkblue 0:9eb40ee5ff41 65 actualisation();
Mrlinkblue 0:9eb40ee5ff41 66 }
Mrlinkblue 0:9eb40ee5ff41 67
Mrlinkblue 0:9eb40ee5ff41 68 calculAngle(voiture_deltat);
Mrlinkblue 0:9eb40ee5ff41 69 calculVitesse(voiture_deltat);
Mrlinkblue 0:9eb40ee5ff41 70 avancer(voiture_deltat);
Mrlinkblue 0:9eb40ee5ff41 71
Mrlinkblue 0:9eb40ee5ff41 72
Mrlinkblue 0:9eb40ee5ff41 73 // converversion vitesse/angle
Mrlinkblue 0:9eb40ee5ff41 74 int t_angle = convers_angle_tempsh(angle_suivre);
Mrlinkblue 0:9eb40ee5ff41 75 int t_vitesse = convers_vitesse_tempsh(vitesse_suivre);
Mrlinkblue 0:9eb40ee5ff41 76
Mrlinkblue 0:9eb40ee5ff41 77 // commande des servos
Mrlinkblue 0:9eb40ee5ff41 78 pwm_servo_direction.pulsewidth_us(t_angle);
Mrlinkblue 0:9eb40ee5ff41 79 pwm_servo_vitesse.pulsewidth_us(t_vitesse);
Mrlinkblue 0:9eb40ee5ff41 80
Mrlinkblue 0:9eb40ee5ff41 81 voiture_deltat = t.read() - t1;
Mrlinkblue 0:9eb40ee5ff41 82 t1 = t.read();
Mrlinkblue 0:9eb40ee5ff41 83 }
Mrlinkblue 0:9eb40ee5ff41 84
Mrlinkblue 0:9eb40ee5ff41 85 }