Paclay-Saris pod racers / Mbed 2 deprecated Algo_charges_fictives_3

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@10:c8d93dc5993c, 2019-06-05 (annotated)

Committer:
Mecaru
Date:
Wed Jun 05 12:29:09 2019 +0000
Revision:
10:c8d93dc5993c
Parent:
9:a15fc52284ff
Child:
11:e227edfced99
Bol qui pousse la voiture derriere (implementation correcte a priori mais fonctionnement pas ouf)

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