Diff: Map/Map.cpp

Revision:

20:30942f018252

Parent:

13:5355aed288b0

--- a/Map/Map.cpp	Thu Jan 07 15:54:49 2016 +0100
+++ b/Map/Map.cpp	Thu Jan 07 18:22:38 2016 +0000
@@ -10,6 +10,7 @@
 #else
     #include "mbed.h"
     extern Serial logger;
+    extern Serial pc;
 #endif
 #include <math.h>
 
@@ -69,7 +70,7 @@
     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,1000,1000,30));// P16
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1000+1000/20,1000+1000/20,30/20));// P16
 }
 
 int Map::getHeight(float x, float y)
@@ -98,7 +99,7 @@
     path.clear();
     
     Point goal(goal_x/mpc,goal_y/mpc);
-    if(getHeight(goal_x,goal_y) >= 32000)
+    /*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);
@@ -112,7 +113,7 @@
             logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
         #endif
         return 5;
-    }
+    }*/
     
     
     unsigned int i=0;
@@ -127,9 +128,9 @@
     std::vector<Point*> closeList;
     
     Point* current;
+    pc.printf("@Map::AStar l.130 : Debut du do\n\r");
     do
     {
-        
         // On cherche le plus petit F dans la liste ouverte
         current = openList[0];
         
@@ -151,6 +152,7 @@
         #endif
         
         // On ajoute tous ses voisins viable das la liste ouverte
+        pc.printf("@Map::AStar l.155 : Debut du for d'ajout des voisins dans la liste ouverte\n\r");
         for(int dx=-1;dx<=1;dx++)
         {
             for(int dy=-1;dy<=1;dy++)
@@ -170,10 +172,8 @@
                         delete closeList[i];
                     
                     path.clear();
-                        
-                    #if LOG_LEVEL >= 1
-                        logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
-                    #endif
+                    
+                    logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
                     
                     return 3;
                 }
@@ -185,11 +185,12 @@
                 }
                 
                 int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
-                if(height>=32000)  // On ignore le point, il n'est pas accessible
+                /*if(height>=32000)  // On ignore le point, il n'est pas accessible
                 {
+                    pc.printf("@Map::AStar l.190 : Point inaccessible\n\r");
                     delete p;
                     continue; // On ignore le point si il est déjà dans la liste fermé
-                }
+                }*/
                 
                 if(p->in(openList,pos))
                 {
@@ -224,6 +225,10 @@
                 }
             }
         }
+        pc.printf("@Map::AStar l.229 : Fin du while\n\r");
+        if(!openList.empty())
+            pc.printf("@Map::AStar l.231 : openList.size = %f\n\r", openList.size());
+        pc.printf("@Map::AStar l.233 : dist(closeList.back(),goal)=%f\n\r", dist(closeList.back(),&goal));
     }
     while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
     
@@ -231,7 +236,7 @@
     #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
-    
+    pc.printf("@Map::AStar l.155 : Debut du tendage\n\r");
     if(!openList.empty())
     {
         #ifdef CODEBLOCK
@@ -319,7 +324,7 @@
                         {
                             y=a*x+b;
                             
-                            #ifdef CODEBLOCK
+                            /*#ifdef CODEBLOCK
                                 f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
                             #endif // CODEBLOCK
                             
@@ -327,7 +332,7 @@
                             {   
                                 necessaire = true;
                                 break;
-                            }
+                            }*/
                         }
                         
                         if(!necessaire)
@@ -353,7 +358,7 @@
                         {
                             x=a*y+b;
                             
-                            #ifdef CODEBLOCK
+                            /*#ifdef CODEBLOCK
                                 f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
                             #endif // CODEBLOCK
                             
@@ -361,7 +366,7 @@
                             {
                                 necessaire = true;
                                 break;
-                            }
+                            }*/
                         }
                         
                         if(!necessaire)