Diff: algorithme.h

Revision:

0:9eb40ee5ff41

Child:

3:97827746c632

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/algorithme.h	Fri May 31 09:41:29 2019 +0000
@@ -0,0 +1,287 @@
+#include "mbed.h"
+#include <vector>
+#include <algorithm>    // std::min_element, std::max_element
+
+#define pi  3.14159265358979323846
+
+using namespace std;
+
+// Liste d'angles/distances
+vector <float> data_distances(360);
+vector <float> data_angles(360);
+
+// Paramètre pour la fonction actualisation (sorties)
+float angle_suivre;
+float vitesse_suivre=0;
+
+// Paramètres de la voiture "fixe" mais à varier pour optimiser
+float largeur_voiture=0.3; // (m) distance de sécurité comprise dedans
+float vitesse_max=1.5; // (m/s)
+float angle_max = 20.0; // (degrees)
+float lidar_dmax = 5; // (m)
+//float distance_securite=0.5; // A VOIR -> Inutile en faite
+float tau_prog = 0.050;           // (s)
+//float lidar_frequence = 10.0; // (Hz) 
+float rot_limite=(pi/2)*(180/pi)/0.5; // (deg/s)
+float acc_limite=3;  // (m/s2)
+
+
+// Paramètres de la voiture variable
+float voiture_angle = 0.0; // self.angle (angle du repère)
+float voiture_angle_actuel = 0.0; // self.angleroues
+
+float facteur_angle = 0.0;
+float facteur_distance = 0.0;
+
+float voiture_vitesse = 0.0;
+
+// Paramètres internes aux fonction
+float angleSuivreSol;
+float C;
+bool compensation;
+float facteur_vitesse = pi*pi/(log(vitesse_max/0.5));
+
+
+float min(float a, float b){
+    // Fonction permettant de déterminer le minimum entre a et b
+    float c;
+    if (a>b){
+        c=b;
+        }
+    else{
+        c=a;
+        }
+    return c;
+    }
+
+float min_vect(vector <float> vect){
+    // Fonction permettant de déterminer le minimum d'un vecteur
+    float vmin = 0;
+    for (int i=0; i<vect.size(); i++){
+        if (vect[i]<vmin){
+            vmin=vect[i];
+            }
+        }
+    return vmin;
+    }
+    
+vector <vector <float> > rechercheSauts(){
+    // Fonction permettant de rechercher un saut dans une série de données 
+    // d'angles et de distances
+    float deriv;
+    vector <vector <float> > sauts(0);
+    vector <float> saut(3); // Triplet d'un angle, saut et de son indice i
+    int consecutive = 0;
+    
+    //On cherche tout les sauts de dérivés à l'avant de la voiture
+    for (int i=0; i<180;i++){
+        deriv = (data_distances[i+1] - data_distances[i])/(data_angles[i+1]-data_angles[i]);
+        if (deriv == 0 and data_distances[i] >= lidar_dmax){ //Plateau
+            //début de plateau
+            if (consecutive > 0){ 
+                consecutive += 1;
+                }
+            else if (consecutive > 0){ //Fin de plateau
+                saut[0] = data_angles[i - int(consecutive)/2];
+                saut[1] = lidar_dmax/(data_angles[1]-data_angles[0]);
+                saut[2] = i - int(consecutive)/2;
+                sauts.push_back(saut);
+                consecutive = 0;
+                }
+            }
+        if (deriv != 0){     //Saut de dérivé ou plateau
+            saut[0] = data_angles[i];
+            saut[1] = deriv;
+            saut[2] = i;
+            sauts.push_back(saut);
+            }
+        }
+    return sauts;
+    }
+
+void sautsCara(vector <vector <float> > sauts){
+    // Cette fonction permet de définir les paramètres de la minimisation :
+    // As la solution théorique et les coef associé
+    compensation = false;
+    float deltaAngle;
+    // On cherche l'indice dans les sauts de celui que l'on va prendre (plus grand)
+    int indiceSauts = 0;
+    float saut_max = 0;
+    for (int i=0; i<sauts.size(); i++){
+        if (abs(sauts[i][1])>saut_max){
+            indiceSauts=i;
+            }
+        }
+    // On cherche un deuxième saut pour savoir si on doit faire une compensation
+    vector <vector <float> > v1;
+    vector <vector <float> > v2;
+    v1=vector <vector <float> >(sauts.begin()+indiceSauts+1,sauts.end());     // sauts[indice_saut+1:]
+    v2=vector <vector <float> >(sauts.begin(),sauts.end()-indiceSauts);     // sauts[:indice_saut]
+    // Concaténation des deux vecteurs sans le saut
+    vector <vector <float> > sautsMoins ;
+    sautsMoins.reserve(v1.size() + v2.size()); // preallocate memory
+    sautsMoins.insert( sautsMoins.end(), v2.begin(), v2.end() );
+    sautsMoins.insert( sautsMoins.end(), v1.begin(), v1.end() );
+    
+    if (sautsMoins.size()!=0){ //S'il y a un saut
+    
+        int indiceSauts2=0;
+        float saut_max2=0;
+        for (int i=0; i<sautsMoins.size(); i++){
+            if (abs(sautsMoins[i][1]) > saut_max2){
+                indiceSauts2=i;
+                saut_max2=abs(sautsMoins[i][1]);
+                
+                }
+            }
+            
+        if (indiceSauts2 >= indiceSauts) { //Décalage d'indice
+            indiceSauts2+=1;
+            }
+        float distanceSaut1 = data_distances[sauts[indiceSauts][2]+1] - data_distances[sauts[indiceSauts][2]];
+        float distanceSaut2 = data_distances[sauts[indiceSauts2][2]+1] - data_distances[sauts[indiceSauts2][2]];
+        if (3*distanceSaut1/4 <= distanceSaut2) { 
+            // On a un deuxième sauts importants donc on compense les coeef de minimisation
+            // Dans certains cas c'est le deuxième saut qui doit être suivi
+            compensation = true;
+            }
+        }
+    //On se ramène dans le repère général
+    float angleSuivreMini = voiture_angle + sauts[indiceSauts][0] - pi/2;
+        
+    if (data_distances[sauts[indiceSauts][2]] == 0){   //On vérifie que l'on a pas de problemes
+        angleSuivreSol = angleSuivreMini + pi/2;
+        }
+    else{
+        //On ne veut pas passer trop près du mur
+        deltaAngle=min(pi/20,atan(largeur_voiture/data_distances[sauts[indiceSauts][2]]));
+        //On détermine le signe de la compensation (virage à droite ou gauche et repère général)
+        //On détermine les distances aux deux bords
+        int indPi2 = int(data_angles.size()/4) ;
+        int ind3Pi2 = int(3*data_angles.size()/4);
+        
+        vector <float> v3;
+        vector <float> v4;
+        v3=vector <float>(data_distances.begin(),data_distances.end()-indPi2);    // distances[:indPi2]
+        v4=vector <float>(data_distances.begin()+ind3Pi2,data_distances.end());      // distances[ind3Pi2:]
+        float distanceBordDroit1 = min_vect(v3);
+        float distanceBordDroit2 = min_vect(v4);
+        float distanceBordDroit = min(distanceBordDroit2,distanceBordDroit1);
+        
+        vector <float> v5;
+        v5=vector <float>(data_distances.begin()+indPi2,data_distances.end()-ind3Pi2);  
+        float distanceBordGauche = min_vect(v5);
+        float signeVirage;
+        if (distanceBordDroit > distanceBordGauche){ //Trop près de la gauche donc compensation à droite
+            signeVirage = -1;
+            }
+        else{
+            signeVirage = 1;
+            }
+        angleSuivreSol = angleSuivreMini + signeVirage*deltaAngle;
+        }
+     C = abs(sauts[indiceSauts][1]);
+}
+
+void actualisation(){
+    //On récupère l'ensemble des sauts de dérivés permettant la mise en place de l'algo
+    
+    vector < vector <float> > sauts = rechercheSauts();
+    float angleSuivreSol;
+    float angleSuivreMini;
+
+    //On calcule l'ensemble des termes permettant la minimisation
+    if (sauts.size()!=0){ //S'il y a un saut (vide ou non)
+        sautsCara(sauts);
+        }   
+    else{
+        angleSuivreMini = voiture_angle - pi/2;
+        angleSuivreSol = angleSuivreMini;
+        compensation = false;
+        C = 10;
+        }
+        
+    //Phase de minimisation : on minimise C(As-A)² + b(A0-A)²
+    //Avec :     - C = fonction du saut de dérivé (confiance) = valeur du saut
+    //           - b = une cte à determiner
+    //           - As = Angle solution directe pour le bon saut de dérivé (le plus grand)
+    //           - A0 = Angle actuel de la voiture 
+    //           - A = Angle à suivre calculé final
+    
+    float b;
+    float C;
+
+    if (compensation){
+        b = 1;
+        C = 0.1;
+        }
+    else{
+        b = 0.25;
+        }
+        
+    //Solution
+    angle_suivre = (C*angleSuivreSol+b*voiture_angle)/(C+b);
+    vitesse_suivre = vitesse_max/2*exp(-facteur_vitesse*(angle_suivre-voiture_angle)*(angle_suivre-voiture_angle));
+}
+
+float calculAngle(float deltat){
+    float signe;
+    
+    //On utilise self.ind pour calculer l'évolution de l'angle des roues
+    float ancien_angle = voiture_angle_actuel;
+    float nouveau_angle = exp(-deltat/tau_prog)*facteur_angle; // delta t définie pour toute la voiture
+        
+    //On commence par limiter l'angle de variation:
+    float delta_angle=nouveau_angle-ancien_angle;
+    
+    if (abs(delta_angle)/deltat > rot_limite){
+        //En fonction de la situation on modifie le signe
+        signe = abs(delta_angle)/delta_angle;
+        nouveau_angle = ancien_angle + signe*rot_limite*deltat;
+        //On verifie que l'on dépasse pas l'angle max
+        }
+    if (abs(nouveau_angle) > angle_max){
+        signe = abs(nouveau_angle)/nouveau_angle;
+        nouveau_angle = signe*angle_max;
+        }
+    return nouveau_angle;
+    }
+
+float calculVitesse(float deltat){
+    float ancienneVitesse = voiture_vitesse;
+    float nouvelleVitesse = ancienneVitesse + exp(-deltat/tau_prog)*facteur_vitesse;
+    
+    float signe;
+    //On commence par limiter l'acceleration:
+    float deltaV = nouvelleVitesse-ancienneVitesse;
+    if (abs(deltaV)/deltat > acc_limite){     
+        //En fonction de la situation on modifie le signe
+        signe = abs(deltaV)/deltaV;
+        nouvelleVitesse = ancienneVitesse + signe*acc_limite*deltat;
+        }
+    //On verifie que l'on dépasse pas la vitesse max:
+    if (abs(nouvelleVitesse) > vitesse_max){
+        signe = abs(nouvelleVitesse)/nouvelleVitesse;
+        nouvelleVitesse = signe*vitesse_max;
+        }
+    return nouvelleVitesse;
+    }
+    
+    
+void avancer(float deltat){
+    vitesse_suivre = calculVitesse(deltat);
+    angle_suivre = calculAngle(deltat);
+    
+    voiture_vitesse = vitesse_suivre; 
+    voiture_angle_actuel = angle_suivre; 
+    
+    facteur_angle = (angle_suivre-voiture_angle)*(1-exp(double( -1/50)));
+    facteur_vitesse = (vitesse_suivre-voiture_vitesse)*(1-exp(double(-1/50)));
+    }
+
+
+
+
+
+
+