Diff: algorithme.h

Revision:

4:c393c14f4502

Parent:

3:97827746c632

Child:

5:73aac5fe9696

--- a/algorithme.h	Fri May 31 13:45:02 2019 +0000
+++ b/algorithme.h	Wed Jun 05 22:55:03 2019 +0000
@@ -20,7 +20,8 @@
 float angle_max = 30.0; // (degrees)
 float lidar_dmax = 5; // (m)
 //float distance_securite=0.5; // A VOIR -> Inutile en faite
-float tau_prog = 2.0;   // (s) La moitié de la fréquence du lidar à peu près
+float temps_un_point=1.0/(360.0*10.0); // (s)
+//float tau_prog = 2.0;   // (s) La moitié de la fréquence du lidar à peu près
 //float lidar_frequence = 10.0; // (Hz) 
 float rot_limite=(pi/2)*(180/pi)/0.5; // (deg/s)
 float acc_limite=5;  // (m/s2)
@@ -37,11 +38,13 @@
 
 // Paramètres internes aux fonction
 float angleSuivreSol;
+float angleSuivreMini;
 float C;
+float b;
 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;
@@ -56,15 +59,16 @@
 
 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++){
+    float vmin;
+    vmin = vect[0];
+    for (int i=1; 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
@@ -78,17 +82,18 @@
         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){ 
+            if (consecutive >= 0){ 
                 consecutive += 1;
                 }
-            else if (consecutive > 0){ //Fin de plateau
+            }
+        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;
@@ -99,10 +104,14 @@
     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;
@@ -112,19 +121,21 @@
             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
+    v2=vector <vector <float> >(sauts.begin(),sauts.begin()+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++){
@@ -146,8 +157,9 @@
             compensation = true;
             }
         }
+    
     //On se ramène dans le repère général
-    float angleSuivreMini = voiture_angle + sauts[indiceSauts][0] - pi/2;
+    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;
@@ -162,16 +174,20 @@
         
         vector <float> v3;
         vector <float> v4;
-        v3=vector <float>(data_distances.begin(),data_distances.end()-indPi2);    // distances[:indPi2]
+        v3=vector <float>(data_distances.begin(),data_distances.begin()+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);  
+        
+        v5=vector <float>(data_distances.begin()+indPi2,data_distances.begin()+ind3Pi2);  
+        
         float distanceBordGauche = min_vect(v5);
         float signeVirage;
+        
         if (distanceBordDroit > distanceBordGauche){ //Trop près de la gauche donc compensation à droite
             signeVirage = -1;
             }
@@ -180,15 +196,14 @@
             }
         angleSuivreSol = angleSuivreMini + signeVirage*deltaAngle;
         }
-     C = abs(sauts[indiceSauts][1]);
+     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)
@@ -208,9 +223,6 @@
     //           - A0 = Angle actuel de la voiture 
     //           - A = Angle à suivre calculé final
     
-    float b;
-    float C;
-
     if (compensation){
         b = 1;
         C = 0.1;
@@ -224,12 +236,15 @@
     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_roues;
-    float nouveau_angle = exp(-deltat/tau_prog)*facteur_angle; // delta t définie pour toute la voiture
+    float nouveau_angle = exp(-(deltat/temps_un_point)/50)*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;
@@ -249,7 +264,7 @@
 
 float calculVitesse(float deltat){
     float ancienneVitesse = voiture_vitesse;
-    float nouvelleVitesse = ancienneVitesse + exp(-deltat/tau_prog)*facteur_vitesse;
+    float nouvelleVitesse = ancienneVitesse + exp(-(deltat/temps_un_point)/20)*facteur_vitesse;
     
     float signe;
     //On commence par limiter l'acceleration:
@@ -273,8 +288,8 @@
     voiture_vitesse = calculVitesse(deltat);
     voiture_angle_roues= calculAngle(deltat);
     
-    facteur_angle = (angle_suivre-voiture_angle)*(1-exp(double( -1/50)));
-    facteur_vitesse = (vitesse_suivre-voiture_vitesse)*(1-exp(double(-1/50)));
+    facteur_angle = (angle_suivre-voiture_angle)*(1-exp(double( -1)));
+    facteur_vitesse = (vitesse_suivre-voiture_vitesse)*(1-exp(double(-1)));
     }