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@0:5d6051eeabfe, 2019-05-10 (annotated)

Committer:
SolalNathan
Date:
Fri May 10 09:08:12 2019 +0000
Revision:
0:5d6051eeabfe
Child:
1:e4b5a39729d2
bug avec cos et sin

Who changed what in which revision?

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