File content as of revision 3:06cbe2f6c494:

#include "mbed.h"
#include "chemin.h"
#include "terrain_precalc.h"
#include "odometrie.h"
#include "tests_moteurs.h"


/*
L'origine du terrain est
    y-----------------|
    |       |  |      |
    |  ¯¯¯¯¯¯¯¯¯¯¯¯¯  |
    |                 |
    0-----------------|> x

    Et l'origine des angles est de x vers y positif.
*/


// Function to Create A New Node
Node* newNode(int x, int y, int distance, int p)
{
    Node* temp = (Node*)malloc(sizeof(Node));
    temp->x = x;
    temp->y = y;
    temp->distance = distance;
    temp->priority = p;
    temp->next = NULL;

    return temp;
}

// Return the value at head
void peek(Node** head, int*x, int*y, int*distance)
{
    *x = (*head)->x;
    *y = (*head)->y;
    *distance = (*head)->distance;
}

// Removes the element with the
// highest priority form the list
void pop(Node** head)
{
    Node* temp = *head;
    (*head) = (*head)->next;
    free(temp);
}



// Function to push according to priority
void push(Node** head, int x, int y, int distance, int p)
{
    Node* start = (*head);

    // Create new Node
    Node* temp = newNode(x, y,distance, p);

    // Special Case: The head of list has lesser
    // priority than new node. So insert new
    // node before head node and change head node.
    if ((*head)->priority > p) {

        // Insert New Node before head
        temp->next = *head;
        (*head) = temp;
    } else {

        // Traverse the list and find a
        // position to insert new node
        while (start->next != NULL &&
                start->next->priority < p) {
            start = start->next;
        }

        // Either at the ends of the list
        // or at required position
        temp->next = start->next;
        start->next = temp;
    }
}

// Function to check is list is empty
int isEmpty(Node** head)
{
    return (*head) == NULL;
}


void calc_distances(int ox, int oy, int gx, int gy)
{
    //Calcule la distance de chaque point du terrain à l'objectif (gx,gy)
    //COmplexite en O(l*L)
    printf("ok\n");

    char visite[l][L] = {{0}}; //sauvegarde les points deja visites pour eviter de passer 2 fois dessus
    int voisins[9][9] = {{0,1}, {-1,0}, {1,0}, {0,-1}, {1,1}, {-1,1}, {1,-1}, {-1,-1} };    //voisin d'un point
    int distance = 0;
    int px = gx;
    int py = gy;
    visite[gx][gy] = 1;
    Node* pq = newNode(gx,gy,distance, 0);
    printf("ok2\n");
    while(!isEmpty(&pq)) { //Tant que l'on n'a pas parcouru tous les points TODO: optimiser pour arreter lorque (px,py) == (ox,oy) pour éviter de tout parcourir, sans que cela infule sur le choix du chemin

        peek(&pq,&px,&py,&distance);    //on recupere les infos du point
        distances[px][py] = distance;
        //printf("position tiree : %d , %d , de distance %d\n" , px,py,distance);


        for(int i=0; i<9; i++) {
            int vx = px+voisins[i][0];
            int vy = py+voisins[i][1];

            if( vx >= 0 && vx < l && vy >= 0 && vy < L && terrain[vx][vy] != 1 && visite[vx][vy] == 0) {
                //printf("positions ajoutee : %d,%d,%d\n" ,  vx, vy,distance );
                visite[vx][vy] = 1;
                push(&pq, vx, vy, distance+1,distance+1);  //on ajoute ses soivins s'ils sont dans le terrain, ne sont pas un mur, et n'ont pas été visités
            }
        }
        pop(&pq); //on peut le pop puisqu'il a été visité

    }
}

Node* trouver_chemin(int ox, int oy, int gx, int gy)
{
    //Trouve le meilleur chemin en se rapprochant de l'objectif
    //Complexité en longueur du chemin (MAX= L*l)
    int voisins[9][9] = {{0,1}, {-1,0}, {1,0}, {0,-1}, {1,1}, {-1,1}, {1,-1}, {-1,-1} };
    int distance = 0;
    Node* pq = newNode(ox,oy,distance, 0);
    int px = ox;
    int py = oy;

    while(!(px==gx && py==gy)) {
        distance++;
        unsigned long long dmin = L*l;
        int xmin = 0;
        int ymin = 0;
        for(int i=0; i<9; i++) {
            int vx = px+voisins[i][0];
            int vy = py+voisins[i][1];

            if( vx >= 0 && vx < l && vy >= 0 && vy < L && terrain[vx][vy] != 1 && distances[vx][vy] < dmin) {
                dmin = distances[vx][vy];
                xmin = vx;
                ymin = vy;
            }
        }
        //printf("position ajoutee chemin : %d , %d , de distance %d\n" , px,py,distance);
        push(&pq, xmin, ymin, distance,distance);
        px = xmin;
        py = ymin;

    }
    return pq;
}


Node* traduction_points_commandes(Node* pq)
{
    /*

    y_____________
    | 9   10   11|
    |            |
    | -1   R    1|
    |            |
    |-11  -10  -9|
    ¯¯¯¯¯¯¯¯¯¯¯¯¯x
         ||
         \/
    ___________
    | 4  3  2 |
    | 5  R  1 |
    | 6  7  8 |
    ¯¯¯¯¯¯¯¯¯¯¯
    */
    int i,px,py,distance,direction = 0;
    peek(&pq,&px,&py,&distance);    //on recupere les infos du point
    pop(&pq);
    Node* cmd = newNode(0,0,0,0);

    int px_new,py_new,direction_new,_ =0;


    while(!isEmpty(&pq)) {
        peek(&pq,&px_new,&py_new,&_);   //on recupere les infos du point
        pop(&pq);
        //printf("%d , %d , %d\n",px,py,px_new - px + 10*(py_new-py));
        switch(px_new - px + 10*(py_new-py)) {
            case 1:
                direction_new=1;
                distance+=100;
                break;
            case -1:
                direction_new=5;
                distance+=100;
                break;
            case 10:
                direction_new=3;
                distance+=100;
                break;
            case -10:
                direction_new=7;
                distance+=100;
                break;
            case 11:
                direction_new=2;
                distance+=141; //racine de 2, la diagonale d'un carré
                break;
            case 9:
                direction_new=4;
                distance+=141;
                break;
            case -9:
                direction_new=8;
                distance+=141;
                break;
            case -11:
                direction_new=6;
                distance+=141;
                break;
        }

        if(direction_new != direction) {
            if(direction ==0) {

                push(&cmd,(int) direction_new, 0, 0, i);

            } else {
                int a = direction-direction_new;
                if(direction == 8 && direction_new == 1) a = -1;
                if(direction == 1 && direction_new == 8) a = 1;
                printf("%d %d, %d,%d , %d \n", a, distance,direction, direction_new,(direction-direction_new));
                push(&cmd,(int) 45*a, distance, 0, i);
            }
            distance = 0;
            direction = direction_new;
            i++;
        }
        px = px_new;
        py = py_new;

    }
    printf("ok");
    i++;
    float a = (1+1)/(7-1);
    float b = 1-a*7;
    push(&cmd, 0, distance, 0,i);


    return cmd;

}

void afficher_terrain()
{
    //Affiche le terrain sur le port serie
    printf("Terrain avec murs: \n");
    for(int j=0; j<=L; j++) printf("_");
    printf("\n");
    for(int i = 0; i < l; i++) {
        printf("|");
        for(int j=0; j<L; j++) {

            if(terrain[i][j] ==0) {
                printf(" ");
            } else {
                printf(".");

            }
        }
        printf("|\n");
    }
    for(int j=0; j<=L; j++) printf("¯");
    printf("\n\n");
}


void afficher_terrain(Node* points)
{
    char terrain_tmp[l][L] = {{0}};
    for(int i = 0; i < l; i++) for(int j=0; j<L; j++) terrain_tmp[i][j] = terrain[i][j];

    int i,px,py,_ = 0;
    while(!isEmpty(&points)) {
        peek(&points,&px,&py,&_);   //on recupere les infos du point
        pop(&points);
        terrain_tmp[px][py] = 2;
    }

    //Affiche le terrain sur le port serie
    printf("Terrain avec murs: \n");
    for(int j=0; j<=L; j++) printf("_");
    printf("\n");
    for(int i = 0; i < l; i++) {
        printf("|");
        for(int j=0; j<L; j++) {

            if(terrain_tmp[i][j] ==0) {
                printf(" ");
            } else if(terrain_tmp[i][j] == 2) {
                printf("X");
            } else {
                printf(".");

            }
        }
        printf("|\n");
    }
    for(int j=0; j<=L; j++) printf("¯");
    printf("\n\n");
}

int conversion_codage_angles(int o)
{

    switch(o) {
        case 1:
            o = 0;
            break;
        case 2:
            o = 45;
            break;
        case 3:
            o = 90;
            break;
        case 4:
            o = 135;
            break;
        case 5:
            o = -180;
            break;
        case 6:
            o = -225;
            break;
        case 7:
            o = 270;
            break;
        case 8:
            o = 315;
            break;
    }
    return o;
}

void aller_a_point(int ox , int oy, int gx , int gy , int vitesse_deplacement){
    
    double angle_absolut = get_angle();

    calc_distances(ox,oy,gx,gy);
    Node* cm = (Node*)malloc(sizeof(Node)); 
    cm = traduction_points_commandes(trouver_chemin(ox,oy,gx,gy));
    
    //afficher_terrain(trouver_chemin(ox,oy,gx,gy));
    
    int o,d,_ = 0;
    pop(&cm);
    peek(&cm,&o,&d,&_);     //on recupere les infos de direction 
    pop(&cm);
    o = conversion_codage_angles(o);
    printf("rotation %d, avancement de %d\n",o,d);
    angle_absolut += o;
    
    //test_rotation_abs(o,100);
    wait(2);
    
    while(!isEmpty(&cm)){ 
        peek(&cm,&o,&d,&_);     //on recupere les infos de direction 
        pop(&cm);
        printf("rotation %d, avancement de %d\n",o,d);
        if(d != 0 && o != 0) {
            test_ligne_droite(10*d, -vitesse_deplacement);
            wait(2);
            angle_absolut += o;
            test_rotation_abs(angle_absolut,50);
            wait(2);
            }
    }
 }