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@4:60e7e1c1d1d8, 2019-05-17 (annotated)

Committer:
Mecaru
Date:
Fri May 17 09:25:04 2019 +0000
Revision:
4:60e7e1c1d1d8
Parent:
3:46ea1b20397d
Child:
5:32434b497a9b
Print Lidar data on serial

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 2:b2ce001ff8f5 3
SolalNathan 2:b2ce001ff8f5 4 // Définition des ports séries
SolalNathan 2:b2ce001ff8f5 5 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
SolalNathan 2:b2ce001ff8f5 6 Serial lidar(PC_6, PC_7, 115200);
SolalNathan 2:b2ce001ff8f5 7
SolalNathan 2:b2ce001ff8f5 8 // Définition des variables globales
SolalNathan 2:b2ce001ff8f5 9 int tableau_distance[360] = {};
SolalNathan 2:b2ce001ff8f5 10 int compteur_tours_lidar = 0;
SolalNathan 2:b2ce001ff8f5 11
SolalNathan 2:b2ce001ff8f5 12 // Défintion des pwm
SolalNathan 2:b2ce001ff8f5 13 PwmOut pwm_lidar(PB_15); // pwm du Lidar
SolalNathan 2:b2ce001ff8f5 14 PwmOut pwm_moteur(PE_6); // pwm de la propulsion
SolalNathan 2:b2ce001ff8f5 15 PwmOut pwm_direction(PE_5); // pwm de la direction
SolalNathan 2:b2ce001ff8f5 16
SolalNathan 2:b2ce001ff8f5 17 void interrupt_lidar_rx(void);
SolalNathan 0:5d6051eeabfe 18
Mecaru 4:60e7e1c1d1d8 19
SolalNathan 2:b2ce001ff8f5 20 float distance(float x_1, float x_2, float y_1, float y_2)
SolalNathan 2:b2ce001ff8f5 21 {
SolalNathan 2:b2ce001ff8f5 22 // Fonction qui renvoie la distance entre deux points (norme 2)
SolalNathan 2:b2ce001ff8f5 23 float norm2;
SolalNathan 2:b2ce001ff8f5 24 norm2 = sqrt((x_1 - x_2)*(x_1 - x_2) + (y_1 - y_2)*(y_1 - y_2));
SolalNathan 2:b2ce001ff8f5 25 return norm2;
SolalNathan 2:b2ce001ff8f5 26 }
SolalNathan 2:b2ce001ff8f5 27
SolalNathan 2:b2ce001ff8f5 28 void update_direction(int* list_lidar, float* vecteur)
SolalNathan 2:b2ce001ff8f5 29 {
SolalNathan 2:b2ce001ff8f5 30 // Fonction de mise à jour de la direction
SolalNathan 2:b2ce001ff8f5 31 float direction[2];
SolalNathan 2:b2ce001ff8f5 32 direction[0] = 0;
SolalNathan 2:b2ce001ff8f5 33 direction[1] = 1;
SolalNathan 2:b2ce001ff8f5 34 float avg_x, avg_y, sum_inv_dist;
SolalNathan 2:b2ce001ff8f5 35 list_lidar[180] = 50; // [mm], point fictif qui pousse la voiture
SolalNathan 2:b2ce001ff8f5 36 int i;
SolalNathan 2:b2ce001ff8f5 37 avg_x = 0;
SolalNathan 2:b2ce001ff8f5 38 avg_y = 0;
SolalNathan 2:b2ce001ff8f5 39
SolalNathan 2:b2ce001ff8f5 40 // Calcul de la direction à prende en fonction des charges fictives
SolalNathan 2:b2ce001ff8f5 41 for (i=0; i<360; i++)
SolalNathan 2:b2ce001ff8f5 42 {
SolalNathan 2:b2ce001ff8f5 43 int theta;
SolalNathan 2:b2ce001ff8f5 44 float r, x, y;
SolalNathan 2:b2ce001ff8f5 45 theta = i;
SolalNathan 2:b2ce001ff8f5 46 r = list_lidar[theta];
SolalNathan 2:b2ce001ff8f5 47
SolalNathan 2:b2ce001ff8f5 48 if (r == 0) break; // non calcul en cas de distance nul (donnée non capté)
SolalNathan 2:b2ce001ff8f5 49
SolalNathan 2:b2ce001ff8f5 50 //x = 0;
SolalNathan 2:b2ce001ff8f5 51 //y = 0;
SolalNathan 2:b2ce001ff8f5 52 x = r*cosf(theta);
SolalNathan 2:b2ce001ff8f5 53 y = r*sinf(theta);
SolalNathan 2:b2ce001ff8f5 54 sum_inv_dist += 1/pow(r, 2);
SolalNathan 2:b2ce001ff8f5 55 avg_x -= x/sum_inv_dist;
SolalNathan 2:b2ce001ff8f5 56 avg_y -= y/sum_inv_dist;
SolalNathan 2:b2ce001ff8f5 57 }
SolalNathan 2:b2ce001ff8f5 58
SolalNathan 2:b2ce001ff8f5 59 avg_x /= sum_inv_dist;
SolalNathan 2:b2ce001ff8f5 60 avg_y /= sum_inv_dist;
SolalNathan 2:b2ce001ff8f5 61 direction[0] = avg_x;
SolalNathan 2:b2ce001ff8f5 62 direction[1] = avg_y;
SolalNathan 2:b2ce001ff8f5 63
SolalNathan 2:b2ce001ff8f5 64 // mise à jour de la direction
SolalNathan 2:b2ce001ff8f5 65 for(i=0; i<2; i++)
SolalNathan 2:b2ce001ff8f5 66 vecteur[i] = direction[i];
SolalNathan 2:b2ce001ff8f5 67 }
SolalNathan 2:b2ce001ff8f5 68
SolalNathan 2:b2ce001ff8f5 69 float angle_servo(float *direction)
SolalNathan 2:b2ce001ff8f5 70 {
SolalNathan 2:b2ce001ff8f5 71 // Calcul basé sur la régression expérimental pour obetenir l'angle
SolalNathan 2:b2ce001ff8f5 72 // le pwm à donner au moteur en fonction de l'angle voulue
SolalNathan 2:b2ce001ff8f5 73
SolalNathan 2:b2ce001ff8f5 74 float angle;
SolalNathan 2:b2ce001ff8f5 75 double pwm;
SolalNathan 2:b2ce001ff8f5 76 float x, y;
SolalNathan 2:b2ce001ff8f5 77 x = direction[0];
SolalNathan 2:b2ce001ff8f5 78 y = direction[1];
SolalNathan 2:b2ce001ff8f5 79 angle = atan(x/y);
Mecaru 3:46ea1b20397d 80 pwm = 14.662756 * angle*180/3.14 + 1453.08; // à refaire
SolalNathan 2:b2ce001ff8f5 81
SolalNathan 2:b2ce001ff8f5 82 if (pwm < 1115) printf("trop petit\n\r");
SolalNathan 2:b2ce001ff8f5 83 if (pwm > 1625) printf("trop grand\n\r");
SolalNathan 2:b2ce001ff8f5 84
SolalNathan 2:b2ce001ff8f5 85 return pwm;
SolalNathan 2:b2ce001ff8f5 86 }
SolalNathan 0:5d6051eeabfe 87
Mecaru 4:60e7e1c1d1d8 88 void afficher_lidar(int *tableau_distances)
Mecaru 4:60e7e1c1d1d8 89 {
Mecaru 4:60e7e1c1d1d8 90 //Affiche les données du lidar dans la liaison série
Mecaru 4:60e7e1c1d1d8 91 int angle;
Mecaru 4:60e7e1c1d1d8 92 for(angle=0;angle<360;angle++){
Mecaru 4:60e7e1c1d1d8 93 float distance = tableau_distances[angle];
Mecaru 4:60e7e1c1d1d8 94 pc.printf("%i,%f\n\r",angle,distance);
Mecaru 4:60e7e1c1d1d8 95
Mecaru 4:60e7e1c1d1d8 96 }
Mecaru 4:60e7e1c1d1d8 97 }
Mecaru 4:60e7e1c1d1d8 98
SolalNathan 0:5d6051eeabfe 99 int main(){
SolalNathan 2:b2ce001ff8f5 100
SolalNathan 2:b2ce001ff8f5 101 pc.printf("\r-------------------------\n\r");
SolalNathan 2:b2ce001ff8f5 102
SolalNathan 2:b2ce001ff8f5 103 float dir[2]; // direction
SolalNathan 2:b2ce001ff8f5 104 float pwm_direction_value;
SolalNathan 2:b2ce001ff8f5 105
SolalNathan 2:b2ce001ff8f5 106
SolalNathan 2:b2ce001ff8f5 107 int i;
SolalNathan 2:b2ce001ff8f5 108
SolalNathan 2:b2ce001ff8f5 109 // pwm du LIDAR
SolalNathan 2:b2ce001ff8f5 110 pwm_lidar.period_us(40);
SolalNathan 2:b2ce001ff8f5 111 pwm_lidar.pulsewidth_us(22); // vitesse fixe
SolalNathan 2:b2ce001ff8f5 112
SolalNathan 2:b2ce001ff8f5 113 // pwm du Moteur
SolalNathan 0:5d6051eeabfe 114 pwm_moteur.period_ms(20);
Mecaru 3:46ea1b20397d 115 pwm_moteur.pulsewidth_us(1400); // correspond à une vitesse nulle
SolalNathan 2:b2ce001ff8f5 116 // Gaspard : 1450, Solal : 1480. Tester les deux
SolalNathan 2:b2ce001ff8f5 117
SolalNathan 2:b2ce001ff8f5 118 // pwm de la direction
SolalNathan 2:b2ce001ff8f5 119 pwm_direction.period_ms(20);
Mecaru 3:46ea1b20397d 120 pwm_direction.pulsewidth_us(1469); // correspond à un vitesse faible
SolalNathan 2:b2ce001ff8f5 121
SolalNathan 2:b2ce001ff8f5 122 // récupération du premier batch de données (7 bytes) du LIDAR
SolalNathan 2:b2ce001ff8f5 123 lidar.putc(0xA5);
SolalNathan 2:b2ce001ff8f5 124 lidar.putc(0x20);
SolalNathan 2:b2ce001ff8f5 125 for(i=0;i<7;i++)
SolalNathan 2:b2ce001ff8f5 126 lidar.getc();
SolalNathan 2:b2ce001ff8f5 127
SolalNathan 2:b2ce001ff8f5 128 pc.printf("FIN intit \n\r");
SolalNathan 2:b2ce001ff8f5 129
SolalNathan 2:b2ce001ff8f5 130 lidar.attach(&interrupt_lidar_rx, Serial::RxIrq);
SolalNathan 2:b2ce001ff8f5 131
SolalNathan 2:b2ce001ff8f5 132 while (1){
Mecaru 4:60e7e1c1d1d8 133 //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);
Mecaru 4:60e7e1c1d1d8 134 afficher_lidar(tableau_distance);
Mecaru 4:60e7e1c1d1d8 135
SolalNathan 2:b2ce001ff8f5 136 update_direction(tableau_distance, dir); // mise à jour à la direction
SolalNathan 2:b2ce001ff8f5 137 pwm_direction_value = angle_servo(dir); // calcul du pwm
SolalNathan 0:5d6051eeabfe 138
SolalNathan 2:b2ce001ff8f5 139 pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
SolalNathan 2:b2ce001ff8f5 140 }
SolalNathan 2:b2ce001ff8f5 141
SolalNathan 2:b2ce001ff8f5 142 }
SolalNathan 2:b2ce001ff8f5 143
SolalNathan 2:b2ce001ff8f5 144
SolalNathan 2:b2ce001ff8f5 145 void interrupt_lidar_rx(void)
SolalNathan 2:b2ce001ff8f5 146 {
SolalNathan 2:b2ce001ff8f5 147
SolalNathan 2:b2ce001ff8f5 148 int SEUIL = 15; // Seuil de qualité
SolalNathan 0:5d6051eeabfe 149
SolalNathan 2:b2ce001ff8f5 150 static uint8_t data[5],i=0;
SolalNathan 2:b2ce001ff8f5 151 uint16_t Quality;
SolalNathan 2:b2ce001ff8f5 152 uint16_t Angle;
SolalNathan 2:b2ce001ff8f5 153 static uint16_t Angle_old=0;
SolalNathan 2:b2ce001ff8f5 154 uint16_t Distance;
SolalNathan 2:b2ce001ff8f5 155 uint16_t Angle_d;
SolalNathan 2:b2ce001ff8f5 156 uint16_t Distance_d;
SolalNathan 2:b2ce001ff8f5 157 data[i] = lidar.getc();
SolalNathan 2:b2ce001ff8f5 158 i++;
SolalNathan 2:b2ce001ff8f5 159 if(i==5)
SolalNathan 2:b2ce001ff8f5 160 {
SolalNathan 2:b2ce001ff8f5 161 i=0;
SolalNathan 2:b2ce001ff8f5 162 Quality = data[0] & 0xFC;
SolalNathan 2:b2ce001ff8f5 163 Quality = Quality >> 2;
SolalNathan 2:b2ce001ff8f5 164
SolalNathan 2:b2ce001ff8f5 165 Angle = data[1] & 0xFE;
SolalNathan 2:b2ce001ff8f5 166 Angle = (Angle>>1) | ((uint16_t)data[2] << 7);
SolalNathan 2:b2ce001ff8f5 167
SolalNathan 2:b2ce001ff8f5 168 Distance = data[3];
SolalNathan 2:b2ce001ff8f5 169 Distance = Distance | ((uint16_t)data[4] << 8);
SolalNathan 2:b2ce001ff8f5 170
SolalNathan 2:b2ce001ff8f5 171 Angle_d = Angle/64; // in degree
SolalNathan 2:b2ce001ff8f5 172 Distance_d = Distance>>2; // in mm
SolalNathan 2:b2ce001ff8f5 173
SolalNathan 2:b2ce001ff8f5 174 // On vérifie que l'on écrit pas en dehors du tableau
SolalNathan 2:b2ce001ff8f5 175 if(Angle_d>359) Angle_d=359;
SolalNathan 2:b2ce001ff8f5 176 if(Angle_d<0) Angle_d=0;
SolalNathan 2:b2ce001ff8f5 177
SolalNathan 2:b2ce001ff8f5 178 if (Quality < SEUIL) {
SolalNathan 2:b2ce001ff8f5 179 // Fiabilisation des données du LIDAR naïve
SolalNathan 2:b2ce001ff8f5 180 tableau_distance[Angle_d] = tableau_distance[Angle_d - 1];
SolalNathan 2:b2ce001ff8f5 181 }
SolalNathan 2:b2ce001ff8f5 182 else
SolalNathan 2:b2ce001ff8f5 183 tableau_distance[Angle_d] = Distance_d;
SolalNathan 2:b2ce001ff8f5 184
SolalNathan 2:b2ce001ff8f5 185 }
SolalNathan 2:b2ce001ff8f5 186 }