Romain Ame
Voili voilou
Dependencies: RoboClaw StepperMotor mbed
Fork of
Robot2016_2-0
by 
ARES
Diff: Map/Map.cpp
- Revision:
- 11:9c70a7f4d7aa
- Child:
- 12:5355aed288b0
diff -r ae3178aa94e9 -r 9c70a7f4d7aa Map/Map.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Map.cpp Tue Jan 05 15:48:25 2016 +0100
@@ -0,0 +1,412 @@
+#include "Map.h"
+
+#include "Obs_circle.h"
+#include "Obs_rect.h"
+
+#ifdef CODEBLOCK
+ using namespace std;
+ #include <iostream>
+ #include <fstream>
+#else
+ #include "mbed.h"
+ extern Serial logger;
+#endif
+#include <math.h>
+
+Map::Map()
+{
+
+}
+
+Map::~Map()
+{
+ for(unsigned int i=0;i<obstacles.size();i++)
+ delete obstacles[i];
+}
+
+void Map::build()
+{
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MG,-100,-100,2100,0));// MG
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MB,2100,-100,2000,3100));// MB
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MH,-100,-100,0,3100));// MH
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MD,2100,3000,-100,3100));// MD
+
+
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M1,778,0,800,400));// M1
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M2,1200,0,1222,400));// M2
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M3,800,0,1200,70));// M3
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M4,1222,2600,1200,3000));// M4
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M5,1200,2930,800,3000));// M5
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M6,800,2600,778,3000));// M6
+
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_E,580,967,0,2033));// E
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_S,2000,1200,1900,1800));// S
+
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D1,70,265,0,335));// D1
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D2,70,565,0,636));// D2
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D3,70,2365,0,2435));// D3
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D4,70,2665,0,2735));// D4
+
+
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC1,800,910,35));// PC1
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC3,1650,1500,35));// PC3
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC2,1750,250,35));// PC2
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC4,1750,3000-250,35));// PC4
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC5,800,3000-910,35));// PC5
+
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P1,200,90,30));// P1
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P2,100,850,30));// P2
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P3,200,850,30));// P3
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P4,1355,870,30));// P4
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P5,1750,90,30));// P5
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P6,1850,90,30));// P6
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P7,1770,1100,30));// P7
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P8,1400,1300,30));// P8
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P9,200,3000-90,30));// P9
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P10,100,3000-850,30));// P10
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P11,200,3000-850,30));// P11
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P12,1355,3000-870,30));// P12
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P13,1750,3000-90,30));// P13
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P14,1850,3000-90,30));// P14
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1400,3000-1300,30));// P16
+}
+
+int Map::getHeight(float x, float y)
+{
+ int height = 0;
+ for(unsigned int i=0;i<obstacles.size();i++)
+ height += obstacles[i]->height(x,y);
+ return height;
+}
+
+float dist(Point *p1,Point *p2)
+{
+ int dx = p1->getx()-p2->getx();
+ if(dx<0) dx=-dx;
+ int dy = p1->gety()-p2->gety();
+ if(dy<0) dy=-dy;
+ return sqrt((float)dx*dx+dy*dy);
+}
+
+char Map::AStar(float x, float y, float goal_x, float goal_y, float mpc)
+{
+ /*! http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003 !*/
+ //float dx,dy; // Permet de diminuer l'erreur par rapport au centre des cases
+ //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
+
+ path.clear();
+
+ Point goal(goal_x/mpc,goal_y/mpc);
+ if(getHeight(goal_x,goal_y) >= 32000)
+ {
+ #if LOG_LEVEL >= 2
+ logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
+ #endif
+ return 4;
+ }
+
+ if(getHeight(x,y) >= 32000)
+ {
+ #if LOG_LEVEL >= 2
+ logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
+ #endif
+ return 5;
+ }
+
+
+ unsigned int i=0;
+ unsigned int instanceDePoint=0;
+
+ std::vector<Point*> openList;
+ openList.push_back(new Point(x/mpc,y/mpc));
+ openList[0]->setG(0);
+ openList[0]->setH(dist(openList[0],&goal));
+ openList[0]->setParent();
+
+ std::vector<Point*> closeList;
+
+ Point* current;
+ do
+ {
+
+ // On cherche le plus petit F dans la liste ouverte
+ current = openList[0];
+
+ unsigned int pos = 0;
+ for(i=0;i<openList.size();i++)
+ if(openList[i]->getF() < current->getF())
+ {
+ current = openList[i];
+ pos = i;
+ }
+
+ // On le place dans la liste fermé
+ closeList.push_back(current);
+ openList.erase(openList.begin()+pos);
+
+ #if LOG_LEVEL >= 4 && LOG_ASTAR
+ logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
+ logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
+ #endif
+
+ // On ajoute tous ses voisins viable das la liste ouverte
+ for(int dx=-1;dx<=1;dx++)
+ {
+ for(int dy=-1;dy<=1;dy++)
+ {
+ if(dx==0 && dy==0)
+ continue;
+ if(!(dx == 0 || dy == 0)) // On skype les mouvement diagoneaux
+ continue;
+
+ Point *p = new Point(current->getx()+dx,current->gety()+dy);
+ instanceDePoint++;
+ if(p == 0 || instanceDePoint >= MAXPOINT) // Overload !!!
+ {
+ for(unsigned int i=0;i<openList.size();i++)
+ delete openList[i];
+ for(unsigned int i=0;i<closeList.size();i++)
+ delete closeList[i];
+
+ path.clear();
+
+ #if LOG_LEVEL >= 1
+ logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
+ #endif
+
+ return 3;
+ }
+
+ if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
+ {
+ delete p;
+ continue;
+ }
+
+ int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
+ if(height>=32000) // On ignore le point, il n'est pas accessible
+ {
+ delete p;
+ continue; // On ignore le point si il est déjà dans la liste fermé
+ }
+
+ if(p->in(openList,pos))
+ {
+ if(dx == 0 || dy == 0) // Mouvement non diagonnal
+ {
+ if(current->getG() + NDIAG_COST < openList[pos]->getG())
+ {
+ openList[pos]->setG(current->getG() + NDIAG_COST);
+ openList[pos]->setParent(current);
+ }
+ }
+ else
+ {
+ if(current->getG() + DIAG_COST < openList[pos]->getG())
+ {
+ openList[pos]->setG(current->getG() + DIAG_COST);
+ openList[pos]->setParent(current);
+ }
+ }
+
+ delete p;
+ }
+ else
+ {
+ openList.push_back(p);
+ if(dx == 0 || dy == 0) // Mouvement non diagonnal
+ p->setG(current->getG() + NDIAG_COST);
+ else
+ p->setG(current->getG() + DIAG_COST);
+ p->setH(height + dist(p,&goal));
+ p->setParent(current);
+ }
+ }
+ }
+ }
+ while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
+
+
+ #if LOG_LEVEL >= 3
+ logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
+ #endif
+
+ if(!openList.empty())
+ {
+ #ifdef CODEBLOCK
+ ofstream f_openlist("openlist.txt");
+ for(i=0;i<openList.size();i++)
+ {
+ f_openlist << i << "," << openList[i]->getx()*mpc << "," << openList[i]->gety()*mpc << endl;
+ delete openList[i];
+ }
+ f_openlist.close();
+
+ ofstream f_closelist("closelist.txt");
+ for(i=0;i<closeList.size();i++)
+ {
+ f_closelist << i << "," << closeList[i]->getx()*mpc << "," << closeList[i]->gety()*mpc << endl;
+ }
+ f_closelist.close();
+ #endif
+
+ // On reconstruit le chemin
+ #if LOG_LEVEL >= 3
+ logger.printf("[info - pathfinder] Recontruction du chemin ... ");
+ #endif
+ path.clear();
+ Point* c = closeList.back();
+ while(c != 0)
+ {
+ path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
+ c = c->getParent();
+ }
+
+ path.front().x=x;
+ path.front().y=y;
+
+ path.back().x=goal_x;
+ path.back().y=goal_y;
+
+ #ifdef CODEBLOCK
+ ofstream f_path("path.txt");
+ for(i=0;i<path.size();i++)
+ {
+ f_path << i << "," << path[i].x << "," << path[i].y << endl;
+ }
+ f_path.close();
+ #endif
+
+ for(i=0;i<closeList.size();i++)
+ delete closeList[i];
+
+ #if LOG_LEVEL >= 3
+ logger.printf("[done] %d elements\r\n",path.size());
+ logger.printf("[info - pathfinder] Tendage du chemin ... ");
+ #endif
+
+
+ // Algo de 'tendage' du chemin
+ bool continuer = true;
+ unsigned int pointValide = 0;
+
+ #ifdef CODEBLOCK
+ ofstream f_tendage("tendage.txtt");
+ #endif // CODEBLOCK
+
+
+ while(continuer)
+ {
+ continuer = false;
+
+ for(unsigned int i1=pointValide;i1<path.size();i1++)
+ {
+ bool necessaire = false;
+
+ for(unsigned int i2=i1+2;i2<path.size();i2++)
+ {
+ //cout << "Entre " << i1 << " et " << i2 << endl;
+ if(path[i1].x!=path[i2].x && atan((path[i1].y-path[i2].y)/(path[i1].x-path[i2].x)) < PI/4)
+ {
+ float a = (path[i1].y-path[i2].y)/(path[i1].x-path[i2].x);
+ float b = path[i1].y - a*path[i1].x;
+ float step = (mpc*0.5f)*cos(atan(a));
+
+ necessaire = false;
+
+ for(x=min(path[i1].x,path[i2].x);x<max(path[i1].x,path[i2].x);x+=step)
+ {
+ y=a*x+b;
+
+ #ifdef CODEBLOCK
+ f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
+ #endif // CODEBLOCK
+
+ if(getHeight(x,y) >= 32000)
+ {
+ necessaire = true;
+ break;
+ }
+ }
+
+ if(!necessaire)
+ {
+ // Ca n'a pas touché, on peut supprimmer le point entre les deux
+ continuer = true;
+ path.erase(path.begin()+i2-1);
+ }
+ else
+ pointValide++;
+ break;
+ }
+ else
+ {
+ // x=a*y+b
+ float a = (path[i1].x-path[i2].x)/(path[i1].y-path[i2].y);
+ float b = path[i1].x - a*path[i1].y;
+ float step = (mpc*0.5f)*cos(atan(a));
+
+ necessaire = false;
+
+ for(y=min(path[i1].y,path[i2].y);y<max(path[i1].y,path[i2].y);y+=step)
+ {
+ x=a*y+b;
+
+ #ifdef CODEBLOCK
+ f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
+ #endif // CODEBLOCK
+
+ if(getHeight(x,y) >= 32000)
+ {
+ necessaire = true;
+ break;
+ }
+ }
+
+ if(!necessaire)
+ {
+ // Ca n'a pas touché, on peut supprimmer le point entre les deux
+ continuer = true;
+ path.erase(path.begin()+i2-1);
+ }
+ else
+ pointValide++;
+ break;
+
+ }
+ }
+ if(continuer)
+ break;
+ }
+ }
+
+ #ifdef CODEBLOCK
+ f_tendage.close();
+ #endif
+
+
+
+ #if LOG_LEVEL >= 3
+ logger.printf("[done] %d elements\r\n",path.size());
+ #endif
+
+ return 1;
+ }
+ else
+ {
+ for(i=0;i<openList.size();i++)
+ delete openList[i];
+
+ for(i=0;i<closeList.size();i++)
+ delete closeList[i];
+
+ path.clear();
+
+ return 2;
+ }
+}
+
+
+
+