Romain Ame
Voili voilou
Dependencies: RoboClaw StepperMotor mbed
Fork of
Robot2016_2-0
by 
ARES
Diff: Map/Map.cpp
- Revision:
- 46:8eae88c45a78
- Parent:
- 45:b53ae54062c6
--- a/Map/Map.cpp Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,412 +0,0 @@
-#include "Map.h"
-#include "../Odometry/Odometry.h"
-#include "Obstacles/Obs_circle.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,700,1000,100));// P16
- obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,600,100));// P16
- obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,1000,100));// 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;
- }
-}
-
-
-
-