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@3:97827746c632, 2019-05-31 (annotated)

Committer:
Mrlinkblue
Date:
Fri May 31 13:45:02 2019 +0000
Revision:
3:97827746c632
Parent:
2:d2e002754654
Child:
4:c393c14f4502
Bug Corrected

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