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@6:9af875ef7b30, 2019-06-06 (annotated)

Committer:
Mrlinkblue
Date:
Thu Jun 06 16:30:59 2019 +0000
Revision:
6:9af875ef7b30
Parent:
5:73aac5fe9696 
Version_jeudi_6_soir

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