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/obstacles.cc | 2 +- digital/io-hub/src/common-cc/path.cc | 26 ++++++++++++++++++-------- digital/io-hub/src/common-cc/path.hh | 1 + 3 files changed, 20 insertions(+), 9 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc index fc8a5204..5e20a611 100644 --- a/digital/io-hub/src/common-cc/obstacles.cc +++ b/digital/io-hub/src/common-cc/obstacles.cc @@ -154,7 +154,7 @@ Obstacles::add_obstacles (Path &path) const if (obstacles_[i].valid) { path.obstacle (index++, obstacles_[i].pos, - obstacle_radius_mm + clearance_mm /*+ BOT_SIZE_SIDE*/); + obstacle_radius_mm /*+ clearance_mm + BOT_SIZE_SIDE*/); } } } 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); diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh index 54822e22..61326aac 100644 --- a/digital/io-hub/src/common-cc/path.hh +++ b/digital/io-hub/src/common-cc/path.hh @@ -3,6 +3,7 @@ // io-hub - Modular Input/Output. {{{ // // Copyright (C) 2013 Nicolas Schodet +// Copyright (C) 2013 Olivier Lanneluc // // APBTeam: // Web: http://apbteam.org/ -- cgit v1.2.3