ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Map/Map.cpp@117:f8c147141a0c, 2015-05-06 (annotated)

Committer:
Jagang
Date:
Wed May 06 11:22:17 2015 +0000
Revision:
117:f8c147141a0c
Parent:
109:53918ba98306
Child:
119:c45efcd706d9
Objectif pince et depot

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 117:f8c147141a0c 74 unsigned int instanceDePoint=0;
Jagang 109:53918ba98306 75
Jagang 109:53918ba98306 76 std::vector<Point*> openList;
Jagang 109:53918ba98306 77 openList.push_back(new Point(x/mpc,y/mpc));
Jagang 109:53918ba98306 78 openList[0]->setG(0);
Jagang 109:53918ba98306 79 openList[0]->setH(dist(openList[0],&goal));
Jagang 109:53918ba98306 80 openList[0]->setParent();
Jagang 109:53918ba98306 81
Jagang 109:53918ba98306 82 std::vector<Point*> closeList;
Jagang 109:53918ba98306 83
Jagang 109:53918ba98306 84 Point* current;
Jagang 109:53918ba98306 85 do
Jagang 109:53918ba98306 86 {
Jagang 109:53918ba98306 87
Jagang 109:53918ba98306 88 // On cherche le plus petit F dans la liste ouverte
Jagang 109:53918ba98306 89 current = openList[0];
Jagang 109:53918ba98306 90
Jagang 109:53918ba98306 91 unsigned int pos = 0;
Jagang 109:53918ba98306 92 for(i=0;i<openList.size();i++)
Jagang 109:53918ba98306 93 if(openList[i]->getF() < current->getF())
Jagang 109:53918ba98306 94 {
Jagang 109:53918ba98306 95 current = openList[i];
Jagang 109:53918ba98306 96 pos = i;
Jagang 109:53918ba98306 97 }
Jagang 109:53918ba98306 98
Jagang 109:53918ba98306 99 // On le place dans la liste fermé
Jagang 109:53918ba98306 100 closeList.push_back(current);
Jagang 109:53918ba98306 101 openList.erase(openList.begin()+pos);
Jagang 109:53918ba98306 102
Jagang 109:53918ba98306 103 #if LOG_LEVEL >= 4 && LOG_ASTAR
Jagang 109:53918ba98306 104 logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
Jagang 109:53918ba98306 105 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 106 #endif
Jagang 109:53918ba98306 107
Jagang 109:53918ba98306 108 // On ajoute tous ses voisins viable das la liste ouverte
Jagang 109:53918ba98306 109 for(int dx=-1;dx<=1;dx++)
Jagang 109:53918ba98306 110 {
Jagang 109:53918ba98306 111 for(int dy=-1;dy<=1;dy++)
Jagang 109:53918ba98306 112 {
Jagang 109:53918ba98306 113 if(dx==0 && dy==0) continue;
Jagang 109:53918ba98306 114
Jagang 109:53918ba98306 115 Point *p = new Point(current->getx()+dx,current->gety()+dy);
Jagang 117:f8c147141a0c 116 instanceDePoint++;
Jagang 117:f8c147141a0c 117 if(p == 0 || instanceDePoint >= MAXPOINT) // Overload !!!
Jagang 109:53918ba98306 118 {
Jagang 109:53918ba98306 119 for(unsigned int i=0;i<openList.size();i++)
Jagang 109:53918ba98306 120 delete openList[i];
Jagang 109:53918ba98306 121 for(unsigned int i=0;i<closeList.size();i++)
Jagang 109:53918ba98306 122 delete closeList[i];
Jagang 109:53918ba98306 123
Jagang 109:53918ba98306 124 path.clear();
Jagang 109:53918ba98306 125
Jagang 109:53918ba98306 126 #if LOG_LEVEL >= 1
Jagang 109:53918ba98306 127 logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
Jagang 109:53918ba98306 128 #endif
Jagang 109:53918ba98306 129
Jagang 109:53918ba98306 130 return 3;
Jagang 109:53918ba98306 131 }
Jagang 109:53918ba98306 132
Jagang 109:53918ba98306 133 if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
Jagang 109:53918ba98306 134 {
Jagang 109:53918ba98306 135 delete p;
Jagang 109:53918ba98306 136 continue;
Jagang 109:53918ba98306 137 }
Jagang 109:53918ba98306 138
Jagang 109:53918ba98306 139 int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
Jagang 109:53918ba98306 140 if(height>=32000) // On ignore le point, il n'est pas accessible
Jagang 109:53918ba98306 141 {
Jagang 109:53918ba98306 142 delete p;
Jagang 109:53918ba98306 143 continue; // On ignore le point si il est déjà dans la liste fermé
Jagang 109:53918ba98306 144 }
Jagang 109:53918ba98306 145
Jagang 109:53918ba98306 146 if(p->in(openList,pos))
Jagang 109:53918ba98306 147 {
Jagang 109:53918ba98306 148 if(dx == 0 || dy == 0) // Mouvement non diagonnal
Jagang 109:53918ba98306 149 {
Jagang 109:53918ba98306 150 if(current->getG() + NDIAG_COST < openList[pos]->getG())
Jagang 109:53918ba98306 151 {
Jagang 109:53918ba98306 152 openList[pos]->setG(current->getG() + NDIAG_COST);
Jagang 109:53918ba98306 153 openList[pos]->setParent(current);
Jagang 109:53918ba98306 154 }
Jagang 109:53918ba98306 155 }
Jagang 109:53918ba98306 156 else
Jagang 109:53918ba98306 157 {
Jagang 109:53918ba98306 158 if(current->getG() + DIAG_COST < openList[pos]->getG())
Jagang 109:53918ba98306 159 {
Jagang 109:53918ba98306 160 openList[pos]->setG(current->getG() + DIAG_COST);
Jagang 109:53918ba98306 161 openList[pos]->setParent(current);
Jagang 109:53918ba98306 162 }
Jagang 109:53918ba98306 163 }
Jagang 109:53918ba98306 164
Jagang 109:53918ba98306 165 delete p;
Jagang 109:53918ba98306 166 }
Jagang 109:53918ba98306 167 else
Jagang 109:53918ba98306 168 {
Jagang 109:53918ba98306 169 openList.push_back(p);
Jagang 109:53918ba98306 170 if(dx == 0 || dy == 0) // Mouvement non diagonnal
Jagang 109:53918ba98306 171 p->setG(current->getG() + NDIAG_COST);
Jagang 109:53918ba98306 172 else
Jagang 109:53918ba98306 173 p->setG(current->getG() + DIAG_COST);
Jagang 109:53918ba98306 174 p->setH(height + dist(p,&goal));
Jagang 109:53918ba98306 175 p->setParent(current);
Jagang 109:53918ba98306 176 }
Jagang 109:53918ba98306 177 }
Jagang 109:53918ba98306 178 }
Jagang 109:53918ba98306 179 }
Jagang 109:53918ba98306 180 while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
Jagang 109:53918ba98306 181
Jagang 109:53918ba98306 182
Jagang 109:53918ba98306 183 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 184 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 185 #endif
Jagang 109:53918ba98306 186
Jagang 109:53918ba98306 187 if(!openList.empty())
Jagang 109:53918ba98306 188 {
Jagang 109:53918ba98306 189 for(i=0;i<openList.size();i++)
Jagang 109:53918ba98306 190 delete openList[i];
Jagang 109:53918ba98306 191
Jagang 109:53918ba98306 192 // On reconstruit le chemin
Jagang 109:53918ba98306 193 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 194 logger.printf("[info - pathfinder] Recontruction du chemin ... ");
Jagang 109:53918ba98306 195 #endif
Jagang 109:53918ba98306 196 path.clear();
Jagang 109:53918ba98306 197 Point* c = closeList.back();
Jagang 109:53918ba98306 198 while(c != 0)
Jagang 109:53918ba98306 199 {
Jagang 109:53918ba98306 200 path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
Jagang 109:53918ba98306 201 c = c->getParent();
Jagang 109:53918ba98306 202 }
Jagang 109:53918ba98306 203
Jagang 109:53918ba98306 204 for(i=0;i<closeList.size();i++)
Jagang 109:53918ba98306 205 delete closeList[i];
Jagang 109:53918ba98306 206
Jagang 109:53918ba98306 207 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 208 logger.printf("[done] %d elements\r\n",path.size());
Jagang 109:53918ba98306 209 logger.printf("[info - pathfinder] Tendage du chemin ... ");
Jagang 109:53918ba98306 210 #endif
Jagang 109:53918ba98306 211
Jagang 109:53918ba98306 212
Jagang 109:53918ba98306 213 // Algo de 'tendage' du chemin
Jagang 109:53918ba98306 214 bool continuer = true;
Jagang 109:53918ba98306 215 std::vector<SimplePoint> tempPath;
Jagang 109:53918ba98306 216 while(continuer)
Jagang 109:53918ba98306 217 {
Jagang 109:53918ba98306 218 for(i=1;i<path.size();i+=2)
Jagang 109:53918ba98306 219 {
Jagang 109:53918ba98306 220 tempPath.push_back(path[i-1]);
Jagang 109:53918ba98306 221 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 222
Jagang 109:53918ba98306 223 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 224 logger.printf("[info - tendeur] Testing (%.3f,%.3f) %d\r\n",path[i].x,path[i].y,height);
Jagang 109:53918ba98306 225 #endif
Jagang 109:53918ba98306 226
Jagang 109:53918ba98306 227 if(path[i-1].x!=path[i+1].x)
Jagang 109:53918ba98306 228 {
Jagang 109:53918ba98306 229 // y=a*x+b
Jagang 109:53918ba98306 230 float a = (path[i-1].y-path[i+1].y)/(path[i-1].x-path[i+1].x);
Jagang 109:53918ba98306 231 float b = path[i-1].y - a*path[i-1].x;
Jagang 109:53918ba98306 232
Jagang 109:53918ba98306 233 float step = (mpc*0.1f)*cos(atan(a));
Jagang 109:53918ba98306 234
Jagang 109:53918ba98306 235 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 236 logger.printf("[info - tendeur] X: a=%.2f b=%.2f\r\n",a,b);
Jagang 109:53918ba98306 237 logger.printf("[info - tendeur] x [%.3f,%.3f] step=%.6f\r\n",path[i-1].x,path[i+1].x,step);
Jagang 109:53918ba98306 238 #endif
Jagang 109:53918ba98306 239 float x;
Jagang 109:53918ba98306 240 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 241 {
Jagang 109:53918ba98306 242 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 243 logger.printf("%.3f\t%.3f\t%.3f\r\n",getHeight(x,(a*x+b)),x,(a*x+b));
Jagang 109:53918ba98306 244 #endif
Jagang 109:53918ba98306 245 if(getHeight(x,(a*x+b)) > height) // Ca ne passe pas sans ce point
Jagang 109:53918ba98306 246 {
Jagang 109:53918ba98306 247 tempPath.push_back(path[i]);
Jagang 109:53918ba98306 248
Jagang 109:53918ba98306 249 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 250 logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 251 #endif
Jagang 109:53918ba98306 252 break;
Jagang 109:53918ba98306 253 }
Jagang 109:53918ba98306 254 }
Jagang 109:53918ba98306 255 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 256 if(x >= max(path[i-1].x,path[i+1].x))
Jagang 109:53918ba98306 257 {
Jagang 109:53918ba98306 258 logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 259 }
Jagang 109:53918ba98306 260 #endif
Jagang 109:53918ba98306 261 }
Jagang 109:53918ba98306 262 else if(path[i-1].y!=path[i+1].y)
Jagang 109:53918ba98306 263 {
Jagang 109:53918ba98306 264 // x=a*y+b
Jagang 109:53918ba98306 265 float a = (path[i-1].x-path[i+1].x)/(path[i-1].y-path[i+1].y);
Jagang 109:53918ba98306 266 float b = path[i-1].x - a*path[i-1].y;
Jagang 109:53918ba98306 267
Jagang 109:53918ba98306 268 float step = (mpc*0.1f)*cos(atan(a));
Jagang 109:53918ba98306 269
Jagang 109:53918ba98306 270 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 271 logger.printf("[info - tendeur] Y: a=%.2f b=%.2f\r\n",a,b);
Jagang 109:53918ba98306 272 logger.printf("[info - tendeur] y [%.3f,%.3f] step=%.6f\r\n",path[i-1].y,path[i+1].y,step);
Jagang 109:53918ba98306 273 #endif
Jagang 109:53918ba98306 274 float y;
Jagang 109:53918ba98306 275 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 276 {
Jagang 109:53918ba98306 277 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 278 logger.printf("%.3f\t%.3f\t%.3f\t%.3f\r\n",getHeight((a*y+b),y),height,(a*y+b),y);
Jagang 109:53918ba98306 279 #endif
Jagang 109:53918ba98306 280 if(getHeight((a*y+b),y) > height) // Ca ne passe pas sans ce point
Jagang 109:53918ba98306 281 {
Jagang 109:53918ba98306 282 tempPath.push_back(path[i]);
Jagang 109:53918ba98306 283
Jagang 109:53918ba98306 284 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 285 logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 286 #endif
Jagang 109:53918ba98306 287 break;
Jagang 109:53918ba98306 288 }
Jagang 109:53918ba98306 289 }
Jagang 109:53918ba98306 290 #if LOG_LEVEL >= 4 & LOG_TENDEUR
Jagang 109:53918ba98306 291 if(y >= max(path[i-1].y,path[i+1].y))
Jagang 109:53918ba98306 292 {
Jagang 109:53918ba98306 293 logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
Jagang 109:53918ba98306 294 }
Jagang 109:53918ba98306 295 #endif
Jagang 109:53918ba98306 296 }
Jagang 109:53918ba98306 297 }
Jagang 109:53918ba98306 298 if(path.back() != tempPath.back())
Jagang 109:53918ba98306 299 tempPath.push_back(path.back());
Jagang 109:53918ba98306 300
Jagang 109:53918ba98306 301 if(tempPath.size() < path.size()) // On a réussit a enlever un point
Jagang 109:53918ba98306 302 {
Jagang 109:53918ba98306 303 path.clear();
Jagang 109:53918ba98306 304 for(i=0;i<tempPath.size();i++)
Jagang 109:53918ba98306 305 path.push_back(tempPath[i]);
Jagang 109:53918ba98306 306 continuer = true;
Jagang 109:53918ba98306 307 }
Jagang 109:53918ba98306 308 else
Jagang 109:53918ba98306 309 continuer = false;
Jagang 109:53918ba98306 310
Jagang 109:53918ba98306 311 tempPath.clear();
Jagang 109:53918ba98306 312 }
Jagang 109:53918ba98306 313
Jagang 109:53918ba98306 314 #if LOG_LEVEL >= 3
Jagang 109:53918ba98306 315 logger.printf("[done] %d elements\r\n",path.size());
Jagang 109:53918ba98306 316 #endif
Jagang 109:53918ba98306 317
Jagang 109:53918ba98306 318 return 1;
Jagang 109:53918ba98306 319 }
Jagang 109:53918ba98306 320 else
Jagang 109:53918ba98306 321 {
Jagang 109:53918ba98306 322 for(i=0;i<openList.size();i++)
Jagang 109:53918ba98306 323 delete openList[i];
Jagang 109:53918ba98306 324 for(i=0;i<closeList.size();i++)
Jagang 109:53918ba98306 325 delete closeList[i];
Jagang 109:53918ba98306 326
Jagang 109:53918ba98306 327 path.clear();
Jagang 109:53918ba98306 328
Jagang 109:53918ba98306 329 return 2;
Jagang 109:53918ba98306 330 }
Jagang 109:53918ba98306 331 }
Jagang 109:53918ba98306 332
Jagang 109:53918ba98306 333
Jagang 109:53918ba98306 334
Jagang 109:53918ba98306 335
Jagang 109:53918ba98306 336
Jagang 109:53918ba98306 337