ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Map/Map.cpp@109:53918ba98306, 2015-05-05 (annotated)

Committer:
Jagang
Date:
Tue May 05 16:35:53 2015 +0000
Revision:
109:53918ba98306
Parent:
12:235b5545ff41
Child:
117:f8c147141a0c
IA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 109:53918ba98306 1 #include "Map.h"
Jagang 109:53918ba98306 2
Jagang 109:53918ba98306 3 #include "mbed.h"
Jagang 109:53918ba98306 4 #include "Obs_circle.h"
Jagang 109:53918ba98306 5 #include "Obs_rect.h"
Jagang 109:53918ba98306 6
Jagang 109:53918ba98306 7 extern Serial logger;
Jagang 109:53918ba98306 8
Jagang 109:53918ba98306 9 Map::Map()
Jagang 109:53918ba98306 10 {
Jagang 109:53918ba98306 11
Jagang 109:53918ba98306 12 }
Jagang 109:53918ba98306 13
Jagang 109:53918ba98306 14 Map::~Map()
Jagang 109:53918ba98306 15 {
Jagang 109:53918ba98306 16 for(unsigned int i=0;i<obstacles.size();i++)
Jagang 109:53918ba98306 17 delete obstacles[i];
Jagang 109:53918ba98306 18 }
Jagang 109:53918ba98306 19
Jagang 109:53918ba98306 20 void Map::build()
Jagang 109:53918ba98306 21 {
Jagang 109:53918ba98306 22 //obstacles.push_back(new Obs_circle(ROBOTRADIUS,2,2,1));
Jagang 109:53918ba98306 23 obstacles.push_back(new Obs_rect(ROBOTRADIUS,-100,-100,2100,0)); // MG
Jagang 109:53918ba98306 24
Jagang 109:53918ba98306 25
Jagang 109:53918ba98306 26 obstacles.push_back(new Obs_rect(ROBOTRADIUS,778,0,800,400)); // M1
Jagang 109:53918ba98306 27 obstacles.push_back(new Obs_rect(ROBOTRADIUS,1200,0,1222,400)); // M2
Jagang 109:53918ba98306 28 obstacles.push_back(new Obs_rect(ROBOTRADIUS,800,0,1200,70)); // M3
Jagang 109:53918ba98306 29
Jagang 109:53918ba98306 30 }
Jagang 109:53918ba98306 31
Jagang 109:53918ba98306 32 int Map::getHeight(float x, float y)
Jagang 109:53918ba98306 33 {
Jagang 109:53918ba98306 34 int height = 0;
Jagang 109:53918ba98306 35 for(unsigned int i=0;i<obstacles.size();i++)
Jagang 109:53918ba98306 36 height += obstacles[i]->height(x,y);
Jagang 109:53918ba98306 37 return height;
Jagang 109:53918ba98306 38 }
Jagang 109:53918ba98306 39
Jagang 109:53918ba98306 40 float dist(Point *p1,Point *p2)
Jagang 109:53918ba98306 41 {
Jagang 109:53918ba98306 42 int dx = p1->getx()-p2->getx();
Jagang 109:53918ba98306 43 if(dx<0) dx=-dx;
Jagang 109:53918ba98306 44 int dy = p1->gety()-p2->gety();
Jagang 109:53918ba98306 45 if(dy<0) dy=-dy;
Jagang 109:53918ba98306 46 return sqrt((float)dx*dx+dy*dy);
Jagang 109:53918ba98306 47 }
Jagang 109:53918ba98306 48
Jagang 109:53918ba98306 49 char Map::AStar(float x, float y, float goal_x, float goal_y, float mpc)
Jagang 109:53918ba98306 50 {
Jagang 109:53918ba98306 51 /*! http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003 !*/
Jagang 109:53918ba98306 52 //float dx,dy; // Permet de diminuer l'erreur par rapport au centre des cases
Jagang 109:53918ba98306 53 //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
Jagang 109:53918ba98306 54
Jagang 109:53918ba98306 55 Point goal(goal_x/mpc,goal_y/mpc);
Jagang 109:53918ba98306 56 if(getHeight(goal_x,goal_y) >= 32000)
Jagang 109:53918ba98306 57 {
Jagang 109:53918ba98306 58 #if LOG_LEVEL >= 2
Jagang 109:53918ba98306 59 logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
Jagang 109:53918ba98306 60 #endif
Jagang 109:53918ba98306 61 return 4;
Jagang 109:53918ba98306 62 }
Jagang 109:53918ba98306 63
Jagang 109:53918ba98306 64 if(getHeight(x,y) >= 32000)
Jagang 109:53918ba98306 65 {
Jagang 109:53918ba98306 66 #if LOG_LEVEL >= 2
Jagang 109:53918ba98306 67 logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
Jagang 109:53918ba98306 68 #endif
Jagang 109:53918ba98306 69 return 5;
Jagang 109:53918ba98306 70 }
Jagang 109:53918ba98306 71
Jagang 109:53918ba98306 72
Jagang 109:53918ba98306 73 unsigned int i=0;
Jagang 109:53918ba98306 74
Jagang 109:53918ba98306 75 std::vector<Point*> openList;
Jagang 109:53918ba98306 76 openList.push_back(new Point(x/mpc,y/mpc));
Jagang 109:53918ba98306 77 openList[0]->setG(0);
Jagang 109:53918ba98306 78 openList[0]->setH(dist(openList[0],&goal));
Jagang 109:53918ba98306 79 openList[0]->setParent();
Jagang 109:53918ba98306 80
Jagang 109:53918ba98306 81 std::vector<Point*> closeList;
Jagang 109:53918ba98306 82
Jagang 109:53918ba98306 83 Point* current;
Jagang 109:53918ba98306 84 do
Jagang 109:53918ba98306 85 {
Jagang 109:53918ba98306 86
Jagang 109:53918ba98306 87 // On cherche le plus petit F dans la liste ouverte
Jagang 109:53918ba98306 88 current = openList[0];
Jagang 109:53918ba98306 89
Jagang 109:53918ba98306 90 unsigned int pos = 0;
Jagang 109:53918ba98306 91 for(i=0;i<openList.size();i++)
Jagang 109:53918ba98306 92 if(openList[i]->getF() < current->getF())
Jagang 109:53918ba98306 93 {
Jagang 109:53918ba98306 94 current = openList[i];
Jagang 109:53918ba98306 95 pos = i;
Jagang 109:53918ba98306 96 }
Jagang 109:53918ba98306 97
Jagang 109:53918ba98306 98 // On le place dans la liste fermé
Jagang 109:53918ba98306 99 closeList.push_back(current);
Jagang 109:53918ba98306 100 openList.erase(openList.begin()+pos);
Jagang 109:53918ba98306 101
Jagang 109:53918ba98306 102 #if LOG_LEVEL >= 4 && LOG_ASTAR
Jagang 109:53918ba98306 103 logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
Jagang 109:53918ba98306 104 logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
Jagang 109:53918ba98306 105 #endif
Jagang 109:53918ba98306 106
Jagang 109:53918ba98306 107 // On ajoute tous ses voisins viable das la liste ouverte
Jagang 109:53918ba98306 108 for(int dx=-1;dx<=1;dx++)
Jagang 109:53918ba98306 109 {
Jagang 109:53918ba98306 110 for(int dy=-1;dy<=1;dy++)
Jagang 109:53918ba98306 111 {
Jagang 109:53918ba98306 112 if(dx==0 && dy==0) continue;
Jagang 109:53918ba98306 113
Jagang 109:53918ba98306 114 Point *p = new Point(current->getx()+dx,current->gety()+dy);
Jagang 109:53918ba98306 115 if(p == 0) // Overload !!!
Jagang 109:53918ba98306 116 {
Jagang 109:53918ba98306 117 for(unsigned int i=0;i<openList.size();i++)
Jagang 109:53918ba98306 118 delete openList[i];
Jagang 109:53918ba98306 119 for(unsigned int i=0;i<closeList.size();i++)
Jagang 109:53918ba98306 120 delete closeList[i];
Jagang 109:53918ba98306 121
Jagang 109:53918ba98306 122 path.clear();
Jagang 109:53918ba98306 123
Jagang 109:53918ba98306 124 #if LOG_LEVEL >= 1
Jagang 109:53918ba98306 125 logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
Jagang 109:53918ba98306 126 #endif
Jagang 109:53918ba98306 127
Jagang 109:53918ba98306 128 return 3;
Jagang 109:53918ba98306 129 }
Jagang 109:53918ba98306 130
Jagang 109:53918ba98306 131 if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
Jagang 109:53918ba98306 132 {
Jagang 109:53918ba98306 133 delete p;
Jagang 109:53918ba98306 134 continue;
Jagang 109:53918ba98306 135 }
Jagang 109:53918ba98306 136
Jagang 109:53918ba98306 137 int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
Jagang 109:53918ba98306 138 if(height>=32000) // On ignore le point, il n'est pas accessible
Jagang 109:53918ba98306 139 {
Jagang 109:53918ba98306 140 delete p;
Jagang 109:53918ba98306 141 continue; // On ignore le point si il est déjà dans la liste fermé
Jagang 109:53918ba98306 142 }
Jagang 109:53918ba98306 143
Jagang 109:53918ba98306 144 if(p->in(openList,pos))
Jagang 109:53918ba98306 145 {
Jagang 109:53918ba98306 146 if(dx == 0 || dy == 0) // Mouvement non diagonnal
Jagang 109:53918ba98306 147 {
Jagang 109:53918ba98306 148 if(current->getG() + NDIAG_COST < openList[pos]->getG())
Jagang 109:53918ba98306 149 {
Jagang 109:53918ba98306 150 openList[pos]->setG(current->getG() + NDIAG_COST);
Jagang 109:53918ba98306 151 openList[pos]->setParent(current);
Jagang 109:53918ba98306 152 }
Jagang 109:53918ba98306 153 }
Jagang 109:53918ba98306 154 else
Jagang 109:53918ba98306 155 {
Jagang 109:53918ba98306 156 if(current->getG() + DIAG_COST < openList[pos]->getG())
Jagang 109:53918ba98306 157 {
Jagang 109:53918ba98306 158 openList[pos]->setG(current->getG() + DIAG_COST);
Jagang 109:53918ba98306 159 openList[pos]->setParent(current);
Jagang 109:53918ba98306 160 }
Jagang 109:53918ba98306 161 }
Jagang 109:53918ba98306 162
Jagang 109:53918ba98306 163 delete p;
Jagang 109:53918ba98306 164 }
Jagang 109:53918ba98306 165 else
Jagang 109:53918ba98306 166 {
Jagang 109:53918ba98306 167 openList.push_back(p);
Jagang 109:53918ba98306 168 if(dx == 0 || dy == 0) // Mouvement non diagonnal
Jagang 109:53918ba98306 169 p->setG(current->getG() + NDIAG_COST);
Jagang 109:53918ba98306 170 else
Jagang 109:53918ba98306 171 p->setG(current->getG() + DIAG_COST);
Jagang 109:53918ba98306 172 p->setH(height + dist(p,&goal));
Jagang 109:53918ba98306 173 p->setParent(current);
Jagang 109:53918ba98306 174 }
Jagang 109:53918ba98306 175 }
Jagang 109:53918ba98306 176 }
Jagang 109:53918ba98306 177 }
Jagang 109:53918ba98306 178 while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
Jagang 109:53918ba98306 179
Jagang 109:53918ba98306 180
Jagang 109:53918ba98306 181 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 182 logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
Jagang 109:53918ba98306 183 #endif
Jagang 109:53918ba98306 184
Jagang 109:53918ba98306 185 if(!openList.empty())
Jagang 109:53918ba98306 186 {
Jagang 109:53918ba98306 187 for(i=0;i<openList.size();i++)
Jagang 109:53918ba98306 188 delete openList[i];
Jagang 109:53918ba98306 189
Jagang 109:53918ba98306 190 // On reconstruit le chemin
Jagang 109:53918ba98306 191 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 192 logger.printf("[info - pathfinder] Recontruction du chemin ... ");
Jagang 109:53918ba98306 193 #endif
Jagang 109:53918ba98306 194 path.clear();
Jagang 109:53918ba98306 195 Point* c = closeList.back();
Jagang 109:53918ba98306 196 while(c != 0)
Jagang 109:53918ba98306 197 {
Jagang 109:53918ba98306 198 path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
Jagang 109:53918ba98306 199 c = c->getParent();
Jagang 109:53918ba98306 200 }
Jagang 109:53918ba98306 201
Jagang 109:53918ba98306 202 for(i=0;i<closeList.size();i++)
Jagang 109:53918ba98306 203 delete closeList[i];
Jagang 109:53918ba98306 204
Jagang 109:53918ba98306 205 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 206 logger.printf("[done] %d elements\r\n",path.size());
Jagang 109:53918ba98306 207 logger.printf("[info - pathfinder] Tendage du chemin ... ");
Jagang 109:53918ba98306 208 #endif
Jagang 109:53918ba98306 209
Jagang 109:53918ba98306 210
Jagang 109:53918ba98306 211 // Algo de 'tendage' du chemin
Jagang 109:53918ba98306 212 bool continuer = true;
Jagang 109:53918ba98306 213 std::vector<SimplePoint> tempPath;
Jagang 109:53918ba98306 214 while(continuer)
Jagang 109:53918ba98306 215 {
Jagang 109:53918ba98306 216 for(i=1;i<path.size();i+=2)
Jagang 109:53918ba98306 217 {
Jagang 109:53918ba98306 218 tempPath.push_back(path[i-1]);
Jagang 109:53918ba98306 219 int height = (getHeight(path[i-1].x*mpc,path[i-1].y*mpc)+getHeight(path[i+1].x*mpc,path[i+1].y*mpc))/2;
Jagang 109:53918ba98306 220
Jagang 109:53918ba98306 221 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 222 logger.printf("[info - tendeur] Testing (%.3f,%.3f) %d\r\n",path[i].x,path[i].y,height);
Jagang 109:53918ba98306 223 #endif
Jagang 109:53918ba98306 224
Jagang 109:53918ba98306 225 if(path[i-1].x!=path[i+1].x)
Jagang 109:53918ba98306 226 {
Jagang 109:53918ba98306 227 // y=a*x+b
Jagang 109:53918ba98306 228 float a = (path[i-1].y-path[i+1].y)/(path[i-1].x-path[i+1].x);
Jagang 109:53918ba98306 229 float b = path[i-1].y - a*path[i-1].x;
Jagang 109:53918ba98306 230
Jagang 109:53918ba98306 231 float step = (mpc*0.1f)*cos(atan(a));
Jagang 109:53918ba98306 232
Jagang 109:53918ba98306 233 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 234 logger.printf("[info - tendeur] X: a=%.2f b=%.2f\r\n",a,b);
Jagang 109:53918ba98306 235 logger.printf("[info - tendeur] x [%.3f,%.3f] step=%.6f\r\n",path[i-1].x,path[i+1].x,step);
Jagang 109:53918ba98306 236 #endif
Jagang 109:53918ba98306 237 float x;
Jagang 109:53918ba98306 238 for(x=min(path[i-1].x,path[i+1].x);x<max(path[i-1].x,path[i+1].x);x+=step)
Jagang 109:53918ba98306 239 {
Jagang 109:53918ba98306 240 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 241 logger.printf("%.3f\t%.3f\t%.3f\r\n",getHeight(x,(a*x+b)),x,(a*x+b));
Jagang 109:53918ba98306 242 #endif
Jagang 109:53918ba98306 243 if(getHeight(x,(a*x+b)) > height) // Ca ne passe pas sans ce point
Jagang 109:53918ba98306 244 {
Jagang 109:53918ba98306 245 tempPath.push_back(path[i]);
Jagang 109:53918ba98306 246
Jagang 109:53918ba98306 247 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 248 logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 249 #endif
Jagang 109:53918ba98306 250 break;
Jagang 109:53918ba98306 251 }
Jagang 109:53918ba98306 252 }
Jagang 109:53918ba98306 253 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 254 if(x >= max(path[i-1].x,path[i+1].x))
Jagang 109:53918ba98306 255 {
Jagang 109:53918ba98306 256 logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 257 }
Jagang 109:53918ba98306 258 #endif
Jagang 109:53918ba98306 259 }
Jagang 109:53918ba98306 260 else if(path[i-1].y!=path[i+1].y)
Jagang 109:53918ba98306 261 {
Jagang 109:53918ba98306 262 // x=a*y+b
Jagang 109:53918ba98306 263 float a = (path[i-1].x-path[i+1].x)/(path[i-1].y-path[i+1].y);
Jagang 109:53918ba98306 264 float b = path[i-1].x - a*path[i-1].y;
Jagang 109:53918ba98306 265
Jagang 109:53918ba98306 266 float step = (mpc*0.1f)*cos(atan(a));
Jagang 109:53918ba98306 267
Jagang 109:53918ba98306 268 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 269 logger.printf("[info - tendeur] Y: a=%.2f b=%.2f\r\n",a,b);
Jagang 109:53918ba98306 270 logger.printf("[info - tendeur] y [%.3f,%.3f] step=%.6f\r\n",path[i-1].y,path[i+1].y,step);
Jagang 109:53918ba98306 271 #endif
Jagang 109:53918ba98306 272 float y;
Jagang 109:53918ba98306 273 for(y=min(path[i-1].y,path[i+1].y);y<max(path[i-1].y,path[i+1].y);y+=step)
Jagang 109:53918ba98306 274 {
Jagang 109:53918ba98306 275 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 276 logger.printf("%.3f\t%.3f\t%.3f\t%.3f\r\n",getHeight((a*y+b),y),height,(a*y+b),y);
Jagang 109:53918ba98306 277 #endif
Jagang 109:53918ba98306 278 if(getHeight((a*y+b),y) > height) // Ca ne passe pas sans ce point
Jagang 109:53918ba98306 279 {
Jagang 109:53918ba98306 280 tempPath.push_back(path[i]);
Jagang 109:53918ba98306 281
Jagang 109:53918ba98306 282 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 283 logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 284 #endif
Jagang 109:53918ba98306 285 break;
Jagang 109:53918ba98306 286 }
Jagang 109:53918ba98306 287 }
Jagang 109:53918ba98306 288 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 289 if(y >= max(path[i-1].y,path[i+1].y))
Jagang 109:53918ba98306 290 {
Jagang 109:53918ba98306 291 logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 292 }
Jagang 109:53918ba98306 293 #endif
Jagang 109:53918ba98306 294 }
Jagang 109:53918ba98306 295 }
Jagang 109:53918ba98306 296 if(path.back() != tempPath.back())
Jagang 109:53918ba98306 297 tempPath.push_back(path.back());
Jagang 109:53918ba98306 298
Jagang 109:53918ba98306 299 if(tempPath.size() < path.size()) // On a réussit a enlever un point
Jagang 109:53918ba98306 300 {
Jagang 109:53918ba98306 301 path.clear();
Jagang 109:53918ba98306 302 for(i=0;i<tempPath.size();i++)
Jagang 109:53918ba98306 303 path.push_back(tempPath[i]);
Jagang 109:53918ba98306 304 continuer = true;
Jagang 109:53918ba98306 305 }
Jagang 109:53918ba98306 306 else
Jagang 109:53918ba98306 307 continuer = false;
Jagang 109:53918ba98306 308
Jagang 109:53918ba98306 309 tempPath.clear();
Jagang 109:53918ba98306 310 }
Jagang 109:53918ba98306 311
Jagang 109:53918ba98306 312 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 313 logger.printf("[done] %d elements\r\n",path.size());
Jagang 109:53918ba98306 314 #endif
Jagang 109:53918ba98306 315
Jagang 109:53918ba98306 316 return 1;
Jagang 109:53918ba98306 317 }
Jagang 109:53918ba98306 318 else
Jagang 109:53918ba98306 319 {
Jagang 109:53918ba98306 320 for(i=0;i<openList.size();i++)
Jagang 109:53918ba98306 321 delete openList[i];
Jagang 109:53918ba98306 322 for(i=0;i<closeList.size();i++)
Jagang 109:53918ba98306 323 delete closeList[i];
Jagang 109:53918ba98306 324
Jagang 109:53918ba98306 325 path.clear();
Jagang 109:53918ba98306 326
Jagang 109:53918ba98306 327 return 2;
Jagang 109:53918ba98306 328 }
Jagang 109:53918ba98306 329 }
Jagang 109:53918ba98306 330
Jagang 109:53918ba98306 331
Jagang 109:53918ba98306 332
Jagang 109:53918ba98306 333
Jagang 109:53918ba98306 334
Jagang 109:53918ba98306 335