ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

Map/map.cpp@56:4fd9636dfb36, 2016-05-04 (annotated)

Committer:
IceTeam
Date:
Wed May 04 19:43:53 2016 +0200
Revision:
56:4fd9636dfb36
Parent:
55:dfeee9e5c0af
Child:
57:86f491f5b25d
Changement dans la gestion des objectifs

Who changed what in which revision?

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