From 8f63fa287cf35d7c960ce8d2ec4fc3a498967d17 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Tue, 16 Apr 2013 16:21:54 +0200 Subject: Do not allow to go into obstacles exclusion circle, except when target is the center of the cake. More dev required to make it more generic. --- digital/io-hub/src/common-cc/path.cc | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) (limited to 'digital/io-hub/src/common-cc/path.cc') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index f83a670f..cc5e290a 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -2,6 +2,7 @@ // io-hub - Modular Input/Output. {{{ // // Copyright (C) 2013 Nicolas Schodet +// Copyright (C) 2013 Olivier Lanneluc // // APBTeam: // Web: http://apbteam.org/ @@ -88,8 +89,8 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num) DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, num); - /* Enlarge the obstacle radius by at least the robot size */ - r += BOT_SIZE_RADIUS * 4 / 3; + /* Enlarge the obstacle radius by the robot size and clearance area width */ + r += BOT_SIZE_SIDE + Obstacles::clearance_mm; /* Store obstacle */ //assert(obstacles_nb < PATH_OBSTACLES_NB); @@ -134,10 +135,10 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num) } #ifdef HOST - // Plot obstacle points + /* Plot obstacle points */ robot->hardware.simu_report.pos( &navpoints[PATH_RESERVED_NAVPOINTS_NB], navpoints_nb-PATH_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID); #if 0 - // Draw the last obstacle + /* Draw the last obstacle */ navpoints[navpoints_nb] = navpoints[navpoints_nb - num]; robot->hardware.simu_report.path( &navpoints[navpoints_nb - num], num + 1); #endif @@ -236,19 +237,28 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) /* Check every obstacle */ for(int j=0; j validated w=%u\n", weight); -- cgit v1.2.3