ARES
Robot's source code
Dependencies: mbed
Diff: Map/Map.cpp
- Revision:
- 123:55e5e9acc541
- Parent:
- 119:c45efcd706d9
--- a/Map/Map.cpp Thu May 07 14:18:07 2015 +0000
+++ b/Map/Map.cpp Mon May 11 20:32:11 2015 +0000
@@ -1,10 +1,17 @@
#include "Map.h"
-#include "mbed.h"
#include "Obs_circle.h"
#include "Obs_rect.h"
-extern Serial logger;
+#ifdef CODEBLOCK
+ using namespace std;
+ #include <iostream>
+ #include <fstream>
+#else
+ #include "mbed.h"
+ extern Serial logger;
+#endif
+#include <math.h>
Map::Map()
{
@@ -19,14 +26,50 @@
void Map::build()
{
- //obstacles.push_back(new Obs_circle(ROBOTRADIUS,2,2,1));
- obstacles.push_back(new Obs_rect(ROBOTRADIUS,-100,-100,2100,0)); // MG
+ 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_rect(ROBOTRADIUS,778,0,800,400)); // M1
- obstacles.push_back(new Obs_rect(ROBOTRADIUS,1200,0,1222,400)); // M2
- obstacles.push_back(new Obs_rect(ROBOTRADIUS,800,0,1200,70)); // M3
+ 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)
@@ -53,22 +96,12 @@
//dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
path.clear();
- #if LOG_LEVEL >= 3
- logger.printf("[info - pathfinder] Start point (%.1f,%.1f)\r\n",x,y);
- logger.printf("[info - pathfinder] Goal point (%.1f,%.1f)\r\n",goal_x,goal_y);
- #endif
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 (%.1f,%.1f) %d\r\n",goal_x,goal_y,getHeight(goal_x,goal_y));
-
- for(unsigned int i=0;i<obstacles.size();i++)
- {
- logger.printf("%d %d\r\n",i,obstacles[i]->height(goal_x,goal_y));
- }
-
+ logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
#endif
return 4;
}
@@ -122,7 +155,10 @@
{
for(int dy=-1;dy<=1;dy++)
{
- if(dx==0 && dy==0) continue;
+ 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++;
@@ -198,8 +234,22 @@
if(!openList.empty())
{
- for(i=0;i<openList.size();i++)
- delete openList[i];
+ #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
@@ -213,6 +263,21 @@
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];
@@ -224,105 +289,104 @@
// Algo de 'tendage' du chemin
bool continuer = true;
- std::vector<SimplePoint> tempPath;
+ unsigned int pointValide = 0;
+
+ #ifdef CODEBLOCK
+ ofstream f_tendage("tendage.txtt");
+ #endif // CODEBLOCK
+
+
while(continuer)
{
- for(i=1;i<path.size();i+=2)
+ continuer = false;
+
+ for(unsigned int i1=pointValide;i1<path.size();i1++)
{
- tempPath.push_back(path[i-1]);
- 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;
-
- #if LOG_LEVEL >= 4 & LOG_TENDEUR
- logger.printf("[info - tendeur] Testing (%.3f,%.3f) %d\r\n",path[i].x,path[i].y,height);
- #endif
+ bool necessaire = false;
- if(path[i-1].x!=path[i+1].x)
- {
- // y=a*x+b
- float a = (path[i-1].y-path[i+1].y)/(path[i-1].x-path[i+1].x);
- float b = path[i-1].y - a*path[i-1].x;
-
- float step = (mpc*0.5f)*cos(atan(a));
-
- #if LOG_LEVEL >= 4 & LOG_TENDEUR
- logger.printf("[info - tendeur] X: a=%.2f b=%.2f\r\n",a,b);
- logger.printf("[info - tendeur] x [%.3f,%.3f] step=%.6f\r\n",path[i-1].x,path[i+1].x,step);
- #endif
- float x;
- for(x=min(path[i-1].x,path[i+1].x);x<max(path[i-1].x,path[i+1].x);x+=step)
- {
- #if LOG_LEVEL >= 5 & LOG_TENDEUR
- logger.printf("%.3f\t%.3f\t%.3f\r\n",getHeight(x,(a*x+b)),x,(a*x+b));
- #endif
- if(getHeight(x,(a*x+b)) > height) // Ca ne passe pas sans ce point
- {
- tempPath.push_back(path[i]);
-
- #if LOG_LEVEL >= 4 & LOG_TENDEUR
- logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
- #endif
- break;
- }
- }
- #if LOG_LEVEL >= 4 & LOG_TENDEUR
- if(x >= max(path[i-1].x,path[i+1].x))
- {
- logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
- }
- #endif
- }
- else if(path[i-1].y!=path[i+1].y)
+ for(unsigned int i2=i1+2;i2<path.size();i2++)
{
- // x=a*y+b
- float a = (path[i-1].x-path[i+1].x)/(path[i-1].y-path[i+1].y);
- float b = path[i-1].x - a*path[i-1].y;
-
- float step = (mpc*0.5f)*cos(atan(a));
-
- #if LOG_LEVEL >= 4 & LOG_TENDEUR
- logger.printf("[info - tendeur] Y: a=%.2f b=%.2f\r\n",a,b);
- logger.printf("[info - tendeur] y [%.3f,%.3f] step=%.6f\r\n",path[i-1].y,path[i+1].y,step);
- #endif
- float y;
- for(y=min(path[i-1].y,path[i+1].y);y<max(path[i-1].y,path[i+1].y);y+=step)
+ //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)
{
- #if LOG_LEVEL >= 5 & LOG_TENDEUR
- logger.printf("%.3f\t%.3f\t%.3f\t%.3f\r\n",getHeight((a*y+b),y),height,(a*y+b),y);
- #endif
- if(getHeight((a*y+b),y) > height) // Ca ne passe pas sans ce point
+ 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)
{
- tempPath.push_back(path[i]);
+ y=a*x+b;
+
+ #ifdef CODEBLOCK
+ f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
+ #endif // CODEBLOCK
- #if LOG_LEVEL >= 4 & LOG_TENDEUR
- logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
- #endif
- break;
+ 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 LOG_LEVEL >= 4 & LOG_TENDEUR
- if(y >= max(path[i-1].y,path[i+1].y))
+ 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)
{
- logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
+ x=a*y+b;
+
+ #ifdef CODEBLOCK
+ f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
+ #endif // CODEBLOCK
+
+ if(getHeight(x,y) >= 32000)
+ {
+ necessaire = true;
+ break;
+ }
}
- #endif
+
+ 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;
}
- if(path.back() != tempPath.back())
- tempPath.push_back(path.back());
-
- if(tempPath.size() < path.size()) // On a réussit a enlever un point
- {
- path.clear();
- for(i=0;i<tempPath.size();i++)
- path.push_back(tempPath[i]);
- continuer = true;
- }
- else
- continuer = false;
-
- tempPath.clear();
}
+ #ifdef CODEBLOCK
+ f_tendage.close();
+ #endif
+
+
+
#if LOG_LEVEL >= 3
logger.printf("[done] %d elements\r\n",path.size());
#endif
@@ -345,5 +409,3 @@
-
-
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 14 Dec 2014 |
| Imports: | 17 |
| Forks: | 0 |
| Commits: | 124 |
| Dependents: | 0 |
| Dependencies: | 1 |
| Followers: | 8 |
