chemin.cpp

00001 #include "mbed.h"
00002 #include "chemin.h"
00003 #include "terrain_precalc.h"
00004 #include "odometrie.h"
00005 #include "tests_moteurs.h"
00006 
00007 
00008 /*
00009 L'origine du terrain est
00010     y-----------------|
00011     |       |  |      |
00012     |  ¯¯¯¯¯¯¯¯¯¯¯¯¯  |
00013     |                 |
00014     0-----------------|> x
00015 
00016     Et l'origine des angles est de x vers y positif.
00017 */
00018 
00019 
00020 // Function to Create A New Node
00021 Node* newNode(int x, int y, int distance, int p)
00022 {
00023     Node* temp = (Node*)malloc(sizeof(Node));
00024     temp->x = x;
00025     temp->y = y;
00026     temp->distance = distance;
00027     temp->priority = p;
00028     temp->next = NULL;
00029 
00030     return temp;
00031 }
00032 
00033 // Return the value at head
00034 void peek(Node** head, int*x, int*y, int*distance)
00035 {
00036     *x = (*head)->x;
00037     *y = (*head)->y;
00038     *distance = (*head)->distance;
00039 }
00040 
00041 // Removes the element with the
00042 // highest priority form the list
00043 void pop(Node** head)
00044 {
00045     Node* temp = *head;
00046     (*head) = (*head)->next;
00047     free(temp);
00048 }
00049 
00050 
00051 
00052 // Function to push according to priority
00053 void push(Node** head, int x, int y, int distance, int p)
00054 {
00055     Node* start = (*head);
00056 
00057     // Create new Node
00058     Node* temp = newNode(x, y,distance, p);
00059 
00060     // Special Case: The head of list has lesser
00061     // priority than new node. So insert new
00062     // node before head node and change head node.
00063     if ((*head)->priority > p) {
00064 
00065         // Insert New Node before head
00066         temp->next = *head;
00067         (*head) = temp;
00068     } else {
00069 
00070         // Traverse the list and find a
00071         // position to insert new node
00072         while (start->next != NULL &&
00073                 start->next->priority < p) {
00074             start = start->next;
00075         }
00076 
00077         // Either at the ends of the list
00078         // or at required position
00079         temp->next = start->next;
00080         start->next = temp;
00081     }
00082 }
00083 
00084 // Function to check is list is empty
00085 int isEmpty(Node** head)
00086 {
00087     return (*head) == NULL;
00088 }
00089 
00090 
00091 void calc_distances(int ox, int oy, int gx, int gy)
00092 {
00093     //Calcule la distance de chaque point du terrain à l'objectif (gx,gy)
00094     //COmplexite en O(l*L)
00095     printf("ok\n");
00096 
00097     char visite[l][L] = {{0}}; //sauvegarde les points deja visites pour eviter de passer 2 fois dessus
00098     int voisins[9][9] = {{0,1}, {-1,0}, {1,0}, {0,-1}, {1,1}, {-1,1}, {1,-1}, {-1,-1} };    //voisin d'un point
00099     int distance = 0;
00100     int px = gx;
00101     int py = gy;
00102     visite[gx][gy] = 1;
00103     Node* pq = newNode(gx,gy,distance, 0);
00104     printf("ok2\n");
00105     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
00106 
00107         peek(&pq,&px,&py,&distance);    //on recupere les infos du point
00108         distances[px][py] = distance;
00109         //printf("position tiree : %d , %d , de distance %d\n" , px,py,distance);
00110 
00111 
00112         for(int i=0; i<9; i++) {
00113             int vx = px+voisins[i][0];
00114             int vy = py+voisins[i][1];
00115 
00116             if( vx >= 0 && vx < l && vy >= 0 && vy < L && terrain[vx][vy] != 1 && visite[vx][vy] == 0) {
00117                 //printf("positions ajoutee : %d,%d,%d\n" ,  vx, vy,distance );
00118                 visite[vx][vy] = 1;
00119                 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
00120             }
00121         }
00122         pop(&pq); //on peut le pop puisqu'il a été visité
00123 
00124     }
00125 }
00126 
00127 Node* trouver_chemin(int ox, int oy, int gx, int gy)
00128 {
00129     //Trouve le meilleur chemin en se rapprochant de l'objectif
00130     //Complexité en longueur du chemin (MAX= L*l)
00131     int voisins[9][9] = {{0,1}, {-1,0}, {1,0}, {0,-1}, {1,1}, {-1,1}, {1,-1}, {-1,-1} };
00132     int distance = 0;
00133     Node* pq = newNode(ox,oy,distance, 0);
00134     int px = ox;
00135     int py = oy;
00136 
00137     while(!(px==gx && py==gy)) {
00138         distance++;
00139         unsigned long long dmin = L*l;
00140         int xmin = 0;
00141         int ymin = 0;
00142         for(int i=0; i<9; i++) {
00143             int vx = px+voisins[i][0];
00144             int vy = py+voisins[i][1];
00145 
00146             if( vx >= 0 && vx < l && vy >= 0 && vy < L && terrain[vx][vy] != 1 && distances[vx][vy] < dmin) {
00147                 dmin = distances[vx][vy];
00148                 xmin = vx;
00149                 ymin = vy;
00150             }
00151         }
00152         //printf("position ajoutee chemin : %d , %d , de distance %d\n" , px,py,distance);
00153         push(&pq, xmin, ymin, distance,distance);
00154         px = xmin;
00155         py = ymin;
00156 
00157     }
00158     return pq;
00159 }
00160 
00161 
00162 Node* traduction_points_commandes(Node* pq)
00163 {
00164     /*
00165 
00166     y_____________
00167     | 9   10   11|
00168     |            |
00169     | -1   R    1|
00170     |            |
00171     |-11  -10  -9|
00172     ¯¯¯¯¯¯¯¯¯¯¯¯¯x
00173          ||
00174          \/
00175     ___________
00176     | 4  3  2 |
00177     | 5  R  1 |
00178     | 6  7  8 |
00179     ¯¯¯¯¯¯¯¯¯¯¯
00180     */
00181     int i,px,py,distance,direction = 0;
00182     peek(&pq,&px,&py,&distance);    //on recupere les infos du point
00183     pop(&pq);
00184     Node* cmd = newNode(0,0,0,0);
00185 
00186     int px_new,py_new,direction_new,_ =0;
00187 
00188 
00189     while(!isEmpty(&pq)) {
00190         peek(&pq,&px_new,&py_new,&_);   //on recupere les infos du point
00191         pop(&pq);
00192         //printf("%d , %d , %d\n",px,py,px_new - px + 10*(py_new-py));
00193         switch(px_new - px + 10*(py_new-py)) {
00194             case 1:
00195                 direction_new=1;
00196                 distance+=100;
00197                 break;
00198             case -1:
00199                 direction_new=5;
00200                 distance+=100;
00201                 break;
00202             case 10:
00203                 direction_new=3;
00204                 distance+=100;
00205                 break;
00206             case -10:
00207                 direction_new=7;
00208                 distance+=100;
00209                 break;
00210             case 11:
00211                 direction_new=2;
00212                 distance+=141; //racine de 2, la diagonale d'un carré
00213                 break;
00214             case 9:
00215                 direction_new=4;
00216                 distance+=141;
00217                 break;
00218             case -9:
00219                 direction_new=8;
00220                 distance+=141;
00221                 break;
00222             case -11:
00223                 direction_new=6;
00224                 distance+=141;
00225                 break;
00226         }
00227 
00228         if(direction_new != direction) {
00229             if(direction ==0) {
00230 
00231                 push(&cmd,(int) direction_new, 0, 0, i);
00232 
00233             } else {
00234                 int a = direction-direction_new;
00235                 if(direction == 8 && direction_new == 1) a = -1;
00236                 if(direction == 1 && direction_new == 8) a = 1;
00237                 printf("%d %d, %d,%d , %d \n", a, distance,direction, direction_new,(direction-direction_new));
00238                 push(&cmd,(int) 45*a, distance, 0, i);
00239             }
00240             distance = 0;
00241             direction = direction_new;
00242             i++;
00243         }
00244         px = px_new;
00245         py = py_new;
00246 
00247     }
00248     printf("ok");
00249     i++;
00250     float a = (1+1)/(7-1);
00251     float b = 1-a*7;
00252     push(&cmd, 0, distance, 0,i);
00253 
00254 
00255     return cmd;
00256 
00257 }
00258 
00259 void afficher_terrain()
00260 {
00261     //Affiche le terrain sur le port serie
00262     printf("Terrain avec murs: \n");
00263     for(int j=0; j<=L; j++) printf("_");
00264     printf("\n");
00265     for(int i = 0; i < l; i++) {
00266         printf("|");
00267         for(int j=0; j<L; j++) {
00268 
00269             if(terrain[i][j] ==0) {
00270                 printf(" ");
00271             } else {
00272                 printf(".");
00273 
00274             }
00275         }
00276         printf("|\n");
00277     }
00278     for(int j=0; j<=L; j++) printf("¯");
00279     printf("\n\n");
00280 }
00281 
00282 
00283 void afficher_terrain(Node* points)
00284 {
00285     char terrain_tmp[l][L] = {{0}};
00286     for(int i = 0; i < l; i++) for(int j=0; j<L; j++) terrain_tmp[i][j] = terrain[i][j];
00287 
00288     int i,px,py,_ = 0;
00289     while(!isEmpty(&points)) {
00290         peek(&points,&px,&py,&_);   //on recupere les infos du point
00291         pop(&points);
00292         terrain_tmp[px][py] = 2;
00293     }
00294 
00295     //Affiche le terrain sur le port serie
00296     printf("Terrain avec murs: \n");
00297     for(int j=0; j<=L; j++) printf("_");
00298     printf("\n");
00299     for(int i = 0; i < l; i++) {
00300         printf("|");
00301         for(int j=0; j<L; j++) {
00302 
00303             if(terrain_tmp[i][j] ==0) {
00304                 printf(" ");
00305             } else if(terrain_tmp[i][j] == 2) {
00306                 printf("X");
00307             } else {
00308                 printf(".");
00309 
00310             }
00311         }
00312         printf("|\n");
00313     }
00314     for(int j=0; j<=L; j++) printf("¯");
00315     printf("\n\n");
00316 }
00317 
00318 int conversion_codage_angles(int o)
00319 {
00320 
00321     switch(o) {
00322         case 1:
00323             o = 0;
00324             break;
00325         case 2:
00326             o = 45;
00327             break;
00328         case 3:
00329             o = 90;
00330             break;
00331         case 4:
00332             o = 135;
00333             break;
00334         case 5:
00335             o = -180;
00336             break;
00337         case 6:
00338             o = -225;
00339             break;
00340         case 7:
00341             o = 270;
00342             break;
00343         case 8:
00344             o = 315;
00345             break;
00346     }
00347     return o;
00348 }
00349 
00350 void aller_a_point(int ox , int oy, int gx , int gy , int vitesse_deplacement){
00351     
00352     double angle_absolut = get_angle();
00353 
00354     calc_distances(ox,oy,gx,gy);
00355     Node* cm = (Node*)malloc(sizeof(Node)); 
00356     cm = traduction_points_commandes(trouver_chemin(ox,oy,gx,gy));
00357     
00358     //afficher_terrain(trouver_chemin(ox,oy,gx,gy));
00359     
00360     int o,d,_ = 0;
00361     pop(&cm);
00362     peek(&cm,&o,&d,&_);     //on recupere les infos de direction 
00363     pop(&cm);
00364     o = conversion_codage_angles(o);
00365     printf("rotation %d, avancement de %d\n",o,d);
00366     angle_absolut += o;
00367     
00368     //test_rotation_abs(o,100);
00369     wait(2);
00370     
00371     while(!isEmpty(&cm)){ 
00372         peek(&cm,&o,&d,&_);     //on recupere les infos de direction 
00373         pop(&cm);
00374         printf("rotation %d, avancement de %d\n",o,d);
00375         if(d != 0 && o != 0) {
00376             test_ligne_droite(10*d, -vitesse_deplacement);
00377             wait(2);
00378             angle_absolut += o;
00379             test_rotation_abs(angle_absolut,50);
00380             wait(2);
00381             }
00382     }
00383  }
00384 
00385 
00386 
00387