From 98a10d5c181d40f8ea3d600d712ed249417a963e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 23 Apr 2010 00:33:53 +0200 Subject: digital/avr/modules/path: use vect_t in interface --- digital/avr/modules/path/path.c | 51 ++++++++++++++----------------- digital/avr/modules/path/path.h | 11 ++++--- digital/avr/modules/path/test/test_path.c | 17 +++++++---- digital/io/src/ai_move_cb.c | 9 +++--- digital/io/src/move.c | 4 +-- digital/io/src/simu.host.c | 4 +-- 6 files changed, 48 insertions(+), 48 deletions(-) (limited to 'digital') diff --git a/digital/avr/modules/path/path.c b/digital/avr/modules/path/path.c index ceb36082..425ff523 100644 --- a/digital/avr/modules/path/path.c +++ b/digital/avr/modules/path/path.c @@ -49,7 +49,7 @@ struct path_point_t { /** Coordinates. */ - int16_t x, y; + vect_t c; /** Weight. */ uint16_t w; /** Next point (preceding in the usual Dijkstra meaning). */ @@ -128,13 +128,13 @@ path_compute_points (void) + fixed_mul_f824 (x, path.rot_b); x = nx; } - path.points[p].x = path.obstacles[i].x + (uint16_t) x; - path.points[p].y = path.obstacles[i].y + (uint16_t) y; + path.points[p].c.x = path.obstacles[i].c.x + (uint16_t) x; + path.points[p].c.y = path.obstacles[i].c.y + (uint16_t) y; /* Check it is in playground. */ - if (path.points[p].x >= path.border_xmin - && path.points[p].y >= path.border_ymin - && path.points[p].x < path.border_xmax - && path.points[p].y < path.border_ymax) + if (path.points[p].c.x >= path.border_xmin + && path.points[p].c.y >= path.border_ymin + && path.points[p].c.x < path.border_xmax + && path.points[p].c.y < path.border_ymax) /* Accept point. */ p++; } @@ -153,8 +153,8 @@ path_compute_weight (uint8_t a, uint8_t b) uint8_t i; uint8_t max_factor; /* Compute distance. */ - dx = path.points[b].x - path.points[a].x; - dy = path.points[b].y - path.points[a].y; + dx = path.points[b].c.x - path.points[a].c.x; + dy = path.points[b].c.y - path.points[a].c.y; ab = fixed_sqrt_ui32 (dx * dx + dy * dy); if (ab == 0) return 0; @@ -169,8 +169,8 @@ path_compute_weight (uint8_t a, uint8_t b) * Use a scalar product between a segment perpendicular vector and * the vector from one segment end to obstacle center. */ int32_t acx, acy; - acx = path.obstacles[i].x - path.points[a].x; - acy = path.obstacles[i].y - path.points[a].y; + acx = path.obstacles[i].c.x - path.points[a].c.x; + acy = path.obstacles[i].c.y - path.points[a].c.y; d = (acx * dy - acy * dx) / ab; d = UTILS_ABS (d); /* If out of the circle, no problem, else, more tests. */ @@ -264,12 +264,10 @@ path_dijkstra (void) /** Setup end points (source and destination coordinates). */ void -path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy) +path_endpoints (vect_t s, vect_t d) { - path.points[0].x = dx; - path.points[0].y = dy; - path.points[1].x = sx; - path.points[1].y = sy; + path.points[0].c = d; + path.points[1].c = s; } /** Try to escape from inside an obstacle. Bigger factor will shorten path @@ -283,12 +281,11 @@ path_escape (uint8_t 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, uint8_t factor, +path_obstacle (uint8_t i, vect_t c, 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].c = c; path.obstacles[i].r = r; path.obstacles[i].factor = factor; path.obstacles[i].valid = valid; @@ -317,12 +314,11 @@ path_update (void) path_dijkstra (); #if AC_PATH_REPORT uint8_t len, i; - uint16_t points[PATH_POINTS_NB * 2]; + vect_t points[PATH_POINTS_NB]; len = 0; for (i = 1; i != 0xff; i = path.points[i].next) { - points[len++] = path.points[i].x; - points[len++] = path.points[i].y; + points[len++] = path.points[i].c; } AC_PATH_REPORT_CALLBACK (points, len, path.obstacles, PATH_OBSTACLES_NB); #endif /* AC_PATH_REPORT */ @@ -330,7 +326,7 @@ path_update (void) /** Retrieve first path point coordinates. Return 0 on failure. */ uint8_t -path_get_next (uint16_t *x, uint16_t *y) +path_get_next (vect_t *p) { uint8_t next; next = path.points[1].next; @@ -338,8 +334,7 @@ path_get_next (uint16_t *x, uint16_t *y) return 0; else { - *x = path.points[next].x; - *y = path.points[next].y; + *p = path.points[next].c; return 1; } } @@ -357,8 +352,8 @@ path_print_graph (void) for (i = 0; i < path.points_nb; i++) { printf (" %d [ shape = Mrecord, label = \"{%d|{%d|%d}}\", " - "pos = \"%d,%d\" ]\n", i, i, path.points[i].x, - path.points[i].y, path.points[i].x, path.points[i].y); + "pos = \"%d,%d\" ]\n", i, i, path.points[i].c.x, + path.points[i].c.y, path.points[i].c.x, path.points[i].c.y); for (j = 0; j < i; j++) { if (path.arcs[i][j] != 0xffff) @@ -370,7 +365,7 @@ path_print_graph (void) } printf ("}\n// Path:\n"); for (i = 1; i != 0xff; i = path.points[i].next) - printf ("// %d, %d\n", path.points[i].x, path.points[i].y); + printf ("// %d, %d\n", path.points[i].c.x, path.points[i].c.y); } #endif diff --git a/digital/avr/modules/path/path.h b/digital/avr/modules/path/path.h index 24fc3e42..24640b5c 100644 --- a/digital/avr/modules/path/path.h +++ b/digital/avr/modules/path/path.h @@ -24,6 +24,7 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "modules/math/geometry/vect.h" /** Infinite validity for an obstacle. */ #define PATH_OBSTACLE_VALID_ALWAYS 0xffff @@ -32,7 +33,7 @@ struct path_obstacle_t { /** Center. */ - int16_t x, y; + vect_t c; /** Radius. */ uint16_t r; /** Factor. If not 0, obstacle is not blocking, but it add weight to path @@ -49,7 +50,7 @@ path_init (int16_t border_xmin, int16_t border_ymin, /** Setup end points (source and destination coordinates). */ void -path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy); +path_endpoints (vect_t s, vect_t d); /** Try to escape from inside an obstacle. Bigger factor will shorten path * followed inside the obstacle. Valid until the next update. */ @@ -59,7 +60,7 @@ path_escape (uint8_t 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, uint8_t factor, +path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, uint16_t valid); /** Slowly make the obstacles disappear. */ @@ -72,13 +73,13 @@ path_update (void); /** Retrieve first path point coordinates. Return 0 on failure. */ uint8_t -path_get_next (uint16_t *x, uint16_t *y); +path_get_next (vect_t *p); #if AC_PATH_REPORT /** Report computed path. */ void -AC_PATH_REPORT_CALLBACK (uint16_t *points, uint8_t len, +AC_PATH_REPORT_CALLBACK (vect_t *points, uint8_t len, struct path_obstacle_t *obstacles, uint8_t obstacles_nb); diff --git a/digital/avr/modules/path/test/test_path.c b/digital/avr/modules/path/test/test_path.c index 218a3183..6d84bed3 100644 --- a/digital/avr/modules/path/test/test_path.c +++ b/digital/avr/modules/path/test/test_path.c @@ -76,7 +76,10 @@ main (int argc, char **argv) path_init (tab[0], tab[1], tab[2], tab[3]); read_tab (argv[2], tab, 2); read_tab (argv[3], tab + 2, 2); - path_endpoints (tab[0], tab[1], tab[2], tab[3]); + vect_t s, d; + s.x = tab[0]; s.y = tab[1]; + d.x = tab[2]; d.y = tab[3]; + path_endpoints (s, d); read_tab (argv[4], tab, 1); if (tab[0]) path_escape (tab[0]); @@ -84,20 +87,22 @@ main (int argc, char **argv) for (i = 0; i + 5 < argc; i++) { read_tab (argv[5 + i], tab, 4); - path_obstacle (i, tab[0], tab[1], tab[2], tab[3], 1); + vect_t o; + o.x = tab[0]; o.y = tab[1]; + path_obstacle (i, o, tab[2], tab[3], 1); } path_update (); path_print_graph (); - uint16_t x, y; - if (path_get_next (&x, &y)) - printf ("// Next point: %d, %d\n", x, y); + vect_t p; + if (path_get_next (&p)) + printf ("// Next point: %d, %d\n", p.x, p.y); else printf ("// Failure\n"); return 0; } void -path_report (uint16_t *points, uint8_t len, +path_report (vect_t *points, uint8_t len, struct path_obstacle_t *obstacles, uint8_t obstacles_nb) { } diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index bca348f6..faabadb2 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -60,18 +60,17 @@ move_get_next_position (void) position_t current_pos; asserv_get_position (¤t_pos); /* Give the current position of the bot to the path module */ - path_endpoints (current_pos.v.x, current_pos.v.y, - move_data.final.v.x, move_data.final.v.y); + path_endpoints (current_pos.v, move_data.final.v); /* Update the path module */ path_update (); - found = path_get_next (&dst.x, &dst.y); + found = path_get_next (&dst); /* If not found, try to escape. */ if (!found) { slow = 1; path_escape (8); path_update (); - found = path_get_next (&dst.x, &dst.y); + found = path_get_next (&dst); } /* If the path is found, move. */ if (found) @@ -175,7 +174,7 @@ ai__MOVE_MOVING__bot_move_failed (void) : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS); vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); vect_translate (&obstacle_pos, &robot_pos.v); - path_obstacle (0, obstacle_pos.x, obstacle_pos.y, MOVE_OBSTACLE_RADIUS, 0, + path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0, MOVE_OBSTACLE_VALIDITY); /* Move backward to turn freely. */ asserv_move_linearly (asserv_get_last_moving_direction () == 1 ? diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 6eb9bfde..c0676c3d 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -54,8 +54,8 @@ move_obstacles_update (void) { uint8_t i; for (i = 0; i < main_obstacles_nb; i++) - path_obstacle (i, main_obstacles_pos[i].x, main_obstacles_pos[i].y, - MOVE_OBSTACLE_RADIUS, 0, MOVE_OBSTACLE_VALIDITY); + path_obstacle (i, main_obstacles_pos[i], MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); } void diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c index c4f607d0..bc4c404f 100644 --- a/digital/io/src/simu.host.c +++ b/digital/io/src/simu.host.c @@ -236,14 +236,14 @@ eeprom_clear_param (void) /** Send computed path. */ void -simu_send_path (uint16_t *points, uint8_t len, +simu_send_path (vect_t *points, uint8_t len, struct path_obstacle_t *obstacles, uint8_t obstacles_nb) { int i; mex_msg_t *m; m = mex_msg_new (MSG_SIMU_IO_PATH); for (i = 0; i < len; i++) - mex_msg_push (m, "h", points[i]); + mex_msg_push (m, "hh", points[i].x, points[i].y); mex_node_send (m); } -- cgit v1.2.3