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

Direction.cpp@18:daba0b3777c0, 2019-06-07 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SolalNathan 18:daba0b3777c0 1 #include "SaphTeamRacing.h"
SolalNathan 18:daba0b3777c0 2
SolalNathan 18:daba0b3777c0 3 void points_fictifs(uint8_t* liste_fictifs, int zero_min, int zero_max,
SolalNathan 18:daba0b3777c0 4 int one_min, int one_max){
SolalNathan 18:daba0b3777c0 5 /*
SolalNathan 18:daba0b3777c0 6 Fonction définissant les points fictifs poussant la voiture.
SolalNathan 18:daba0b3777c0 7 zero_min, zero_max, one_min et one_max doivent êtres compris entre 0
SolalNathan 18:daba0b3777c0 8 et 360°. Il faut zero_min < zero_max et one_min < one_max.
SolalNathan 18:daba0b3777c0 9 */
SolalNathan 18:daba0b3777c0 10 for (int i = zero_min; i < zero_max; i++) {
SolalNathan 18:daba0b3777c0 11 liste_fictifs[i] = 0;
SolalNathan 18:daba0b3777c0 12 }
SolalNathan 18:daba0b3777c0 13 for (int i = one_max; i < one_max; i++) {
SolalNathan 18:daba0b3777c0 14 liste_fictifs[i] = 1;
SolalNathan 18:daba0b3777c0 15 }
SolalNathan 18:daba0b3777c0 16 }
SolalNathan 18:daba0b3777c0 17
SolalNathan 18:daba0b3777c0 18 void update_direction(float* list_lidar_var, float* vecteur)
SolalNathan 18:daba0b3777c0 19 {
SolalNathan 18:daba0b3777c0 20 /*
SolalNathan 18:daba0b3777c0 21 Fonction de mise à jour de la direction grâce à l'algorithme des charges
SolalNathan 18:daba0b3777c0 22 fictives.
SolalNathan 18:daba0b3777c0 23 */
SolalNathan 18:daba0b3777c0 24
SolalNathan 18:daba0b3777c0 25 float direction[2], list_lidar[360];
SolalNathan 18:daba0b3777c0 26 uint8_t liste_fictifs[360];
SolalNathan 18:daba0b3777c0 27 int i;
SolalNathan 18:daba0b3777c0 28 for(i=0; i<360; i++) list_lidar[i]=list_lidar_var[i];
SolalNathan 18:daba0b3777c0 29
SolalNathan 18:daba0b3777c0 30 direction[0] = 1;
SolalNathan 18:daba0b3777c0 31 direction[1] = 0;
SolalNathan 18:daba0b3777c0 32 float avg_x = 0;
SolalNathan 18:daba0b3777c0 33 float avg_y = 0;
SolalNathan 18:daba0b3777c0 34
SolalNathan 18:daba0b3777c0 35 points_fictifs(liste_fictifs, 0, 360, 90, 270);
SolalNathan 18:daba0b3777c0 36
SolalNathan 18:daba0b3777c0 37 // Calcul de la direction à prende en fonction des charges fictives
SolalNathan 18:daba0b3777c0 38 for (i=0; i<360; i++) {
SolalNathan 18:daba0b3777c0 39 int theta = i;
SolalNathan 18:daba0b3777c0 40 float r;
SolalNathan 18:daba0b3777c0 41 double x, y;
SolalNathan 18:daba0b3777c0 42 if (liste_fictifs[theta] == 1) {
SolalNathan 18:daba0b3777c0 43 r = 500;
SolalNathan 18:daba0b3777c0 44 } else {
SolalNathan 18:daba0b3777c0 45 //r = 0; //test
SolalNathan 18:daba0b3777c0 46 if(theta != 0) r = list_lidar[360-theta];
SolalNathan 18:daba0b3777c0 47 else r = list_lidar[0];
SolalNathan 18:daba0b3777c0 48
SolalNathan 18:daba0b3777c0 49 }
SolalNathan 18:daba0b3777c0 50
SolalNathan 18:daba0b3777c0 51 if (r > 0) {
SolalNathan 18:daba0b3777c0 52 double theta_d = theta*3.141592/180;
SolalNathan 18:daba0b3777c0 53 x = r*cosf(theta_d);
SolalNathan 18:daba0b3777c0 54 y = r*sinf(theta_d);
SolalNathan 18:daba0b3777c0 55 double puissance = 0.5*(double)cosf(2*theta_d) + 1.5;
SolalNathan 18:daba0b3777c0 56 double r_pow = pow((double) r, (double) puissance);
SolalNathan 18:daba0b3777c0 57 avg_x = avg_x - x/r_pow;
SolalNathan 18:daba0b3777c0 58 avg_y = avg_y - y/r_pow;
SolalNathan 18:daba0b3777c0 59
SolalNathan 18:daba0b3777c0 60 }
SolalNathan 18:daba0b3777c0 61 }
SolalNathan 18:daba0b3777c0 62
SolalNathan 18:daba0b3777c0 63 direction[0] = avg_x;
SolalNathan 18:daba0b3777c0 64 direction[1] = avg_y;
SolalNathan 18:daba0b3777c0 65
SolalNathan 18:daba0b3777c0 66 // mise à jour de la direction
SolalNathan 18:daba0b3777c0 67 for(i=0; i<2; i++)
SolalNathan 18:daba0b3777c0 68 vecteur[i] = direction[i];
SolalNathan 18:daba0b3777c0 69 }