assert1
Dependencies: mbed X_NUCLEO_IHM02A1
chemin.cpp
- Committer:
- JimmyAREM
- Date:
- 2019-03-30
- Revision:
- 3:06cbe2f6c494
- Parent:
- 2:977799d72329
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); } } }