File content as of revision 7:20d05f0d11a2:

#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 = 30.0; // (degrees)
float lidar_dmax = 5; // (m)
//float distance_securite=0.5; // A VOIR -> Inutile en faite
float temps_un_point=0.0005; // (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)


// Paramètres de la voiture variable
float voiture_angle = 90.0; // self.angle (angle du repère)
float voiture_angle_roues = 0.0; // self.angleroues

float facteur_angle = 0.0;
float facteur_distance = 0.0;

float voiture_vitesse = 2.0;

// Paramètres internes aux fonction
float angleSuivreSol;
float angleSuivreMini;
float C;
float b;
bool compensation;

float facteur_vitesse = 180/(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;
    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
    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é
    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.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++){
            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
    angleSuivreMini = voiture_angle + sauts[indiceSauts][0] - 180/2;
        
    if (data_distances[sauts[indiceSauts][2]] == 0){   //On vérifie que l'on a pas de problemes
        angleSuivreSol = angleSuivreMini + 180/2;
        }
    else{
        //On ne veut pas passer trop près du mur
        deltaAngle=min(180/20,atan(largeur_voiture/data_distances[sauts[indiceSauts][2]])*180/pi);
        //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.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.begin()+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();

    //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 - 180/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
    
    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_roues;
    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;
    
    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/temps_un_point)/20)*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;
        }
    // Vitesse négative ?
    return nouvelleVitesse;
    }
    
    
void avancer(float deltat){
    voiture_vitesse = calculVitesse(deltat);
    voiture_angle_roues= calculAngle(deltat);
    
    facteur_angle = (angle_suivre-voiture_angle)*(0.9);
    facteur_vitesse = (vitesse_suivre-voiture_vitesse)*(0.9);
    }