Paclay-Saris pod racers / Mbed 2 deprecated Algo_charges_fictives_4

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@18:daba0b3777c0, 2019-06-07 (annotated)

Committer:
SolalNathan
Date:
Fri Jun 07 01:11:01 2019 +0000
Revision:
18:daba0b3777c0
Parent:
17:4a65cce4ac4f 
formatage du code en differents modules

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SolalNathan 0:5d6051eeabfe 1 #include "mbed.h"
SolalNathan 2:b2ce001ff8f5 2 #include <math.h>
SolalNathan 18:daba0b3777c0 3 #include "SaphTeamRacing.h"
SolalNathan 2:b2ce001ff8f5 4
SolalNathan 2:b2ce001ff8f5 5 // Définition des ports séries
Mecaru 13:bed77b03701d 6 //Serial pc(USBTX, USBRX, 115200);
Mecaru 13:bed77b03701d 7 Serial pc(PC_10, PC_11, 115200);
SolalNathan 2:b2ce001ff8f5 8 Serial lidar(PC_6, PC_7, 115200);
SolalNathan 2:b2ce001ff8f5 9
SolalNathan 2:b2ce001ff8f5 10 // Définition des variables globales
Mecaru 12:594a1b936f4b 11 float tableau_distance0[360] = {};
Mecaru 12:594a1b936f4b 12 float tableau_distance1[360] = {};
SolalNathan 18:daba0b3777c0 13 bool tableau_en_cours = false; //tableau en cours de remplissage
Mecaru 12:594a1b936f4b 14 uint8_t flag_fin_tour_lidar=0;
SolalNathan 2:b2ce001ff8f5 15 int compteur_tours_lidar = 0;
Mecaru 11:e227edfced99 16 bool run = false;
Mecaru 17:4a65cce4ac4f 17 bool stop = true;
SolalNathan 2:b2ce001ff8f5 18
SolalNathan 2:b2ce001ff8f5 19 // Défintion des pwm
SolalNathan 2:b2ce001ff8f5 20 PwmOut pwm_lidar(PB_15); // pwm du Lidar
SolalNathan 2:b2ce001ff8f5 21 PwmOut pwm_moteur(PE_6); // pwm de la propulsion
SolalNathan 2:b2ce001ff8f5 22 PwmOut pwm_direction(PE_5); // pwm de la direction
SolalNathan 2:b2ce001ff8f5 23
SolalNathan 2:b2ce001ff8f5 24 void interrupt_lidar_rx(void);
SolalNathan 0:5d6051eeabfe 25
Mecaru 12:594a1b936f4b 26 int main()
Mecaru 12:594a1b936f4b 27 {
Mecaru 12:594a1b936f4b 28
SolalNathan 2:b2ce001ff8f5 29 pc.printf("\r-------------------------\n\r");
Mecaru 12:594a1b936f4b 30
SolalNathan 2:b2ce001ff8f5 31 float dir[2]; // direction
SolalNathan 2:b2ce001ff8f5 32 float pwm_direction_value;
SolalNathan 18:daba0b3777c0 33
SolalNathan 2:b2ce001ff8f5 34 // pwm du LIDAR
SolalNathan 2:b2ce001ff8f5 35 pwm_lidar.period_us(40);
Mecaru 5:32434b497a9b 36 pwm_lidar.pulsewidth_us(40); // vitesse fixe
Mecaru 12:594a1b936f4b 37
Mecaru 11:e227edfced99 38 //pwm moteur
SolalNathan 0:5d6051eeabfe 39 pwm_moteur.period_ms(20);
SolalNathan 2:b2ce001ff8f5 40
SolalNathan 2:b2ce001ff8f5 41 // pwm de la direction
SolalNathan 2:b2ce001ff8f5 42 pwm_direction.period_ms(20);
Mecaru 6:83dafe088914 43 pwm_direction.pulsewidth_us(1480); // correspond à un vitesse faible
SolalNathan 2:b2ce001ff8f5 44
SolalNathan 2:b2ce001ff8f5 45 // récupération du premier batch de données (7 bytes) du LIDAR
SolalNathan 2:b2ce001ff8f5 46 lidar.putc(0xA5);
SolalNathan 2:b2ce001ff8f5 47 lidar.putc(0x20);
SolalNathan 18:daba0b3777c0 48 for(int i=0; i<7; i++)
SolalNathan 2:b2ce001ff8f5 49 lidar.getc();
SolalNathan 2:b2ce001ff8f5 50
SolalNathan 2:b2ce001ff8f5 51 pc.printf("FIN intit \n\r");
SolalNathan 2:b2ce001ff8f5 52
Mecaru 12:594a1b936f4b 53 lidar.attach(&interrupt_lidar_rx, Serial::RxIrq);
Mecaru 12:594a1b936f4b 54
Mecaru 12:594a1b936f4b 55 while (1) {
Mecaru 12:594a1b936f4b 56 //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);
Mecaru 12:594a1b936f4b 57
Mecaru 12:594a1b936f4b 58 if(pc.readable()) {
Mecaru 12:594a1b936f4b 59 char entree = pc.getc();
Mecaru 12:594a1b936f4b 60 pc.printf("%c \n\r",entree);
Mecaru 12:594a1b936f4b 61 if (entree == 'a') {
Mecaru 12:594a1b936f4b 62 run = true;
Mecaru 17:4a65cce4ac4f 63 stop = false;
Mecaru 12:594a1b936f4b 64 }
Mecaru 12:594a1b936f4b 65 if (entree == 'z') {
Mecaru 12:594a1b936f4b 66 run = false;
Mecaru 12:594a1b936f4b 67 }
Mecaru 11:e227edfced99 68 }
Mecaru 12:594a1b936f4b 69
Mecaru 12:594a1b936f4b 70
Mecaru 13:bed77b03701d 71 if(0) {
SolalNathan 18:daba0b3777c0 72 if(!tableau_en_cours)
SolalNathan 18:daba0b3777c0 73 afficher_lidar(tableau_distance1, pc);
SolalNathan 18:daba0b3777c0 74 else afficher_lidar(tableau_distance0, pc);
Mecaru 12:594a1b936f4b 75
Mecaru 11:e227edfced99 76 }
Mecaru 12:594a1b936f4b 77
Mecaru 12:594a1b936f4b 78 if(flag_fin_tour_lidar==1) {
Mecaru 12:594a1b936f4b 79 flag_fin_tour_lidar=0;
SolalNathan 18:daba0b3777c0 80 if(!tableau_en_cours)
Mecaru 12:594a1b936f4b 81 update_direction(tableau_distance1, dir); // mise à jour à la direction
Mecaru 12:594a1b936f4b 82 else update_direction(tableau_distance0, dir); // mise à jour à la direction
Mecaru 12:594a1b936f4b 83 pc.printf("direction,%f,%f\n\r",dir[0],dir[1]);
Mecaru 12:594a1b936f4b 84 pwm_direction_value = angle_servo(dir); // calcul du pwm
Mecaru 12:594a1b936f4b 85 }
Mecaru 12:594a1b936f4b 86
Mecaru 17:4a65cce4ac4f 87 if (run==true) {
Mecaru 12:594a1b936f4b 88 // vitesse constante
Mecaru 17:4a65cce4ac4f 89 if (stop==false) {
Mecaru 17:4a65cce4ac4f 90 pwm_moteur.pulsewidth_us(1440);
Mecaru 17:4a65cce4ac4f 91 stop=true;
Mecaru 17:4a65cce4ac4f 92 }
Mecaru 12:594a1b936f4b 93 pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
Mecaru 12:594a1b936f4b 94 } else {
Mecaru 17:4a65cce4ac4f 95 stop=false;
Mecaru 12:594a1b936f4b 96 pwm_moteur.pulsewidth_us(1480);
Mecaru 12:594a1b936f4b 97 }
Mecaru 12:594a1b936f4b 98
Mecaru 11:e227edfced99 99 }
Mecaru 11:e227edfced99 100 }
SolalNathan 2:b2ce001ff8f5 101
SolalNathan 2:b2ce001ff8f5 102 void interrupt_lidar_rx(void)
Mecaru 12:594a1b936f4b 103 {
SolalNathan 2:b2ce001ff8f5 104
Mecaru 5:32434b497a9b 105 int SEUIL = 0; // Seuil de qualité
Mecaru 12:594a1b936f4b 106
SolalNathan 18:daba0b3777c0 107 static uint8_t data[5], i = 0, Angle_d_old = 0;
SolalNathan 18:daba0b3777c0 108 uint16_t Quality, Angle, Distance, Angle_d, Distance_d;
SolalNathan 2:b2ce001ff8f5 109 data[i] = lidar.getc();
SolalNathan 2:b2ce001ff8f5 110 i++;
Mecaru 12:594a1b936f4b 111 if(i==5) {
SolalNathan 2:b2ce001ff8f5 112 i=0;
SolalNathan 2:b2ce001ff8f5 113 Quality = data[0] & 0xFC;
SolalNathan 2:b2ce001ff8f5 114 Quality = Quality >> 2;
Mecaru 12:594a1b936f4b 115
SolalNathan 2:b2ce001ff8f5 116 Angle = data[1] & 0xFE;
SolalNathan 2:b2ce001ff8f5 117 Angle = (Angle>>1) | ((uint16_t)data[2] << 7);
Mecaru 12:594a1b936f4b 118
SolalNathan 2:b2ce001ff8f5 119 Distance = data[3];
SolalNathan 2:b2ce001ff8f5 120 Distance = Distance | ((uint16_t)data[4] << 8);
Mecaru 12:594a1b936f4b 121
SolalNathan 2:b2ce001ff8f5 122 Angle_d = Angle/64; // in degree
SolalNathan 2:b2ce001ff8f5 123 Distance_d = Distance>>2; // in mm
SolalNathan 2:b2ce001ff8f5 124
Mecaru 12:594a1b936f4b 125 // On vérifie que l'on écrit pas en dehors du tableau
Mecaru 8:2ce9493549e8 126 //Angle_d = 360 - Angle_d;
SolalNathan 2:b2ce001ff8f5 127 if(Angle_d>359) Angle_d=359;
Mecaru 12:594a1b936f4b 128
Mecaru 12:594a1b936f4b 129 if(Angle < (Angle_d_old - 180)) {
SolalNathan 18:daba0b3777c0 130 tableau_en_cours=!tableau_en_cours;
Mecaru 12:594a1b936f4b 131 flag_fin_tour_lidar=1;
Mecaru 12:594a1b936f4b 132 }
Mecaru 12:594a1b936f4b 133
SolalNathan 2:b2ce001ff8f5 134 if (Quality < SEUIL) {
SolalNathan 2:b2ce001ff8f5 135 // Fiabilisation des données du LIDAR naïve
SolalNathan 18:daba0b3777c0 136 if(!tableau_en_cours)
Mecaru 12:594a1b936f4b 137 tableau_distance0[Angle_d] = tableau_distance0[Angle_d_old];
Mecaru 12:594a1b936f4b 138 else tableau_distance1[Angle_d] = tableau_distance1[Angle_d_old];
SolalNathan 18:daba0b3777c0 139 } else if(!tableau_en_cours)
Mecaru 12:594a1b936f4b 140 tableau_distance0[Angle_d] = Distance_d;
Mecaru 12:594a1b936f4b 141 else tableau_distance1[Angle_d] = Distance_d;
Mecaru 12:594a1b936f4b 142
Mecaru 12:594a1b936f4b 143 Angle_d_old = Angle;
Mecaru 7:dc7e66870bd0 144
Mecaru 7:dc7e66870bd0 145 //tableau_distance[Angle_d] = Distance_d;
SolalNathan 2:b2ce001ff8f5 146 }
SolalNathan 18:daba0b3777c0 147 }