From 3c8372708ae4c9676c7102358b6858117dae2c78 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 18 Aug 2009 01:55:12 +0200 Subject: * digital/avr/modules/path (closes #81): - added escaping to avoid being blocked if inside an obstacle. - added obstacle factor to avoid a zone. --- digital/avr/modules/path/path.c | 29 +++++++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) (limited to 'digital/avr/modules/path/path.c') diff --git a/digital/avr/modules/path/path.c b/digital/avr/modules/path/path.c index 835fb30f..3b1982cb 100644 --- a/digital/avr/modules/path/path.c +++ b/digital/avr/modules/path/path.c @@ -72,6 +72,8 @@ struct path_t uint32_t margin; /** List of obstacles. */ struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + /** Escape factor, 0 if none. */ + uint8_t escape_factor; /** List of points. First two points are the destination and source * points. Then comes the obstacles points. */ struct path_point_t points[PATH_POINTS_NB]; @@ -178,7 +180,15 @@ path_compute_weight (uint8_t a, uint8_t b) - d * d); if (!((m - f > 0 && m + f > 0 && m - f > ab && m + f > ab) || (m - f < 0 && m + f < 0 && m - f < ab && m + f < ab))) - return 0xffff; + { + uint8_t factor = path.obstacles[i].factor; + if (path.escape_factor + && (a == 1 || b == 1) + && (a != 0 && b != 0) + && (factor == 0u || factor > path.escape_factor)) + factor = path.escape_factor; + return factor == 0 ? 0xffffu : factor * (uint16_t) ab; + } } } } @@ -260,15 +270,25 @@ path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy) path.points[1].y = sy; } -/** Set up an obstacle at given position with the given radius and validity - * period. */ +/** Try to escape from inside an obstacle. Bigger factor will shorten path + * followed inside the obstacle. Valid until the next update. */ +void +path_escape (uint8_t factor) +{ + path.escape_factor = factor; +} + +/** Set up an obstacle at given position with the given radius, factor and + * validity period. */ void -path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid) +path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint8_t factor, + uint16_t valid) { assert (i < AC_PATH_OBSTACLES_NB); path.obstacles[i].x = x; path.obstacles[i].y = y; path.obstacles[i].r = r; + path.obstacles[i].factor = factor; path.obstacles[i].valid = valid; } @@ -291,6 +311,7 @@ path_update (void) { path_compute_points (); path_compute_arcs (); + path.escape_factor = 0; path_dijkstra (); #if AC_PATH_REPORT uint8_t len, i; -- cgit v1.2.3