summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-23 00:33:53 +0200
committerNicolas Schodet2010-04-23 00:33:53 +0200
commit98a10d5c181d40f8ea3d600d712ed249417a963e (patch)
treea066093939980912ffefb94fba1f8712376fbcf1 /digital
parentf664a063297ce25d51e69332f864ed3e288fb116 (diff)
digital/avr/modules/path: use vect_t in interface
Diffstat (limited to 'digital')
-rw-r--r--digital/avr/modules/path/path.c51
-rw-r--r--digital/avr/modules/path/path.h11
-rw-r--r--digital/avr/modules/path/test/test_path.c17
-rw-r--r--digital/io/src/ai_move_cb.c9
-rw-r--r--digital/io/src/move.c4
-rw-r--r--digital/io/src/simu.host.c4
6 files changed, 48 insertions, 48 deletions
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 (&current_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);
}