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

Committer:
Mecaru
Date:
Thu Jun 06 10:25:41 2019 +0000
Revision:
11:e227edfced99
Parent:
10:c8d93dc5993c
Child:
12:594a1b936f4b
version 6 juin matin

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