Benoît Roussel / Mbed 2 deprecated test_algo

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

algorithme.h@0:9eb40ee5ff41, 2019-05-31 (annotated)

Committer:
Mrlinkblue
Date:
Fri May 31 09:41:29 2019 +0000
Revision:
0:9eb40ee5ff41
Child:
3:97827746c632
Version avant projet libre;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mrlinkblue 0:9eb40ee5ff41 1 #include "mbed.h"
Mrlinkblue 0:9eb40ee5ff41 2 #include <vector>
Mrlinkblue 0:9eb40ee5ff41 3 #include <algorithm> // std::min_element, std::max_element
Mrlinkblue 0:9eb40ee5ff41 4
Mrlinkblue 0:9eb40ee5ff41 5 #define pi 3.14159265358979323846
Mrlinkblue 0:9eb40ee5ff41 6
Mrlinkblue 0:9eb40ee5ff41 7 using namespace std;
Mrlinkblue 0:9eb40ee5ff41 8
Mrlinkblue 0:9eb40ee5ff41 9 // Liste d'angles/distances
Mrlinkblue 0:9eb40ee5ff41 10 vector <float> data_distances(360);
Mrlinkblue 0:9eb40ee5ff41 11 vector <float> data_angles(360);
Mrlinkblue 0:9eb40ee5ff41 12
Mrlinkblue 0:9eb40ee5ff41 13 // Paramètre pour la fonction actualisation (sorties)
Mrlinkblue 0:9eb40ee5ff41 14 float angle_suivre;
Mrlinkblue 0:9eb40ee5ff41 15 float vitesse_suivre=0;
Mrlinkblue 0:9eb40ee5ff41 16
Mrlinkblue 0:9eb40ee5ff41 17 // Paramètres de la voiture "fixe" mais à varier pour optimiser
Mrlinkblue 0:9eb40ee5ff41 18 float largeur_voiture=0.3; // (m) distance de sécurité comprise dedans
Mrlinkblue 0:9eb40ee5ff41 19 float vitesse_max=1.5; // (m/s)
Mrlinkblue 0:9eb40ee5ff41 20 float angle_max = 20.0; // (degrees)
Mrlinkblue 0:9eb40ee5ff41 21 float lidar_dmax = 5; // (m)
Mrlinkblue 0:9eb40ee5ff41 22 //float distance_securite=0.5; // A VOIR -> Inutile en faite
Mrlinkblue 0:9eb40ee5ff41 23 float tau_prog = 0.050; // (s)
Mrlinkblue 0:9eb40ee5ff41 24 //float lidar_frequence = 10.0; // (Hz)
Mrlinkblue 0:9eb40ee5ff41 25 float rot_limite=(pi/2)*(180/pi)/0.5; // (deg/s)
Mrlinkblue 0:9eb40ee5ff41 26 float acc_limite=3; // (m/s2)
Mrlinkblue 0:9eb40ee5ff41 27
Mrlinkblue 0:9eb40ee5ff41 28
Mrlinkblue 0:9eb40ee5ff41 29 // Paramètres de la voiture variable
Mrlinkblue 0:9eb40ee5ff41 30 float voiture_angle = 0.0; // self.angle (angle du repère)
Mrlinkblue 0:9eb40ee5ff41 31 float voiture_angle_actuel = 0.0; // self.angleroues
Mrlinkblue 0:9eb40ee5ff41 32
Mrlinkblue 0:9eb40ee5ff41 33 float facteur_angle = 0.0;
Mrlinkblue 0:9eb40ee5ff41 34 float facteur_distance = 0.0;
Mrlinkblue 0:9eb40ee5ff41 35
Mrlinkblue 0:9eb40ee5ff41 36 float voiture_vitesse = 0.0;
Mrlinkblue 0:9eb40ee5ff41 37
Mrlinkblue 0:9eb40ee5ff41 38 // Paramètres internes aux fonction
Mrlinkblue 0:9eb40ee5ff41 39 float angleSuivreSol;
Mrlinkblue 0:9eb40ee5ff41 40 float C;
Mrlinkblue 0:9eb40ee5ff41 41 bool compensation;
Mrlinkblue 0:9eb40ee5ff41 42 float facteur_vitesse = pi*pi/(log(vitesse_max/0.5));
Mrlinkblue 0:9eb40ee5ff41 43
Mrlinkblue 0:9eb40ee5ff41 44
Mrlinkblue 0:9eb40ee5ff41 45 float min(float a, float b){
Mrlinkblue 0:9eb40ee5ff41 46 // Fonction permettant de déterminer le minimum entre a et b
Mrlinkblue 0:9eb40ee5ff41 47 float c;
Mrlinkblue 0:9eb40ee5ff41 48 if (a>b){
Mrlinkblue 0:9eb40ee5ff41 49 c=b;
Mrlinkblue 0:9eb40ee5ff41 50 }
Mrlinkblue 0:9eb40ee5ff41 51 else{
Mrlinkblue 0:9eb40ee5ff41 52 c=a;
Mrlinkblue 0:9eb40ee5ff41 53 }
Mrlinkblue 0:9eb40ee5ff41 54 return c;
Mrlinkblue 0:9eb40ee5ff41 55 }
Mrlinkblue 0:9eb40ee5ff41 56
Mrlinkblue 0:9eb40ee5ff41 57 float min_vect(vector <float> vect){
Mrlinkblue 0:9eb40ee5ff41 58 // Fonction permettant de déterminer le minimum d'un vecteur
Mrlinkblue 0:9eb40ee5ff41 59 float vmin = 0;
Mrlinkblue 0:9eb40ee5ff41 60 for (int i=0; i<vect.size(); i++){
Mrlinkblue 0:9eb40ee5ff41 61 if (vect[i]<vmin){
Mrlinkblue 0:9eb40ee5ff41 62 vmin=vect[i];
Mrlinkblue 0:9eb40ee5ff41 63 }
Mrlinkblue 0:9eb40ee5ff41 64 }
Mrlinkblue 0:9eb40ee5ff41 65 return vmin;
Mrlinkblue 0:9eb40ee5ff41 66 }
Mrlinkblue 0:9eb40ee5ff41 67
Mrlinkblue 0:9eb40ee5ff41 68 vector <vector <float> > rechercheSauts(){
Mrlinkblue 0:9eb40ee5ff41 69 // Fonction permettant de rechercher un saut dans une série de données
Mrlinkblue 0:9eb40ee5ff41 70 // d'angles et de distances
Mrlinkblue 0:9eb40ee5ff41 71 float deriv;
Mrlinkblue 0:9eb40ee5ff41 72 vector <vector <float> > sauts(0);
Mrlinkblue 0:9eb40ee5ff41 73 vector <float> saut(3); // Triplet d'un angle, saut et de son indice i
Mrlinkblue 0:9eb40ee5ff41 74 int consecutive = 0;
Mrlinkblue 0:9eb40ee5ff41 75
Mrlinkblue 0:9eb40ee5ff41 76 //On cherche tout les sauts de dérivés à l'avant de la voiture
Mrlinkblue 0:9eb40ee5ff41 77 for (int i=0; i<180;i++){
Mrlinkblue 0:9eb40ee5ff41 78 deriv = (data_distances[i+1] - data_distances[i])/(data_angles[i+1]-data_angles[i]);
Mrlinkblue 0:9eb40ee5ff41 79 if (deriv == 0 and data_distances[i] >= lidar_dmax){ //Plateau
Mrlinkblue 0:9eb40ee5ff41 80 //début de plateau
Mrlinkblue 0:9eb40ee5ff41 81 if (consecutive > 0){
Mrlinkblue 0:9eb40ee5ff41 82 consecutive += 1;
Mrlinkblue 0:9eb40ee5ff41 83 }
Mrlinkblue 0:9eb40ee5ff41 84 else if (consecutive > 0){ //Fin de plateau
Mrlinkblue 0:9eb40ee5ff41 85 saut[0] = data_angles[i - int(consecutive)/2];
Mrlinkblue 0:9eb40ee5ff41 86 saut[1] = lidar_dmax/(data_angles[1]-data_angles[0]);
Mrlinkblue 0:9eb40ee5ff41 87 saut[2] = i - int(consecutive)/2;
Mrlinkblue 0:9eb40ee5ff41 88 sauts.push_back(saut);
Mrlinkblue 0:9eb40ee5ff41 89 consecutive = 0;
Mrlinkblue 0:9eb40ee5ff41 90 }
Mrlinkblue 0:9eb40ee5ff41 91 }
Mrlinkblue 0:9eb40ee5ff41 92 if (deriv != 0){ //Saut de dérivé ou plateau
Mrlinkblue 0:9eb40ee5ff41 93 saut[0] = data_angles[i];
Mrlinkblue 0:9eb40ee5ff41 94 saut[1] = deriv;
Mrlinkblue 0:9eb40ee5ff41 95 saut[2] = i;
Mrlinkblue 0:9eb40ee5ff41 96 sauts.push_back(saut);
Mrlinkblue 0:9eb40ee5ff41 97 }
Mrlinkblue 0:9eb40ee5ff41 98 }
Mrlinkblue 0:9eb40ee5ff41 99 return sauts;
Mrlinkblue 0:9eb40ee5ff41 100 }
Mrlinkblue 0:9eb40ee5ff41 101
Mrlinkblue 0:9eb40ee5ff41 102 void sautsCara(vector <vector <float> > sauts){
Mrlinkblue 0:9eb40ee5ff41 103 // Cette fonction permet de définir les paramètres de la minimisation :
Mrlinkblue 0:9eb40ee5ff41 104 // As la solution théorique et les coef associé
Mrlinkblue 0:9eb40ee5ff41 105 compensation = false;
Mrlinkblue 0:9eb40ee5ff41 106 float deltaAngle;
Mrlinkblue 0:9eb40ee5ff41 107 // On cherche l'indice dans les sauts de celui que l'on va prendre (plus grand)
Mrlinkblue 0:9eb40ee5ff41 108 int indiceSauts = 0;
Mrlinkblue 0:9eb40ee5ff41 109 float saut_max = 0;
Mrlinkblue 0:9eb40ee5ff41 110 for (int i=0; i<sauts.size(); i++){
Mrlinkblue 0:9eb40ee5ff41 111 if (abs(sauts[i][1])>saut_max){
Mrlinkblue 0:9eb40ee5ff41 112 indiceSauts=i;
Mrlinkblue 0:9eb40ee5ff41 113 }
Mrlinkblue 0:9eb40ee5ff41 114 }
Mrlinkblue 0:9eb40ee5ff41 115 // On cherche un deuxième saut pour savoir si on doit faire une compensation
Mrlinkblue 0:9eb40ee5ff41 116 vector <vector <float> > v1;
Mrlinkblue 0:9eb40ee5ff41 117 vector <vector <float> > v2;
Mrlinkblue 0:9eb40ee5ff41 118 v1=vector <vector <float> >(sauts.begin()+indiceSauts+1,sauts.end()); // sauts[indice_saut+1:]
Mrlinkblue 0:9eb40ee5ff41 119 v2=vector <vector <float> >(sauts.begin(),sauts.end()-indiceSauts); // sauts[:indice_saut]
Mrlinkblue 0:9eb40ee5ff41 120 // Concaténation des deux vecteurs sans le saut
Mrlinkblue 0:9eb40ee5ff41 121 vector <vector <float> > sautsMoins ;
Mrlinkblue 0:9eb40ee5ff41 122 sautsMoins.reserve(v1.size() + v2.size()); // preallocate memory
Mrlinkblue 0:9eb40ee5ff41 123 sautsMoins.insert( sautsMoins.end(), v2.begin(), v2.end() );
Mrlinkblue 0:9eb40ee5ff41 124 sautsMoins.insert( sautsMoins.end(), v1.begin(), v1.end() );
Mrlinkblue 0:9eb40ee5ff41 125
Mrlinkblue 0:9eb40ee5ff41 126 if (sautsMoins.size()!=0){ //S'il y a un saut
Mrlinkblue 0:9eb40ee5ff41 127
Mrlinkblue 0:9eb40ee5ff41 128 int indiceSauts2=0;
Mrlinkblue 0:9eb40ee5ff41 129 float saut_max2=0;
Mrlinkblue 0:9eb40ee5ff41 130 for (int i=0; i<sautsMoins.size(); i++){
Mrlinkblue 0:9eb40ee5ff41 131 if (abs(sautsMoins[i][1]) > saut_max2){
Mrlinkblue 0:9eb40ee5ff41 132 indiceSauts2=i;
Mrlinkblue 0:9eb40ee5ff41 133 saut_max2=abs(sautsMoins[i][1]);
Mrlinkblue 0:9eb40ee5ff41 134
Mrlinkblue 0:9eb40ee5ff41 135 }
Mrlinkblue 0:9eb40ee5ff41 136 }
Mrlinkblue 0:9eb40ee5ff41 137
Mrlinkblue 0:9eb40ee5ff41 138 if (indiceSauts2 >= indiceSauts) { //Décalage d'indice
Mrlinkblue 0:9eb40ee5ff41 139 indiceSauts2+=1;
Mrlinkblue 0:9eb40ee5ff41 140 }
Mrlinkblue 0:9eb40ee5ff41 141 float distanceSaut1 = data_distances[sauts[indiceSauts][2]+1] - data_distances[sauts[indiceSauts][2]];
Mrlinkblue 0:9eb40ee5ff41 142 float distanceSaut2 = data_distances[sauts[indiceSauts2][2]+1] - data_distances[sauts[indiceSauts2][2]];
Mrlinkblue 0:9eb40ee5ff41 143 if (3*distanceSaut1/4 <= distanceSaut2) {
Mrlinkblue 0:9eb40ee5ff41 144 // On a un deuxième sauts importants donc on compense les coeef de minimisation
Mrlinkblue 0:9eb40ee5ff41 145 // Dans certains cas c'est le deuxième saut qui doit être suivi
Mrlinkblue 0:9eb40ee5ff41 146 compensation = true;
Mrlinkblue 0:9eb40ee5ff41 147 }
Mrlinkblue 0:9eb40ee5ff41 148 }
Mrlinkblue 0:9eb40ee5ff41 149 //On se ramène dans le repère général
Mrlinkblue 0:9eb40ee5ff41 150 float angleSuivreMini = voiture_angle + sauts[indiceSauts][0] - pi/2;
Mrlinkblue 0:9eb40ee5ff41 151
Mrlinkblue 0:9eb40ee5ff41 152 if (data_distances[sauts[indiceSauts][2]] == 0){ //On vérifie que l'on a pas de problemes
Mrlinkblue 0:9eb40ee5ff41 153 angleSuivreSol = angleSuivreMini + pi/2;
Mrlinkblue 0:9eb40ee5ff41 154 }
Mrlinkblue 0:9eb40ee5ff41 155 else{
Mrlinkblue 0:9eb40ee5ff41 156 //On ne veut pas passer trop près du mur
Mrlinkblue 0:9eb40ee5ff41 157 deltaAngle=min(pi/20,atan(largeur_voiture/data_distances[sauts[indiceSauts][2]]));
Mrlinkblue 0:9eb40ee5ff41 158 //On détermine le signe de la compensation (virage à droite ou gauche et repère général)
Mrlinkblue 0:9eb40ee5ff41 159 //On détermine les distances aux deux bords
Mrlinkblue 0:9eb40ee5ff41 160 int indPi2 = int(data_angles.size()/4) ;
Mrlinkblue 0:9eb40ee5ff41 161 int ind3Pi2 = int(3*data_angles.size()/4);
Mrlinkblue 0:9eb40ee5ff41 162
Mrlinkblue 0:9eb40ee5ff41 163 vector <float> v3;
Mrlinkblue 0:9eb40ee5ff41 164 vector <float> v4;
Mrlinkblue 0:9eb40ee5ff41 165 v3=vector <float>(data_distances.begin(),data_distances.end()-indPi2); // distances[:indPi2]
Mrlinkblue 0:9eb40ee5ff41 166 v4=vector <float>(data_distances.begin()+ind3Pi2,data_distances.end()); // distances[ind3Pi2:]
Mrlinkblue 0:9eb40ee5ff41 167 float distanceBordDroit1 = min_vect(v3);
Mrlinkblue 0:9eb40ee5ff41 168 float distanceBordDroit2 = min_vect(v4);
Mrlinkblue 0:9eb40ee5ff41 169 float distanceBordDroit = min(distanceBordDroit2,distanceBordDroit1);
Mrlinkblue 0:9eb40ee5ff41 170
Mrlinkblue 0:9eb40ee5ff41 171 vector <float> v5;
Mrlinkblue 0:9eb40ee5ff41 172 v5=vector <float>(data_distances.begin()+indPi2,data_distances.end()-ind3Pi2);
Mrlinkblue 0:9eb40ee5ff41 173 float distanceBordGauche = min_vect(v5);
Mrlinkblue 0:9eb40ee5ff41 174 float signeVirage;
Mrlinkblue 0:9eb40ee5ff41 175 if (distanceBordDroit > distanceBordGauche){ //Trop près de la gauche donc compensation à droite
Mrlinkblue 0:9eb40ee5ff41 176 signeVirage = -1;
Mrlinkblue 0:9eb40ee5ff41 177 }
Mrlinkblue 0:9eb40ee5ff41 178 else{
Mrlinkblue 0:9eb40ee5ff41 179 signeVirage = 1;
Mrlinkblue 0:9eb40ee5ff41 180 }
Mrlinkblue 0:9eb40ee5ff41 181 angleSuivreSol = angleSuivreMini + signeVirage*deltaAngle;
Mrlinkblue 0:9eb40ee5ff41 182 }
Mrlinkblue 0:9eb40ee5ff41 183 C = abs(sauts[indiceSauts][1]);
Mrlinkblue 0:9eb40ee5ff41 184 }
Mrlinkblue 0:9eb40ee5ff41 185
Mrlinkblue 0:9eb40ee5ff41 186 void actualisation(){
Mrlinkblue 0:9eb40ee5ff41 187 //On récupère l'ensemble des sauts de dérivés permettant la mise en place de l'algo
Mrlinkblue 0:9eb40ee5ff41 188
Mrlinkblue 0:9eb40ee5ff41 189 vector < vector <float> > sauts = rechercheSauts();
Mrlinkblue 0:9eb40ee5ff41 190 float angleSuivreSol;
Mrlinkblue 0:9eb40ee5ff41 191 float angleSuivreMini;
Mrlinkblue 0:9eb40ee5ff41 192
Mrlinkblue 0:9eb40ee5ff41 193 //On calcule l'ensemble des termes permettant la minimisation
Mrlinkblue 0:9eb40ee5ff41 194 if (sauts.size()!=0){ //S'il y a un saut (vide ou non)
Mrlinkblue 0:9eb40ee5ff41 195 sautsCara(sauts);
Mrlinkblue 0:9eb40ee5ff41 196 }
Mrlinkblue 0:9eb40ee5ff41 197 else{
Mrlinkblue 0:9eb40ee5ff41 198 angleSuivreMini = voiture_angle - pi/2;
Mrlinkblue 0:9eb40ee5ff41 199 angleSuivreSol = angleSuivreMini;
Mrlinkblue 0:9eb40ee5ff41 200 compensation = false;
Mrlinkblue 0:9eb40ee5ff41 201 C = 10;
Mrlinkblue 0:9eb40ee5ff41 202 }
Mrlinkblue 0:9eb40ee5ff41 203
Mrlinkblue 0:9eb40ee5ff41 204 //Phase de minimisation : on minimise C(As-A)² + b(A0-A)²
Mrlinkblue 0:9eb40ee5ff41 205 //Avec : - C = fonction du saut de dérivé (confiance) = valeur du saut
Mrlinkblue 0:9eb40ee5ff41 206 // - b = une cte à determiner
Mrlinkblue 0:9eb40ee5ff41 207 // - As = Angle solution directe pour le bon saut de dérivé (le plus grand)
Mrlinkblue 0:9eb40ee5ff41 208 // - A0 = Angle actuel de la voiture
Mrlinkblue 0:9eb40ee5ff41 209 // - A = Angle à suivre calculé final
Mrlinkblue 0:9eb40ee5ff41 210
Mrlinkblue 0:9eb40ee5ff41 211 float b;
Mrlinkblue 0:9eb40ee5ff41 212 float C;
Mrlinkblue 0:9eb40ee5ff41 213
Mrlinkblue 0:9eb40ee5ff41 214 if (compensation){
Mrlinkblue 0:9eb40ee5ff41 215 b = 1;
Mrlinkblue 0:9eb40ee5ff41 216 C = 0.1;
Mrlinkblue 0:9eb40ee5ff41 217 }
Mrlinkblue 0:9eb40ee5ff41 218 else{
Mrlinkblue 0:9eb40ee5ff41 219 b = 0.25;
Mrlinkblue 0:9eb40ee5ff41 220 }
Mrlinkblue 0:9eb40ee5ff41 221
Mrlinkblue 0:9eb40ee5ff41 222 //Solution
Mrlinkblue 0:9eb40ee5ff41 223 angle_suivre = (C*angleSuivreSol+b*voiture_angle)/(C+b);
Mrlinkblue 0:9eb40ee5ff41 224 vitesse_suivre = vitesse_max/2*exp(-facteur_vitesse*(angle_suivre-voiture_angle)*(angle_suivre-voiture_angle));
Mrlinkblue 0:9eb40ee5ff41 225 }
Mrlinkblue 0:9eb40ee5ff41 226
Mrlinkblue 0:9eb40ee5ff41 227 float calculAngle(float deltat){
Mrlinkblue 0:9eb40ee5ff41 228 float signe;
Mrlinkblue 0:9eb40ee5ff41 229
Mrlinkblue 0:9eb40ee5ff41 230 //On utilise self.ind pour calculer l'évolution de l'angle des roues
Mrlinkblue 0:9eb40ee5ff41 231 float ancien_angle = voiture_angle_actuel;
Mrlinkblue 0:9eb40ee5ff41 232 float nouveau_angle = exp(-deltat/tau_prog)*facteur_angle; // delta t définie pour toute la voiture
Mrlinkblue 0:9eb40ee5ff41 233
Mrlinkblue 0:9eb40ee5ff41 234 //On commence par limiter l'angle de variation:
Mrlinkblue 0:9eb40ee5ff41 235 float delta_angle=nouveau_angle-ancien_angle;
Mrlinkblue 0:9eb40ee5ff41 236
Mrlinkblue 0:9eb40ee5ff41 237 if (abs(delta_angle)/deltat > rot_limite){
Mrlinkblue 0:9eb40ee5ff41 238 //En fonction de la situation on modifie le signe
Mrlinkblue 0:9eb40ee5ff41 239 signe = abs(delta_angle)/delta_angle;
Mrlinkblue 0:9eb40ee5ff41 240 nouveau_angle = ancien_angle + signe*rot_limite*deltat;
Mrlinkblue 0:9eb40ee5ff41 241 //On verifie que l'on dépasse pas l'angle max
Mrlinkblue 0:9eb40ee5ff41 242 }
Mrlinkblue 0:9eb40ee5ff41 243 if (abs(nouveau_angle) > angle_max){
Mrlinkblue 0:9eb40ee5ff41 244 signe = abs(nouveau_angle)/nouveau_angle;
Mrlinkblue 0:9eb40ee5ff41 245 nouveau_angle = signe*angle_max;
Mrlinkblue 0:9eb40ee5ff41 246 }
Mrlinkblue 0:9eb40ee5ff41 247 return nouveau_angle;
Mrlinkblue 0:9eb40ee5ff41 248 }
Mrlinkblue 0:9eb40ee5ff41 249
Mrlinkblue 0:9eb40ee5ff41 250 float calculVitesse(float deltat){
Mrlinkblue 0:9eb40ee5ff41 251 float ancienneVitesse = voiture_vitesse;
Mrlinkblue 0:9eb40ee5ff41 252 float nouvelleVitesse = ancienneVitesse + exp(-deltat/tau_prog)*facteur_vitesse;
Mrlinkblue 0:9eb40ee5ff41 253
Mrlinkblue 0:9eb40ee5ff41 254 float signe;
Mrlinkblue 0:9eb40ee5ff41 255 //On commence par limiter l'acceleration:
Mrlinkblue 0:9eb40ee5ff41 256 float deltaV = nouvelleVitesse-ancienneVitesse;
Mrlinkblue 0:9eb40ee5ff41 257 if (abs(deltaV)/deltat > acc_limite){
Mrlinkblue 0:9eb40ee5ff41 258 //En fonction de la situation on modifie le signe
Mrlinkblue 0:9eb40ee5ff41 259 signe = abs(deltaV)/deltaV;
Mrlinkblue 0:9eb40ee5ff41 260 nouvelleVitesse = ancienneVitesse + signe*acc_limite*deltat;
Mrlinkblue 0:9eb40ee5ff41 261 }
Mrlinkblue 0:9eb40ee5ff41 262 //On verifie que l'on dépasse pas la vitesse max:
Mrlinkblue 0:9eb40ee5ff41 263 if (abs(nouvelleVitesse) > vitesse_max){
Mrlinkblue 0:9eb40ee5ff41 264 signe = abs(nouvelleVitesse)/nouvelleVitesse;
Mrlinkblue 0:9eb40ee5ff41 265 nouvelleVitesse = signe*vitesse_max;
Mrlinkblue 0:9eb40ee5ff41 266 }
Mrlinkblue 0:9eb40ee5ff41 267 return nouvelleVitesse;
Mrlinkblue 0:9eb40ee5ff41 268 }
Mrlinkblue 0:9eb40ee5ff41 269
Mrlinkblue 0:9eb40ee5ff41 270
Mrlinkblue 0:9eb40ee5ff41 271 void avancer(float deltat){
Mrlinkblue 0:9eb40ee5ff41 272 vitesse_suivre = calculVitesse(deltat);
Mrlinkblue 0:9eb40ee5ff41 273 angle_suivre = calculAngle(deltat);
Mrlinkblue 0:9eb40ee5ff41 274
Mrlinkblue 0:9eb40ee5ff41 275 voiture_vitesse = vitesse_suivre;
Mrlinkblue 0:9eb40ee5ff41 276 voiture_angle_actuel = angle_suivre;
Mrlinkblue 0:9eb40ee5ff41 277
Mrlinkblue 0:9eb40ee5ff41 278 facteur_angle = (angle_suivre-voiture_angle)*(1-exp(double( -1/50)));
Mrlinkblue 0:9eb40ee5ff41 279 facteur_vitesse = (vitesse_suivre-voiture_vitesse)*(1-exp(double(-1/50)));
Mrlinkblue 0:9eb40ee5ff41 280 }
Mrlinkblue 0:9eb40ee5ff41 281
Mrlinkblue 0:9eb40ee5ff41 282
Mrlinkblue 0:9eb40ee5ff41 283
Mrlinkblue 0:9eb40ee5ff41 284
Mrlinkblue 0:9eb40ee5ff41 285
Mrlinkblue 0:9eb40ee5ff41 286
Mrlinkblue 0:9eb40ee5ff41 287