Jimmy Maingam / Mbed 2 deprecated odometrieDecembre

assert1

Dependencies:   mbed X_NUCLEO_IHM02A1

chemin.cpp@3:06cbe2f6c494, 2019-03-30 (annotated)

Committer:
JimmyAREM
Date:
Sat Mar 30 15:10:23 2019 +0000
Revision:
3:06cbe2f6c494
Parent:
2:977799d72329 
odo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pierr 2:977799d72329 1 #include "mbed.h"
Pierr 2:977799d72329 2 #include "chemin.h"
Pierr 2:977799d72329 3 #include "terrain_precalc.h"
Pierr 2:977799d72329 4 #include "odometrie.h"
Pierr 2:977799d72329 5 #include "tests_moteurs.h"
Pierr 2:977799d72329 6
Pierr 2:977799d72329 7
Pierr 2:977799d72329 8 /*
Pierr 2:977799d72329 9 L'origine du terrain est
Pierr 2:977799d72329 10 y-----------------|
Pierr 2:977799d72329 11 | | | |
Pierr 2:977799d72329 12 | ¯¯¯¯¯¯¯¯¯¯¯¯¯ |
Pierr 2:977799d72329 13 | |
Pierr 2:977799d72329 14 0-----------------|> x
Pierr 2:977799d72329 15
Pierr 2:977799d72329 16 Et l'origine des angles est de x vers y positif.
Pierr 2:977799d72329 17 */
Pierr 2:977799d72329 18
Pierr 2:977799d72329 19
Pierr 2:977799d72329 20 // Function to Create A New Node
Pierr 2:977799d72329 21 Node* newNode(int x, int y, int distance, int p)
Pierr 2:977799d72329 22 {
Pierr 2:977799d72329 23 Node* temp = (Node*)malloc(sizeof(Node));
Pierr 2:977799d72329 24 temp->x = x;
Pierr 2:977799d72329 25 temp->y = y;
Pierr 2:977799d72329 26 temp->distance = distance;
Pierr 2:977799d72329 27 temp->priority = p;
Pierr 2:977799d72329 28 temp->next = NULL;
Pierr 2:977799d72329 29
Pierr 2:977799d72329 30 return temp;
Pierr 2:977799d72329 31 }
Pierr 2:977799d72329 32
Pierr 2:977799d72329 33 // Return the value at head
Pierr 2:977799d72329 34 void peek(Node** head, int*x, int*y, int*distance)
Pierr 2:977799d72329 35 {
Pierr 2:977799d72329 36 *x = (*head)->x;
Pierr 2:977799d72329 37 *y = (*head)->y;
Pierr 2:977799d72329 38 *distance = (*head)->distance;
Pierr 2:977799d72329 39 }
Pierr 2:977799d72329 40
Pierr 2:977799d72329 41 // Removes the element with the
Pierr 2:977799d72329 42 // highest priority form the list
Pierr 2:977799d72329 43 void pop(Node** head)
Pierr 2:977799d72329 44 {
Pierr 2:977799d72329 45 Node* temp = *head;
Pierr 2:977799d72329 46 (*head) = (*head)->next;
Pierr 2:977799d72329 47 free(temp);
Pierr 2:977799d72329 48 }
Pierr 2:977799d72329 49
Pierr 2:977799d72329 50
Pierr 2:977799d72329 51
Pierr 2:977799d72329 52 // Function to push according to priority
Pierr 2:977799d72329 53 void push(Node** head, int x, int y, int distance, int p)
Pierr 2:977799d72329 54 {
Pierr 2:977799d72329 55 Node* start = (*head);
Pierr 2:977799d72329 56
Pierr 2:977799d72329 57 // Create new Node
Pierr 2:977799d72329 58 Node* temp = newNode(x, y,distance, p);
Pierr 2:977799d72329 59
Pierr 2:977799d72329 60 // Special Case: The head of list has lesser
Pierr 2:977799d72329 61 // priority than new node. So insert new
Pierr 2:977799d72329 62 // node before head node and change head node.
Pierr 2:977799d72329 63 if ((*head)->priority > p) {
Pierr 2:977799d72329 64
Pierr 2:977799d72329 65 // Insert New Node before head
Pierr 2:977799d72329 66 temp->next = *head;
Pierr 2:977799d72329 67 (*head) = temp;
Pierr 2:977799d72329 68 } else {
Pierr 2:977799d72329 69
Pierr 2:977799d72329 70 // Traverse the list and find a
Pierr 2:977799d72329 71 // position to insert new node
Pierr 2:977799d72329 72 while (start->next != NULL &&
Pierr 2:977799d72329 73 start->next->priority < p) {
Pierr 2:977799d72329 74 start = start->next;
Pierr 2:977799d72329 75 }
Pierr 2:977799d72329 76
Pierr 2:977799d72329 77 // Either at the ends of the list
Pierr 2:977799d72329 78 // or at required position
Pierr 2:977799d72329 79 temp->next = start->next;
Pierr 2:977799d72329 80 start->next = temp;
Pierr 2:977799d72329 81 }
Pierr 2:977799d72329 82 }
Pierr 2:977799d72329 83
Pierr 2:977799d72329 84 // Function to check is list is empty
Pierr 2:977799d72329 85 int isEmpty(Node** head)
Pierr 2:977799d72329 86 {
Pierr 2:977799d72329 87 return (*head) == NULL;
Pierr 2:977799d72329 88 }
Pierr 2:977799d72329 89
Pierr 2:977799d72329 90
Pierr 2:977799d72329 91 void calc_distances(int ox, int oy, int gx, int gy)
Pierr 2:977799d72329 92 {
Pierr 2:977799d72329 93 //Calcule la distance de chaque point du terrain à l'objectif (gx,gy)
Pierr 2:977799d72329 94 //COmplexite en O(l*L)
Pierr 2:977799d72329 95 printf("ok\n");
Pierr 2:977799d72329 96
Pierr 2:977799d72329 97 char visite[l][L] = {{0}}; //sauvegarde les points deja visites pour eviter de passer 2 fois dessus
Pierr 2:977799d72329 98 int voisins[9][9] = {{0,1}, {-1,0}, {1,0}, {0,-1}, {1,1}, {-1,1}, {1,-1}, {-1,-1} }; //voisin d'un point
Pierr 2:977799d72329 99 int distance = 0;
Pierr 2:977799d72329 100 int px = gx;
Pierr 2:977799d72329 101 int py = gy;
Pierr 2:977799d72329 102 visite[gx][gy] = 1;
Pierr 2:977799d72329 103 Node* pq = newNode(gx,gy,distance, 0);
Pierr 2:977799d72329 104 printf("ok2\n");
Pierr 2:977799d72329 105 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
Pierr 2:977799d72329 106
Pierr 2:977799d72329 107 peek(&pq,&px,&py,&distance); //on recupere les infos du point
Pierr 2:977799d72329 108 distances[px][py] = distance;
Pierr 2:977799d72329 109 //printf("position tiree : %d , %d , de distance %d\n" , px,py,distance);
Pierr 2:977799d72329 110
Pierr 2:977799d72329 111
Pierr 2:977799d72329 112 for(int i=0; i<9; i++) {
Pierr 2:977799d72329 113 int vx = px+voisins[i][0];
Pierr 2:977799d72329 114 int vy = py+voisins[i][1];
Pierr 2:977799d72329 115
Pierr 2:977799d72329 116 if( vx >= 0 && vx < l && vy >= 0 && vy < L && terrain[vx][vy] != 1 && visite[vx][vy] == 0) {
Pierr 2:977799d72329 117 //printf("positions ajoutee : %d,%d,%d\n" , vx, vy,distance );
Pierr 2:977799d72329 118 visite[vx][vy] = 1;
Pierr 2:977799d72329 119 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
Pierr 2:977799d72329 120 }
Pierr 2:977799d72329 121 }
Pierr 2:977799d72329 122 pop(&pq); //on peut le pop puisqu'il a été visité
Pierr 2:977799d72329 123
Pierr 2:977799d72329 124 }
Pierr 2:977799d72329 125 }
Pierr 2:977799d72329 126
Pierr 2:977799d72329 127 Node* trouver_chemin(int ox, int oy, int gx, int gy)
Pierr 2:977799d72329 128 {
Pierr 2:977799d72329 129 //Trouve le meilleur chemin en se rapprochant de l'objectif
Pierr 2:977799d72329 130 //Complexité en longueur du chemin (MAX= L*l)
Pierr 2:977799d72329 131 int voisins[9][9] = {{0,1}, {-1,0}, {1,0}, {0,-1}, {1,1}, {-1,1}, {1,-1}, {-1,-1} };
Pierr 2:977799d72329 132 int distance = 0;
Pierr 2:977799d72329 133 Node* pq = newNode(ox,oy,distance, 0);
Pierr 2:977799d72329 134 int px = ox;
Pierr 2:977799d72329 135 int py = oy;
Pierr 2:977799d72329 136
Pierr 2:977799d72329 137 while(!(px==gx && py==gy)) {
Pierr 2:977799d72329 138 distance++;
Pierr 2:977799d72329 139 unsigned long long dmin = L*l;
Pierr 2:977799d72329 140 int xmin = 0;
Pierr 2:977799d72329 141 int ymin = 0;
Pierr 2:977799d72329 142 for(int i=0; i<9; i++) {
Pierr 2:977799d72329 143 int vx = px+voisins[i][0];
Pierr 2:977799d72329 144 int vy = py+voisins[i][1];
Pierr 2:977799d72329 145
Pierr 2:977799d72329 146 if( vx >= 0 && vx < l && vy >= 0 && vy < L && terrain[vx][vy] != 1 && distances[vx][vy] < dmin) {
Pierr 2:977799d72329 147 dmin = distances[vx][vy];
Pierr 2:977799d72329 148 xmin = vx;
Pierr 2:977799d72329 149 ymin = vy;
Pierr 2:977799d72329 150 }
Pierr 2:977799d72329 151 }
Pierr 2:977799d72329 152 //printf("position ajoutee chemin : %d , %d , de distance %d\n" , px,py,distance);
Pierr 2:977799d72329 153 push(&pq, xmin, ymin, distance,distance);
Pierr 2:977799d72329 154 px = xmin;
Pierr 2:977799d72329 155 py = ymin;
Pierr 2:977799d72329 156
Pierr 2:977799d72329 157 }
Pierr 2:977799d72329 158 return pq;
Pierr 2:977799d72329 159 }
Pierr 2:977799d72329 160
Pierr 2:977799d72329 161
Pierr 2:977799d72329 162 Node* traduction_points_commandes(Node* pq)
Pierr 2:977799d72329 163 {
Pierr 2:977799d72329 164 /*
Pierr 2:977799d72329 165
Pierr 2:977799d72329 166 y_____________
Pierr 2:977799d72329 167 | 9 10 11|
Pierr 2:977799d72329 168 | |
Pierr 2:977799d72329 169 | -1 R 1|
Pierr 2:977799d72329 170 | |
Pierr 2:977799d72329 171 |-11 -10 -9|
Pierr 2:977799d72329 172 ¯¯¯¯¯¯¯¯¯¯¯¯¯x
Pierr 2:977799d72329 173 ||
Pierr 2:977799d72329 174 \/
Pierr 2:977799d72329 175 ___________
Pierr 2:977799d72329 176 | 4 3 2 |
Pierr 2:977799d72329 177 | 5 R 1 |
Pierr 2:977799d72329 178 | 6 7 8 |
Pierr 2:977799d72329 179 ¯¯¯¯¯¯¯¯¯¯¯
Pierr 2:977799d72329 180 */
Pierr 2:977799d72329 181 int i,px,py,distance,direction = 0;
Pierr 2:977799d72329 182 peek(&pq,&px,&py,&distance); //on recupere les infos du point
Pierr 2:977799d72329 183 pop(&pq);
Pierr 2:977799d72329 184 Node* cmd = newNode(0,0,0,0);
Pierr 2:977799d72329 185
Pierr 2:977799d72329 186 int px_new,py_new,direction_new,_ =0;
Pierr 2:977799d72329 187
Pierr 2:977799d72329 188
Pierr 2:977799d72329 189 while(!isEmpty(&pq)) {
Pierr 2:977799d72329 190 peek(&pq,&px_new,&py_new,&_); //on recupere les infos du point
Pierr 2:977799d72329 191 pop(&pq);
Pierr 2:977799d72329 192 //printf("%d , %d , %d\n",px,py,px_new - px + 10*(py_new-py));
Pierr 2:977799d72329 193 switch(px_new - px + 10*(py_new-py)) {
Pierr 2:977799d72329 194 case 1:
Pierr 2:977799d72329 195 direction_new=1;
Pierr 2:977799d72329 196 distance+=100;
Pierr 2:977799d72329 197 break;
Pierr 2:977799d72329 198 case -1:
Pierr 2:977799d72329 199 direction_new=5;
Pierr 2:977799d72329 200 distance+=100;
Pierr 2:977799d72329 201 break;
Pierr 2:977799d72329 202 case 10:
Pierr 2:977799d72329 203 direction_new=3;
Pierr 2:977799d72329 204 distance+=100;
Pierr 2:977799d72329 205 break;
Pierr 2:977799d72329 206 case -10:
Pierr 2:977799d72329 207 direction_new=7;
Pierr 2:977799d72329 208 distance+=100;
Pierr 2:977799d72329 209 break;
Pierr 2:977799d72329 210 case 11:
Pierr 2:977799d72329 211 direction_new=2;
Pierr 2:977799d72329 212 distance+=141; //racine de 2, la diagonale d'un carré
Pierr 2:977799d72329 213 break;
Pierr 2:977799d72329 214 case 9:
Pierr 2:977799d72329 215 direction_new=4;
Pierr 2:977799d72329 216 distance+=141;
Pierr 2:977799d72329 217 break;
Pierr 2:977799d72329 218 case -9:
Pierr 2:977799d72329 219 direction_new=8;
Pierr 2:977799d72329 220 distance+=141;
Pierr 2:977799d72329 221 break;
Pierr 2:977799d72329 222 case -11:
Pierr 2:977799d72329 223 direction_new=6;
Pierr 2:977799d72329 224 distance+=141;
Pierr 2:977799d72329 225 break;
Pierr 2:977799d72329 226 }
Pierr 2:977799d72329 227
Pierr 2:977799d72329 228 if(direction_new != direction) {
Pierr 2:977799d72329 229 if(direction ==0) {
Pierr 2:977799d72329 230
Pierr 2:977799d72329 231 push(&cmd,(int) direction_new, 0, 0, i);
Pierr 2:977799d72329 232
Pierr 2:977799d72329 233 } else {
Pierr 2:977799d72329 234 int a = direction-direction_new;
Pierr 2:977799d72329 235 if(direction == 8 && direction_new == 1) a = -1;
Pierr 2:977799d72329 236 if(direction == 1 && direction_new == 8) a = 1;
Pierr 2:977799d72329 237 printf("%d %d, %d,%d , %d \n", a, distance,direction, direction_new,(direction-direction_new));
Pierr 2:977799d72329 238 push(&cmd,(int) 45*a, distance, 0, i);
Pierr 2:977799d72329 239 }
Pierr 2:977799d72329 240 distance = 0;
Pierr 2:977799d72329 241 direction = direction_new;
Pierr 2:977799d72329 242 i++;
Pierr 2:977799d72329 243 }
Pierr 2:977799d72329 244 px = px_new;
Pierr 2:977799d72329 245 py = py_new;
Pierr 2:977799d72329 246
Pierr 2:977799d72329 247 }
Pierr 2:977799d72329 248 printf("ok");
Pierr 2:977799d72329 249 i++;
Pierr 2:977799d72329 250 float a = (1+1)/(7-1);
Pierr 2:977799d72329 251 float b = 1-a*7;
Pierr 2:977799d72329 252 push(&cmd, 0, distance, 0,i);
Pierr 2:977799d72329 253
Pierr 2:977799d72329 254
Pierr 2:977799d72329 255 return cmd;
Pierr 2:977799d72329 256
Pierr 2:977799d72329 257 }
Pierr 2:977799d72329 258
Pierr 2:977799d72329 259 void afficher_terrain()
Pierr 2:977799d72329 260 {
Pierr 2:977799d72329 261 //Affiche le terrain sur le port serie
Pierr 2:977799d72329 262 printf("Terrain avec murs: \n");
Pierr 2:977799d72329 263 for(int j=0; j<=L; j++) printf("_");
Pierr 2:977799d72329 264 printf("\n");
Pierr 2:977799d72329 265 for(int i = 0; i < l; i++) {
Pierr 2:977799d72329 266 printf("|");
Pierr 2:977799d72329 267 for(int j=0; j<L; j++) {
Pierr 2:977799d72329 268
Pierr 2:977799d72329 269 if(terrain[i][j] ==0) {
Pierr 2:977799d72329 270 printf(" ");
Pierr 2:977799d72329 271 } else {
Pierr 2:977799d72329 272 printf(".");
Pierr 2:977799d72329 273
Pierr 2:977799d72329 274 }
Pierr 2:977799d72329 275 }
Pierr 2:977799d72329 276 printf("|\n");
Pierr 2:977799d72329 277 }
Pierr 2:977799d72329 278 for(int j=0; j<=L; j++) printf("¯");
Pierr 2:977799d72329 279 printf("\n\n");
Pierr 2:977799d72329 280 }
Pierr 2:977799d72329 281
Pierr 2:977799d72329 282
Pierr 2:977799d72329 283 void afficher_terrain(Node* points)
Pierr 2:977799d72329 284 {
Pierr 2:977799d72329 285 char terrain_tmp[l][L] = {{0}};
Pierr 2:977799d72329 286 for(int i = 0; i < l; i++) for(int j=0; j<L; j++) terrain_tmp[i][j] = terrain[i][j];
Pierr 2:977799d72329 287
Pierr 2:977799d72329 288 int i,px,py,_ = 0;
Pierr 2:977799d72329 289 while(!isEmpty(&points)) {
Pierr 2:977799d72329 290 peek(&points,&px,&py,&_); //on recupere les infos du point
Pierr 2:977799d72329 291 pop(&points);
Pierr 2:977799d72329 292 terrain_tmp[px][py] = 2;
Pierr 2:977799d72329 293 }
Pierr 2:977799d72329 294
Pierr 2:977799d72329 295 //Affiche le terrain sur le port serie
Pierr 2:977799d72329 296 printf("Terrain avec murs: \n");
Pierr 2:977799d72329 297 for(int j=0; j<=L; j++) printf("_");
Pierr 2:977799d72329 298 printf("\n");
Pierr 2:977799d72329 299 for(int i = 0; i < l; i++) {
Pierr 2:977799d72329 300 printf("|");
Pierr 2:977799d72329 301 for(int j=0; j<L; j++) {
Pierr 2:977799d72329 302
Pierr 2:977799d72329 303 if(terrain_tmp[i][j] ==0) {
Pierr 2:977799d72329 304 printf(" ");
Pierr 2:977799d72329 305 } else if(terrain_tmp[i][j] == 2) {
Pierr 2:977799d72329 306 printf("X");
Pierr 2:977799d72329 307 } else {
Pierr 2:977799d72329 308 printf(".");
Pierr 2:977799d72329 309
Pierr 2:977799d72329 310 }
Pierr 2:977799d72329 311 }
Pierr 2:977799d72329 312 printf("|\n");
Pierr 2:977799d72329 313 }
Pierr 2:977799d72329 314 for(int j=0; j<=L; j++) printf("¯");
Pierr 2:977799d72329 315 printf("\n\n");
Pierr 2:977799d72329 316 }
Pierr 2:977799d72329 317
Pierr 2:977799d72329 318 int conversion_codage_angles(int o)
Pierr 2:977799d72329 319 {
Pierr 2:977799d72329 320
Pierr 2:977799d72329 321 switch(o) {
Pierr 2:977799d72329 322 case 1:
Pierr 2:977799d72329 323 o = 0;
Pierr 2:977799d72329 324 break;
Pierr 2:977799d72329 325 case 2:
Pierr 2:977799d72329 326 o = 45;
Pierr 2:977799d72329 327 break;
Pierr 2:977799d72329 328 case 3:
Pierr 2:977799d72329 329 o = 90;
Pierr 2:977799d72329 330 break;
Pierr 2:977799d72329 331 case 4:
Pierr 2:977799d72329 332 o = 135;
Pierr 2:977799d72329 333 break;
Pierr 2:977799d72329 334 case 5:
Pierr 2:977799d72329 335 o = -180;
Pierr 2:977799d72329 336 break;
Pierr 2:977799d72329 337 case 6:
Pierr 2:977799d72329 338 o = -225;
Pierr 2:977799d72329 339 break;
Pierr 2:977799d72329 340 case 7:
Pierr 2:977799d72329 341 o = 270;
Pierr 2:977799d72329 342 break;
Pierr 2:977799d72329 343 case 8:
Pierr 2:977799d72329 344 o = 315;
Pierr 2:977799d72329 345 break;
Pierr 2:977799d72329 346 }
Pierr 2:977799d72329 347 return o;
Pierr 2:977799d72329 348 }
Pierr 2:977799d72329 349
Pierr 2:977799d72329 350 void aller_a_point(int ox , int oy, int gx , int gy , int vitesse_deplacement){
Pierr 2:977799d72329 351
Pierr 2:977799d72329 352 double angle_absolut = get_angle();
Pierr 2:977799d72329 353
Pierr 2:977799d72329 354 calc_distances(ox,oy,gx,gy);
Pierr 2:977799d72329 355 Node* cm = (Node*)malloc(sizeof(Node));
Pierr 2:977799d72329 356 cm = traduction_points_commandes(trouver_chemin(ox,oy,gx,gy));
Pierr 2:977799d72329 357
Pierr 2:977799d72329 358 //afficher_terrain(trouver_chemin(ox,oy,gx,gy));
Pierr 2:977799d72329 359
Pierr 2:977799d72329 360 int o,d,_ = 0;
Pierr 2:977799d72329 361 pop(&cm);
Pierr 2:977799d72329 362 peek(&cm,&o,&d,&_); //on recupere les infos de direction
Pierr 2:977799d72329 363 pop(&cm);
Pierr 2:977799d72329 364 o = conversion_codage_angles(o);
Pierr 2:977799d72329 365 printf("rotation %d, avancement de %d\n",o,d);
Pierr 2:977799d72329 366 angle_absolut += o;
Pierr 2:977799d72329 367
Pierr 2:977799d72329 368 //test_rotation_abs(o,100);
Pierr 2:977799d72329 369 wait(2);
Pierr 2:977799d72329 370
Pierr 2:977799d72329 371 while(!isEmpty(&cm)){
Pierr 2:977799d72329 372 peek(&cm,&o,&d,&_); //on recupere les infos de direction
Pierr 2:977799d72329 373 pop(&cm);
Pierr 2:977799d72329 374 printf("rotation %d, avancement de %d\n",o,d);
Pierr 2:977799d72329 375 if(d != 0 && o != 0) {
Pierr 2:977799d72329 376 test_ligne_droite(10*d, -vitesse_deplacement);
Pierr 2:977799d72329 377 wait(2);
Pierr 2:977799d72329 378 angle_absolut += o;
Pierr 2:977799d72329 379 test_rotation_abs(angle_absolut,50);
Pierr 2:977799d72329 380 wait(2);
Pierr 2:977799d72329 381 }
Pierr 2:977799d72329 382 }
Pierr 2:977799d72329 383 }
Pierr 2:977799d72329 384
Pierr 2:977799d72329 385
Pierr 2:977799d72329 386
Pierr 2:977799d72329 387