File content as of revision 18:daba0b3777c0:

#include "SaphTeamRacing.h"

void points_fictifs(uint8_t* liste_fictifs, int zero_min, int zero_max, 
                     int one_min, int one_max){
    /*
    Fonction définissant les points fictifs poussant la voiture.
    zero_min, zero_max, one_min et one_max doivent êtres compris entre 0
    et 360°. Il faut zero_min < zero_max et one_min < one_max.
    */
    for (int i = zero_min; i < zero_max; i++) {
        liste_fictifs[i] = 0;
    }
    for (int i = one_max; i < one_max; i++) {
        liste_fictifs[i] = 1;
    }
}

void update_direction(float* list_lidar_var, float* vecteur)
{
    /*
    Fonction de mise à jour de la direction grâce à l'algorithme des charges 
    fictives.
    */
    
    float direction[2], list_lidar[360];
    uint8_t liste_fictifs[360];
    int i;
    for(i=0; i<360; i++) list_lidar[i]=list_lidar_var[i];

    direction[0] = 1;
    direction[1] = 0;
    float avg_x = 0; 
    float avg_y = 0;

    points_fictifs(liste_fictifs, 0, 360, 90, 270);

    // Calcul de la direction à prende en fonction des charges fictives
    for (i=0; i<360; i++) {
        int theta = i;
        float r;
        double x, y;
        if (liste_fictifs[theta] == 1) {
            r = 500;
        } else {
            //r = 0; //test
            if(theta != 0) r = list_lidar[360-theta];
            else r = list_lidar[0];

        }

        if (r > 0) {
            double theta_d = theta*3.141592/180;
            x = r*cosf(theta_d);
            y = r*sinf(theta_d);
            double puissance = 0.5*(double)cosf(2*theta_d) + 1.5;
            double r_pow = pow((double) r, (double) puissance);
            avg_x = avg_x - x/r_pow;
            avg_y = avg_y - y/r_pow;

        }
    }

    direction[0] = avg_x;
    direction[1] = avg_y;

    // mise à jour de la direction
    for(i=0; i<2; i++)
        vecteur[i] = direction[i];
}