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@13:bed77b03701d, 2019-06-06 (annotated)

Committer:
Mecaru
Date:
Thu Jun 06 12:19:16 2019 +0000
Revision:
13:bed77b03701d
Parent:
12:594a1b936f4b
Child:
14:e1e393537fb6
avec blue

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 13:bed77b03701d 5 //Serial pc(USBTX, USBRX, 115200);
Mecaru 13:bed77b03701d 6 Serial pc(PC_10, PC_11, 115200);
SolalNathan 2:b2ce001ff8f5 7 Serial lidar(PC_6, PC_7, 115200);
SolalNathan 2:b2ce001ff8f5 8
SolalNathan 2:b2ce001ff8f5 9 // Définition des variables globales
Mecaru 12:594a1b936f4b 10 float tableau_distance0[360] = {};
Mecaru 12:594a1b936f4b 11 float tableau_distance1[360] = {};
Mecaru 12:594a1b936f4b 12 uint8_t tableau_en_cours = 0; //tableau en cours de remplissage
Mecaru 12:594a1b936f4b 13 uint8_t flag_fin_tour_lidar=0;
SolalNathan 2:b2ce001ff8f5 14 int compteur_tours_lidar = 0;
Mecaru 6:83dafe088914 15 int affiche_lidar = 0;
Mecaru 11:e227edfced99 16 bool run = false;
SolalNathan 2:b2ce001ff8f5 17
SolalNathan 2:b2ce001ff8f5 18 // Défintion des pwm
SolalNathan 2:b2ce001ff8f5 19 PwmOut pwm_lidar(PB_15); // pwm du Lidar
SolalNathan 2:b2ce001ff8f5 20 PwmOut pwm_moteur(PE_6); // pwm de la propulsion
SolalNathan 2:b2ce001ff8f5 21 PwmOut pwm_direction(PE_5); // pwm de la direction
SolalNathan 2:b2ce001ff8f5 22
SolalNathan 2:b2ce001ff8f5 23 void interrupt_lidar_rx(void);
SolalNathan 0:5d6051eeabfe 24
Mecaru 4:60e7e1c1d1d8 25
SolalNathan 2:b2ce001ff8f5 26 float distance(float x_1, float x_2, float y_1, float y_2)
SolalNathan 2:b2ce001ff8f5 27 {
SolalNathan 2:b2ce001ff8f5 28 // Fonction qui renvoie la distance entre deux points (norme 2)
Mecaru 12:594a1b936f4b 29 float norm2;
SolalNathan 2:b2ce001ff8f5 30 norm2 = sqrt((x_1 - x_2)*(x_1 - x_2) + (y_1 - y_2)*(y_1 - y_2));
SolalNathan 2:b2ce001ff8f5 31 return norm2;
SolalNathan 2:b2ce001ff8f5 32 }
Mecaru 12:594a1b936f4b 33
Mecaru 12:594a1b936f4b 34 void update_direction(float* list_lidar_var, float* vecteur)
SolalNathan 2:b2ce001ff8f5 35 {
Mecaru 11:e227edfced99 36 //pc.printf("Update commence\n\r");
SolalNathan 2:b2ce001ff8f5 37 // Fonction de mise à jour de la direction
SolalNathan 2:b2ce001ff8f5 38 float direction[2];
Mecaru 12:594a1b936f4b 39 int i;
Mecaru 12:594a1b936f4b 40 float list_lidar[360];
Mecaru 12:594a1b936f4b 41 uint8_t liste_fictifs[360];
Mecaru 12:594a1b936f4b 42 for(i=0; i<360; i++) list_lidar[i]=list_lidar_var[i];
Mecaru 12:594a1b936f4b 43
Mecaru 13:bed77b03701d 44 //pour les essais
Mecaru 13:bed77b03701d 45 //for(i=0; i<360; i++)
Mecaru 13:bed77b03701d 46 // list_lidar[i]=00;
Mecaru 13:bed77b03701d 47
Mecaru 13:bed77b03701d 48 //list_lidar[0]=100;
Mecaru 13:bed77b03701d 49 //list_lidar[90]=20.0;
Mecaru 13:bed77b03701d 50 //list_lidar[270]=100.0;
Mecaru 13:bed77b03701d 51 //list_lidar[45]=20.0;
Mecaru 13:bed77b03701d 52 //list_lidar[315]=100;
Mecaru 13:bed77b03701d 53 ///////////////////
Mecaru 13:bed77b03701d 54
Mecaru 12:594a1b936f4b 55
Mecaru 12:594a1b936f4b 56 direction[0] = 1;
Mecaru 12:594a1b936f4b 57 direction[1] = 0;
SolalNathan 2:b2ce001ff8f5 58 float avg_x, avg_y, sum_inv_dist;
Mecaru 9:a15fc52284ff 59 //list_lidar[180] = 50; // [mm], point fictif qui pousse la voiture
Mecaru 10:c8d93dc5993c 60 //Définition des points fictifs poussant la voiture
Mecaru 12:594a1b936f4b 61
Mecaru 12:594a1b936f4b 62 for (i=0; i<360; i++) {
Mecaru 9:a15fc52284ff 63 liste_fictifs[i] = 0;
Mecaru 12:594a1b936f4b 64 }
Mecaru 12:594a1b936f4b 65 // for (i=135; i<=225; i++) {
Mecaru 12:594a1b936f4b 66 // //for (int i=0; i<180; i++){ //test
Mecaru 12:594a1b936f4b 67 // liste_fictifs[i] = 1;
Mecaru 12:594a1b936f4b 68 // }
Mecaru 12:594a1b936f4b 69 liste_fictifs[180] = 1;
Mecaru 11:e227edfced99 70
SolalNathan 2:b2ce001ff8f5 71 avg_x = 0;
SolalNathan 2:b2ce001ff8f5 72 avg_y = 0;
Mecaru 12:594a1b936f4b 73
SolalNathan 2:b2ce001ff8f5 74 // Calcul de la direction à prende en fonction des charges fictives
Mecaru 12:594a1b936f4b 75 for (i=0; i<360; i++) {
SolalNathan 2:b2ce001ff8f5 76 int theta;
SolalNathan 2:b2ce001ff8f5 77 float r, x, y;
SolalNathan 2:b2ce001ff8f5 78 theta = i;
Mecaru 12:594a1b936f4b 79 if (liste_fictifs[theta] == 1) {
Mecaru 11:e227edfced99 80 r = 50;
Mecaru 12:594a1b936f4b 81 } else {
Mecaru 10:c8d93dc5993c 82 //r = 0; //test
Mecaru 12:594a1b936f4b 83 if(theta != 0) r = list_lidar[360-theta];
Mecaru 12:594a1b936f4b 84 else r = list_lidar[0];
Mecaru 12:594a1b936f4b 85
Mecaru 9:a15fc52284ff 86 }
Mecaru 12:594a1b936f4b 87
Mecaru 12:594a1b936f4b 88 if (r != 0) {
Mecaru 12:594a1b936f4b 89
Mecaru 12:594a1b936f4b 90 //x = 0;
Mecaru 12:594a1b936f4b 91 //y = 0;
Mecaru 12:594a1b936f4b 92 x = r*cosf((float)theta*3.14/180);
Mecaru 12:594a1b936f4b 93 y = r*sinf((float)theta*3.14/180);
Mecaru 12:594a1b936f4b 94 //sum_inv_dist += 1/pow(r, 2);
Mecaru 12:594a1b936f4b 95 //avg_x -= x/pow(r,2);
Mecaru 12:594a1b936f4b 96 //avg_y -= y/pow(r,2);
Mecaru 12:594a1b936f4b 97 float puissance = 2;//0.5*abs(cosf(2*theta)) + 1.5;
Mecaru 12:594a1b936f4b 98 avg_x = avg_x - x/pow(r,puissance);
Mecaru 12:594a1b936f4b 99 avg_y = avg_y - y/pow(r,puissance);
Mecaru 12:594a1b936f4b 100
Mecaru 12:594a1b936f4b 101 //pc.printf("Angle:%i r:%4.2f x:%4.2f y:%4.2f %4.2f %4.2f\n\r", theta, r, x,y, avg_x,avg_y);
SolalNathan 2:b2ce001ff8f5 102 }
Mecaru 12:594a1b936f4b 103 }
Mecaru 12:594a1b936f4b 104
Mecaru 8:2ce9493549e8 105 //avg_x /= sum_inv_dist;
Mecaru 8:2ce9493549e8 106 //avg_y /= sum_inv_dist;
SolalNathan 2:b2ce001ff8f5 107 direction[0] = avg_x;
SolalNathan 2:b2ce001ff8f5 108 direction[1] = avg_y;
Mecaru 11:e227edfced99 109 //pc.printf("Update termine\n\r");
SolalNathan 2:b2ce001ff8f5 110 // mise à jour de la direction
SolalNathan 2:b2ce001ff8f5 111 for(i=0; i<2; i++)
SolalNathan 2:b2ce001ff8f5 112 vecteur[i] = direction[i];
Mecaru 11:e227edfced99 113
SolalNathan 2:b2ce001ff8f5 114 }
Mecaru 12:594a1b936f4b 115
SolalNathan 2:b2ce001ff8f5 116 float angle_servo(float *direction)
SolalNathan 2:b2ce001ff8f5 117 {
SolalNathan 2:b2ce001ff8f5 118 // Calcul basé sur la régression expérimental pour obetenir l'angle
SolalNathan 2:b2ce001ff8f5 119 // le pwm à donner au moteur en fonction de l'angle voulue
Mecaru 12:594a1b936f4b 120
SolalNathan 2:b2ce001ff8f5 121 float angle;
SolalNathan 2:b2ce001ff8f5 122 double pwm;
SolalNathan 2:b2ce001ff8f5 123 float x, y;
SolalNathan 2:b2ce001ff8f5 124 x = direction[0];
SolalNathan 2:b2ce001ff8f5 125 y = direction[1];
SolalNathan 2:b2ce001ff8f5 126 angle = atan(x/y);
Mecaru 3:46ea1b20397d 127 pwm = 14.662756 * angle*180/3.14 + 1453.08; // à refaire
Mecaru 12:594a1b936f4b 128
Mecaru 6:83dafe088914 129 //if (pwm < 1115) printf("trop petit\n\r");
Mecaru 6:83dafe088914 130 //if (pwm > 1625) printf("trop grand\n\r");
Mecaru 9:a15fc52284ff 131 //if (angle > 5*3.14/180){
Mecaru 9:a15fc52284ff 132 // pwm = 1745;
Mecaru 9:a15fc52284ff 133 //}
Mecaru 9:a15fc52284ff 134 //else{
Mecaru 9:a15fc52284ff 135 // if (angle < -5*3.14/180){
Mecaru 9:a15fc52284ff 136 // pwm = 1080;
Mecaru 9:a15fc52284ff 137 // }
Mecaru 9:a15fc52284ff 138 // else{
Mecaru 9:a15fc52284ff 139 // pwm = 1453;
Mecaru 9:a15fc52284ff 140 // }
Mecaru 9:a15fc52284ff 141 //}
Mecaru 12:594a1b936f4b 142
SolalNathan 2:b2ce001ff8f5 143 return pwm;
SolalNathan 2:b2ce001ff8f5 144 }
SolalNathan 0:5d6051eeabfe 145
Mecaru 12:594a1b936f4b 146 void afficher_lidar(float *tableau_distances_var)
Mecaru 4:60e7e1c1d1d8 147 {
Mecaru 4:60e7e1c1d1d8 148 //Affiche les données du lidar dans la liaison série
Mecaru 4:60e7e1c1d1d8 149 int angle;
Mecaru 12:594a1b936f4b 150 float tableau_distances[360];
Mecaru 12:594a1b936f4b 151 for(angle=0; angle<360; angle++) tableau_distances[angle]=tableau_distances_var[angle];
Mecaru 12:594a1b936f4b 152
Mecaru 12:594a1b936f4b 153 for(angle=0; angle<360; angle++) {
Mecaru 7:dc7e66870bd0 154 float distance = tableau_distances[angle];
Mecaru 7:dc7e66870bd0 155 pc.printf("%i,%f\n\r",angle,distance);
Mecaru 12:594a1b936f4b 156 }
Mecaru 4:60e7e1c1d1d8 157 }
Mecaru 4:60e7e1c1d1d8 158
Mecaru 12:594a1b936f4b 159 int main()
Mecaru 12:594a1b936f4b 160 {
Mecaru 12:594a1b936f4b 161
SolalNathan 2:b2ce001ff8f5 162 pc.printf("\r-------------------------\n\r");
Mecaru 12:594a1b936f4b 163
SolalNathan 2:b2ce001ff8f5 164 float dir[2]; // direction
SolalNathan 2:b2ce001ff8f5 165 float pwm_direction_value;
Mecaru 12:594a1b936f4b 166
Mecaru 12:594a1b936f4b 167
SolalNathan 2:b2ce001ff8f5 168 int i;
Mecaru 12:594a1b936f4b 169
Mecaru 7:dc7e66870bd0 170
Mecaru 7:dc7e66870bd0 171
Mecaru 7:dc7e66870bd0 172
Mecaru 7:dc7e66870bd0 173
SolalNathan 2:b2ce001ff8f5 174 // pwm du LIDAR
SolalNathan 2:b2ce001ff8f5 175 pwm_lidar.period_us(40);
Mecaru 5:32434b497a9b 176 pwm_lidar.pulsewidth_us(40); // vitesse fixe
Mecaru 12:594a1b936f4b 177
Mecaru 11:e227edfced99 178 //pwm moteur
SolalNathan 0:5d6051eeabfe 179 pwm_moteur.period_ms(20);
SolalNathan 2:b2ce001ff8f5 180
SolalNathan 2:b2ce001ff8f5 181 // pwm de la direction
SolalNathan 2:b2ce001ff8f5 182 pwm_direction.period_ms(20);
Mecaru 6:83dafe088914 183 pwm_direction.pulsewidth_us(1480); // correspond à un vitesse faible
SolalNathan 2:b2ce001ff8f5 184
SolalNathan 2:b2ce001ff8f5 185 // récupération du premier batch de données (7 bytes) du LIDAR
SolalNathan 2:b2ce001ff8f5 186 lidar.putc(0xA5);
SolalNathan 2:b2ce001ff8f5 187 lidar.putc(0x20);
Mecaru 12:594a1b936f4b 188 for(i=0; i<7; i++)
SolalNathan 2:b2ce001ff8f5 189 lidar.getc();
SolalNathan 2:b2ce001ff8f5 190
SolalNathan 2:b2ce001ff8f5 191 pc.printf("FIN intit \n\r");
SolalNathan 2:b2ce001ff8f5 192
Mecaru 12:594a1b936f4b 193 lidar.attach(&interrupt_lidar_rx, Serial::RxIrq);
Mecaru 12:594a1b936f4b 194
Mecaru 12:594a1b936f4b 195 while (1) {
Mecaru 12:594a1b936f4b 196 //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);
Mecaru 12:594a1b936f4b 197
Mecaru 12:594a1b936f4b 198 if(pc.readable()) {
Mecaru 12:594a1b936f4b 199 char entree = pc.getc();
Mecaru 12:594a1b936f4b 200 pc.printf("%c \n\r",entree);
Mecaru 12:594a1b936f4b 201 if (entree == 'a') {
Mecaru 12:594a1b936f4b 202 run = true;
Mecaru 12:594a1b936f4b 203 }
Mecaru 12:594a1b936f4b 204 if (entree == 'z') {
Mecaru 12:594a1b936f4b 205 run = false;
Mecaru 12:594a1b936f4b 206 }
Mecaru 11:e227edfced99 207 }
Mecaru 12:594a1b936f4b 208
Mecaru 12:594a1b936f4b 209
Mecaru 13:bed77b03701d 210 if(0) {
Mecaru 12:594a1b936f4b 211 if(tableau_en_cours == 0)
Mecaru 12:594a1b936f4b 212 afficher_lidar(tableau_distance1);
Mecaru 12:594a1b936f4b 213 else afficher_lidar(tableau_distance0);
Mecaru 12:594a1b936f4b 214
Mecaru 12:594a1b936f4b 215 affiche_lidar = 0;
Mecaru 11:e227edfced99 216 }
Mecaru 12:594a1b936f4b 217
Mecaru 12:594a1b936f4b 218 if(flag_fin_tour_lidar==1) {
Mecaru 12:594a1b936f4b 219 flag_fin_tour_lidar=0;
Mecaru 12:594a1b936f4b 220 if(tableau_en_cours == 0)
Mecaru 12:594a1b936f4b 221 update_direction(tableau_distance1, dir); // mise à jour à la direction
Mecaru 12:594a1b936f4b 222 else update_direction(tableau_distance0, dir); // mise à jour à la direction
Mecaru 12:594a1b936f4b 223 pc.printf("direction,%f,%f\n\r",dir[0],dir[1]);
Mecaru 12:594a1b936f4b 224 pwm_direction_value = angle_servo(dir); // calcul du pwm
Mecaru 12:594a1b936f4b 225 }
Mecaru 12:594a1b936f4b 226
Mecaru 12:594a1b936f4b 227 if (run == true) {
Mecaru 12:594a1b936f4b 228 // vitesse constante
Mecaru 12:594a1b936f4b 229 pwm_moteur.pulsewidth_us(1440);
Mecaru 12:594a1b936f4b 230 pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
Mecaru 12:594a1b936f4b 231 } else {
Mecaru 12:594a1b936f4b 232 pwm_moteur.pulsewidth_us(1480);
Mecaru 12:594a1b936f4b 233 }
Mecaru 12:594a1b936f4b 234
Mecaru 11:e227edfced99 235 }
Mecaru 11:e227edfced99 236 }
SolalNathan 2:b2ce001ff8f5 237
SolalNathan 2:b2ce001ff8f5 238 void interrupt_lidar_rx(void)
Mecaru 12:594a1b936f4b 239 {
SolalNathan 2:b2ce001ff8f5 240
Mecaru 5:32434b497a9b 241 int SEUIL = 0; // Seuil de qualité
Mecaru 12:594a1b936f4b 242
SolalNathan 2:b2ce001ff8f5 243 static uint8_t data[5],i=0;
SolalNathan 2:b2ce001ff8f5 244 uint16_t Quality;
SolalNathan 2:b2ce001ff8f5 245 uint16_t Angle;
SolalNathan 2:b2ce001ff8f5 246 uint16_t Distance;
SolalNathan 2:b2ce001ff8f5 247 uint16_t Angle_d;
Mecaru 12:594a1b936f4b 248 static uint16_t Angle_d_old=0;
SolalNathan 2:b2ce001ff8f5 249 uint16_t Distance_d;
Mecaru 6:83dafe088914 250 affiche_lidar ++;
SolalNathan 2:b2ce001ff8f5 251 data[i] = lidar.getc();
SolalNathan 2:b2ce001ff8f5 252 i++;
Mecaru 12:594a1b936f4b 253 if(i==5) {
SolalNathan 2:b2ce001ff8f5 254 i=0;
SolalNathan 2:b2ce001ff8f5 255 Quality = data[0] & 0xFC;
SolalNathan 2:b2ce001ff8f5 256 Quality = Quality >> 2;
Mecaru 12:594a1b936f4b 257
SolalNathan 2:b2ce001ff8f5 258 Angle = data[1] & 0xFE;
SolalNathan 2:b2ce001ff8f5 259 Angle = (Angle>>1) | ((uint16_t)data[2] << 7);
Mecaru 12:594a1b936f4b 260
SolalNathan 2:b2ce001ff8f5 261 Distance = data[3];
SolalNathan 2:b2ce001ff8f5 262 Distance = Distance | ((uint16_t)data[4] << 8);
Mecaru 12:594a1b936f4b 263
SolalNathan 2:b2ce001ff8f5 264 Angle_d = Angle/64; // in degree
SolalNathan 2:b2ce001ff8f5 265 Distance_d = Distance>>2; // in mm
SolalNathan 2:b2ce001ff8f5 266
Mecaru 12:594a1b936f4b 267 // On vérifie que l'on écrit pas en dehors du tableau
Mecaru 8:2ce9493549e8 268 //Angle_d = 360 - Angle_d;
SolalNathan 2:b2ce001ff8f5 269 if(Angle_d>359) Angle_d=359;
Mecaru 12:594a1b936f4b 270
Mecaru 12:594a1b936f4b 271 if(Angle < (Angle_d_old - 180)) {
Mecaru 12:594a1b936f4b 272 tableau_en_cours=1-tableau_en_cours;
Mecaru 12:594a1b936f4b 273 flag_fin_tour_lidar=1;
Mecaru 12:594a1b936f4b 274 }
Mecaru 12:594a1b936f4b 275
SolalNathan 2:b2ce001ff8f5 276 if (Quality < SEUIL) {
SolalNathan 2:b2ce001ff8f5 277 // Fiabilisation des données du LIDAR naïve
Mecaru 12:594a1b936f4b 278 if(tableau_en_cours==0)
Mecaru 12:594a1b936f4b 279 tableau_distance0[Angle_d] = tableau_distance0[Angle_d_old];
Mecaru 12:594a1b936f4b 280 else tableau_distance1[Angle_d] = tableau_distance1[Angle_d_old];
Mecaru 12:594a1b936f4b 281 } else if(tableau_en_cours==0)
Mecaru 12:594a1b936f4b 282 tableau_distance0[Angle_d] = Distance_d;
Mecaru 12:594a1b936f4b 283 else tableau_distance1[Angle_d] = Distance_d;
Mecaru 12:594a1b936f4b 284
Mecaru 12:594a1b936f4b 285 Angle_d_old = Angle;
Mecaru 7:dc7e66870bd0 286
Mecaru 7:dc7e66870bd0 287 //tableau_distance[Angle_d] = Distance_d;
SolalNathan 2:b2ce001ff8f5 288 }
SolalNathan 2:b2ce001ff8f5 289 }