ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

Map/map.cpp@57:86f491f5b25d, 2016-05-04 (annotated)

Committer:
IceTeam
Date:
Wed May 04 21:04:50 2016 +0200
Revision:
57:86f491f5b25d
Parent:
56:4fd9636dfb36
Child:
60:8d2320a54a32
Rajout de la classe Objectif, petits changements dans le fonctionnement de map

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 47:be4eebf40568 1 #include "map.h"
IceTeam 47:be4eebf40568 2
IceTeam 57:86f491f5b25d 3 /* Dernier Changement : Romain 20h30 */
IceTeam 57:86f491f5b25d 4
IceTeam 47:be4eebf40568 5 map::map (Odometry* nodo) : Codo(nodo) {
IceTeam 47:be4eebf40568 6 }
IceTeam 47:be4eebf40568 7
IceTeam 54:be4ea8da9057 8 void map::Build (int couleur, int formation) {
IceTeam 57:86f491f5b25d 9 if (couleur == VERT) {
IceTeam 57:86f491f5b25d 10 max_x_table = 2800;
IceTeam 57:86f491f5b25d 11 max_y_table = 1800;
IceTeam 57:86f491f5b25d 12 min_x_table = 0;
IceTeam 57:86f491f5b25d 13 min_y_table = 0;
IceTeam 57:86f491f5b25d 14 }
IceTeam 56:4fd9636dfb36 15
IceTeam 54:be4ea8da9057 16 addObs(obsCarr (800, 100, 100, 15));
IceTeam 54:be4ea8da9057 17 addObs(obsCarr (2200, 100, 100, 15));
IceTeam 52:98f8a6ccb6ae 18
IceTeam 54:be4ea8da9057 19 addObs(obsCarr (1500, 750, 1100, 15));
IceTeam 54:be4ea8da9057 20 addObs(obsCarr (1500, 1050, 20, 300));
IceTeam 52:98f8a6ccb6ae 21
IceTeam 57:86f491f5b25d 22 if (formation == 1) {
IceTeam 54:be4ea8da9057 23 addObs(obsCarr (0, 2000, 250, 150)); // Coté haut droite
IceTeam 54:be4ea8da9057 24 addObs(obsCarr (200, 2000, 200, 50));
IceTeam 52:98f8a6ccb6ae 25
IceTeam 54:be4ea8da9057 26 addObs(obsCarr (3000, 2000, 250, 150)); // Coté bas droite
IceTeam 54:be4ea8da9057 27 addObs(obsCarr (2800, 2000, 200, 50));
IceTeam 52:98f8a6ccb6ae 28
IceTeam 54:be4ea8da9057 29 addObs(obsCarr (200, 2000-450, 40, 40)); // Coquillages du haut droit
IceTeam 54:be4ea8da9057 30 addObs(obsCarr (200, 2000-750, 40, 40));
IceTeam 52:98f8a6ccb6ae 31
IceTeam 54:be4ea8da9057 32 addObs(obsCarr (900, 2000-550, 40, 40));
IceTeam 54:be4ea8da9057 33 addObs(obsCarr (1200, 2000-350, 40, 40)); // Coquillages du milieu/haut
IceTeam 52:98f8a6ccb6ae 34
IceTeam 54:be4ea8da9057 35 addObs(obsCarr (1500, 2000-550, 40, 40));
IceTeam 54:be4ea8da9057 36 addObs(obsCarr (1500, 2000-350, 40, 40));
IceTeam 52:98f8a6ccb6ae 37
IceTeam 54:be4ea8da9057 38 addObs(obsCarr (3000-900, 2000-550, 40, 40));
IceTeam 54:be4ea8da9057 39 addObs(obsCarr (3000-1200, 2000-350, 40, 40));
IceTeam 52:98f8a6ccb6ae 40
IceTeam 54:be4ea8da9057 41 addObs(obsCarr (3000-200, 2000-450, 40, 40)); // Coquillages du bas droite
IceTeam 54:be4ea8da9057 42 addObs(obsCarr (3000-200, 2000-750, 40, 40));
IceTeam 52:98f8a6ccb6ae 43 }
IceTeam 52:98f8a6ccb6ae 44 else {
IceTeam 54:be4ea8da9057 45 addObs(obsCarr (1250, 1000, 220, 220));
IceTeam 54:be4ea8da9057 46 addObs(obsCarr (1500, 750, 220, 220));
IceTeam 54:be4ea8da9057 47 addObs(obsCarr (1500, 1250, 220, 220));
IceTeam 52:98f8a6ccb6ae 48 }
IceTeam 52:98f8a6ccb6ae 49 }
IceTeam 52:98f8a6ccb6ae 50
IceTeam 47:be4eebf40568 51 void map::addObs (obsCarr nobs) {
IceTeam 47:be4eebf40568 52 obs.push_back (nobs);
IceTeam 47:be4eebf40568 53 }
IceTeam 47:be4eebf40568 54
IceTeam 47:be4eebf40568 55 void map::FindWay (float depX, float depY, float arrX, float arrY) {
IceTeam 47:be4eebf40568 56 point depart(depX, depY);
IceTeam 47:be4eebf40568 57 point arrivee(arrX, arrY);
IceTeam 47:be4eebf40568 58 FindWay(depart, arrivee);
IceTeam 47:be4eebf40568 59 }
IceTeam 47:be4eebf40568 60
IceTeam 47:be4eebf40568 61 void map::FindWay (point dep, point arr) {
IceTeam 47:be4eebf40568 62 //logger.printf("On a cherche un chemin\n\r");
IceTeam 47:be4eebf40568 63 nVector<pointParcours> open;
IceTeam 47:be4eebf40568 64 nVector<pointParcours> close;
IceTeam 47:be4eebf40568 65 points4 tmp;
IceTeam 47:be4eebf40568 66 bool val[4] = {true,true,true,true};
IceTeam 47:be4eebf40568 67 int os = obs.size ();
IceTeam 47:be4eebf40568 68 int i, j;
IceTeam 47:be4eebf40568 69 bool ended=false; // On teste tous les points ajoutes dans l'open list pour savoir s'il y a intersection avec un obstacle. Ended passe à true quand aucun ne coupe un obstacle.
IceTeam 47:be4eebf40568 70 endedParc = false;
IceTeam 47:be4eebf40568 71
IceTeam 47:be4eebf40568 72 path.clear();
IceTeam 47:be4eebf40568 73
IceTeam 47:be4eebf40568 74 pointParcours depP (dep, NULL, arr);
IceTeam 47:be4eebf40568 75 int indTMP1=0; // Le point actuel
IceTeam 47:be4eebf40568 76 int PointEnding = 0;
IceTeam 47:be4eebf40568 77 open.push_back (depP);
IceTeam 47:be4eebf40568 78
IceTeam 47:be4eebf40568 79 while (!ended && !open.empty ()) {
IceTeam 47:be4eebf40568 80 for (i = 0; i < open.size (); ++i) {
IceTeam 47:be4eebf40568 81 if (open[i].getP2 () < open[indTMP1].getP2 ())
IceTeam 47:be4eebf40568 82 indTMP1 = i;
IceTeam 47:be4eebf40568 83 }
IceTeam 47:be4eebf40568 84
IceTeam 47:be4eebf40568 85 close.push_first (open[indTMP1]);
IceTeam 47:be4eebf40568 86 open.erase (indTMP1);
IceTeam 47:be4eebf40568 87 indTMP1 = 0;
IceTeam 47:be4eebf40568 88
IceTeam 47:be4eebf40568 89 ended = true;
IceTeam 47:be4eebf40568 90 for (i = 0; i < os; ++i) {
IceTeam 47:be4eebf40568 91 if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
IceTeam 47:be4eebf40568 92 ended = false;
IceTeam 47:be4eebf40568 93 tmp = obs[i].getPoints ();
IceTeam 47:be4eebf40568 94
IceTeam 56:4fd9636dfb36 95 // On vérifie si le point est sur la table
IceTeam 56:4fd9636dfb36 96 if (tmp.p0.getX() < min_x_table || tmp.p0.getY() < min_y_table || tmp.p0.getX() > max_x_table || tmp.p0.getY() > max_y_table)
IceTeam 56:4fd9636dfb36 97 val[0] = false;
IceTeam 47:be4eebf40568 98 // On vérifie si le point croise un obstacle
IceTeam 56:4fd9636dfb36 99 for (j = 0; j < os && val[0]; ++j)
IceTeam 47:be4eebf40568 100 if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
IceTeam 47:be4eebf40568 101 val[0] = false;
IceTeam 47:be4eebf40568 102 // On vérifie si le point existe déjà dans la liste ouverte
IceTeam 56:4fd9636dfb36 103 for (j = 0; j < open.size () && val[0]; ++j) {
IceTeam 47:be4eebf40568 104 if (open[j] == tmp.p0)
IceTeam 47:be4eebf40568 105 val[0] = false;
IceTeam 47:be4eebf40568 106 }
IceTeam 47:be4eebf40568 107 // On vérifie si le point existe déjà dans la liste fermée
IceTeam 56:4fd9636dfb36 108 for (j = 0; j < close.size () && val[0]; ++j) {
IceTeam 47:be4eebf40568 109 if (close[j] == tmp.p0)
IceTeam 47:be4eebf40568 110 val[0] = false;
IceTeam 47:be4eebf40568 111 }
IceTeam 47:be4eebf40568 112 if (val[0]) {
IceTeam 47:be4eebf40568 113 open.push_back (pointParcours (tmp.p0, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 114 }
IceTeam 47:be4eebf40568 115
IceTeam 47:be4eebf40568 116 // On repete l'operation pour le second point
IceTeam 56:4fd9636dfb36 117 if (tmp.p1.getX() < min_x_table || tmp.p1.getY() < min_y_table || tmp.p1.getX() > max_x_table || tmp.p1.getY() > max_y_table)
IceTeam 56:4fd9636dfb36 118 val[1] = false;
IceTeam 56:4fd9636dfb36 119 for (j = 0; j < os && val[1]; ++j)
IceTeam 47:be4eebf40568 120 if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
IceTeam 47:be4eebf40568 121 val[1] = false;
IceTeam 56:4fd9636dfb36 122 for (j = 0; j < open.size () && val[1]; ++j) {
IceTeam 47:be4eebf40568 123 if (open[j] == tmp.p1)
IceTeam 47:be4eebf40568 124 val[1] = false;
IceTeam 47:be4eebf40568 125 }
IceTeam 56:4fd9636dfb36 126 for (j = 0; j < close.size () && val[1]; ++j) {
IceTeam 47:be4eebf40568 127 if (close[j] == tmp.p1)
IceTeam 47:be4eebf40568 128 val[1] = false;
IceTeam 47:be4eebf40568 129 }
IceTeam 47:be4eebf40568 130 if (val[1]) {
IceTeam 47:be4eebf40568 131 open.push_back (pointParcours (tmp.p1, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 132 }
IceTeam 47:be4eebf40568 133
IceTeam 47:be4eebf40568 134 // On répète l'opération pour le troisième point
IceTeam 56:4fd9636dfb36 135 if (tmp.p2.getX() < min_x_table || tmp.p2.getY() < min_y_table || tmp.p2.getX() > max_x_table || tmp.p2.getY() > max_y_table)
IceTeam 56:4fd9636dfb36 136 val[2] = false;
IceTeam 56:4fd9636dfb36 137 for (j = 0; j < os && val[2]; ++j)
IceTeam 47:be4eebf40568 138 if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
IceTeam 47:be4eebf40568 139 val[2] = false;
IceTeam 56:4fd9636dfb36 140 for (j = 0; j < open.size () && val[2]; ++j) {
IceTeam 47:be4eebf40568 141 if (open[j] == tmp.p2)
IceTeam 47:be4eebf40568 142 val[2] = false;
IceTeam 47:be4eebf40568 143 }
IceTeam 56:4fd9636dfb36 144 for (j = 0; j < close.size () && val[2]; ++j) {
IceTeam 47:be4eebf40568 145 if (close[j] == tmp.p2)
IceTeam 47:be4eebf40568 146 val[2] = false;
IceTeam 47:be4eebf40568 147 }
IceTeam 47:be4eebf40568 148 if (val[2]) {
IceTeam 47:be4eebf40568 149 open.push_back (pointParcours (tmp.p2, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 150 }
IceTeam 47:be4eebf40568 151
IceTeam 47:be4eebf40568 152 // On répète l'opération pour le quatrieme point
IceTeam 56:4fd9636dfb36 153 if (tmp.p3.getX() < min_x_table || tmp.p3.getY() < min_y_table || tmp.p3.getX() > max_x_table || tmp.p3.getY() > max_y_table)
IceTeam 56:4fd9636dfb36 154 val[3] = false;
IceTeam 56:4fd9636dfb36 155 for (j = 0; j < os && val[3]; ++j)
IceTeam 47:be4eebf40568 156 if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
IceTeam 47:be4eebf40568 157 val[3] = false;
IceTeam 56:4fd9636dfb36 158 for (j = 0; j < open.size () && val[3]; ++j) {
IceTeam 47:be4eebf40568 159 if (open[j] == tmp.p3)
IceTeam 47:be4eebf40568 160 val[3] = false;
IceTeam 47:be4eebf40568 161 }
IceTeam 56:4fd9636dfb36 162 for (j = 0; j < close.size () && val[3]; ++j) {
IceTeam 47:be4eebf40568 163 if (close[j] == tmp.p3)
IceTeam 47:be4eebf40568 164 val[3] = false;
IceTeam 47:be4eebf40568 165 }
IceTeam 47:be4eebf40568 166 if (val[3]) {
IceTeam 47:be4eebf40568 167 open.push_back (pointParcours (tmp.p3, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 168 }
IceTeam 47:be4eebf40568 169
IceTeam 47:be4eebf40568 170 val[0] = true;
IceTeam 47:be4eebf40568 171 val[1] = true;
IceTeam 47:be4eebf40568 172 val[2] = true;
IceTeam 47:be4eebf40568 173 val[3] = true;
IceTeam 47:be4eebf40568 174 }
IceTeam 47:be4eebf40568 175 }
IceTeam 47:be4eebf40568 176 }
IceTeam 47:be4eebf40568 177
IceTeam 47:be4eebf40568 178 /* L'algorithme n'est pas bon. Je devrais prendre ici le plus court chemin vers l'arrivée pour ceux qui ne sont pas bloqués, et pas un aléatoire ... */
IceTeam 47:be4eebf40568 179 if (ended) {
IceTeam 47:be4eebf40568 180 pointParcours* pente;
IceTeam 47:be4eebf40568 181 pente = &close[0];
IceTeam 47:be4eebf40568 182 while (pente != NULL) {
IceTeam 47:be4eebf40568 183 path.push_first (*pente);
IceTeam 47:be4eebf40568 184 pente = pente->getPere ();
IceTeam 47:be4eebf40568 185 }
IceTeam 47:be4eebf40568 186 path.push_back (pointParcours(arr, NULL, arr));
IceTeam 47:be4eebf40568 187 path.erase(0);
IceTeam 47:be4eebf40568 188 endedParc = true;
IceTeam 47:be4eebf40568 189 /*
IceTeam 47:be4eebf40568 190 if (path.size() > 1)
IceTeam 47:be4eebf40568 191 path.erase(0);*/
IceTeam 47:be4eebf40568 192 }
IceTeam 47:be4eebf40568 193 }
IceTeam 47:be4eebf40568 194
IceTeam 47:be4eebf40568 195 void map::Execute(float XObjectif, float YObjectif) {
IceTeam 47:be4eebf40568 196 logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
IceTeam 47:be4eebf40568 197 FindWay (Codo->getX(), Codo->getY(), XObjectif, YObjectif);
IceTeam 47:be4eebf40568 198
IceTeam 47:be4eebf40568 199 if (endedParc) {
IceTeam 47:be4eebf40568 200 //logger.printf("\n\r");
IceTeam 47:be4eebf40568 201 for (int i = 0; i < path.size (); i++) {
IceTeam 47:be4eebf40568 202 logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
IceTeam 47:be4eebf40568 203 //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX()));
IceTeam 47:be4eebf40568 204 Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
IceTeam 47:be4eebf40568 205 logger.printf("Goto Fini\n\r");
IceTeam 47:be4eebf40568 206 }
IceTeam 47:be4eebf40568 207 //logger.printf("Chemin fini !\n\r");
IceTeam 47:be4eebf40568 208 }
IceTeam 47:be4eebf40568 209 else {
IceTeam 47:be4eebf40568 210 logger.printf("Chemin pas trouve ...\n\r");
IceTeam 47:be4eebf40568 211 }
IceTeam 47:be4eebf40568 212 endedParc = false;
IceTeam 57:86f491f5b25d 213 }
IceTeam 57:86f491f5b25d 214
IceTeam 57:86f491f5b25d 215 void map::Execute(float XObjectif, float YObjectif) {
IceTeam 57:86f491f5b25d 216 // logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
IceTeam 57:86f491f5b25d 217 Objectif o;
IceTeam 57:86f491f5b25d 218 FindWay (Codo->getX(), Codo->getY(), o.getX(), o.getY());
IceTeam 57:86f491f5b25d 219
IceTeam 57:86f491f5b25d 220 if (endedParc) {
IceTeam 57:86f491f5b25d 221 //logger.printf("\n\r");
IceTeam 57:86f491f5b25d 222 for (int i = 0; i < path.size (); i++) {
IceTeam 57:86f491f5b25d 223 // logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
IceTeam 57:86f491f5b25d 224 //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX()));
IceTeam 57:86f491f5b25d 225 Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
IceTeam 57:86f491f5b25d 226 // logger.printf("Goto Fini\n\r");
IceTeam 57:86f491f5b25d 227 }
IceTeam 57:86f491f5b25d 228
IceTeam 57:86f491f5b25d 229 //logger.printf("Chemin fini !\n\r");
IceTeam 57:86f491f5b25d 230 }
IceTeam 57:86f491f5b25d 231 else {
IceTeam 57:86f491f5b25d 232 logger.printf("Chemin pas trouve ...\n\r");
IceTeam 57:86f491f5b25d 233 }
IceTeam 57:86f491f5b25d 234 endedParc = false;
IceTeam 47:be4eebf40568 235 }