summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNélio Laranjeiro2008-05-01 22:25:12 +0200
committerNélio Laranjeiro2008-05-01 22:25:12 +0200
commitbf90bbff132481a909d9787eab849da8d20340a9 (patch)
tree4d4be30ec12b16cf5453aab962f2dee73ef8ccb2
parentabf969b98e551a393811bd5908bdb3eb14ca0763 (diff)
move_fsm: Ignore obstacles out of the table.
-rw-r--r--digital/io/src/move.c11
-rw-r--r--digital/io/src/move_cb.c55
2 files changed, 59 insertions, 7 deletions
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index ac6b9170..005a8fbc 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -34,10 +34,15 @@
*/
struct move_data_t move_data;
-/* Go to a position with the start FSM. */
+/**
+ * Go to a position with the start FSM.
+ * @param position_x the X position.
+ * @param position_y the Y position.
+ * @param backward_movement_allowed do we allow backward movement?
+ */
void
-move_start (uint16_t position_x, uint16_t position_y,
- uint8_t backward_movement_allowed)
+move_start (uint16_t position_x, uint16_t position_y, uint8_t
+ backward_movement_allowed)
{
/* Set parameters. */
move_data.final.x = position_x;
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index de7ce204..10f10b06 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -82,7 +82,29 @@
#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (50)
/**
- * Easier function to get the next intermediate positon from the path module.
+ * A detection offset for the sharps.
+ */
+#define MOVE_DETECTION_OFFSET 250
+
+/**
+ * Verify after the computation of the obstacle, this shall only be called
+ * after the function move_compute_obstacle_position.
+ * \return true if the position computed is in the table, false otherwise.
+ */
+uint8_t
+move_obstacle_in_table (void)
+{
+ if ((move_data.obstacle.x <= PG_WIDTH - MOVE_DETECTION_OFFSET)
+ && (move_data.obstacle.x > MOVE_DETECTION_OFFSET)
+ && (move_data.obstacle.y <= PG_HEIGHT- MOVE_DETECTION_OFFSET)
+ && (move_data.obstacle.y > MOVE_DETECTION_OFFSET))
+ return 0x1;
+ else
+ return 0x0;
+}
+
+/**
+ * Easier function to get the next intermediate position from the path module.
* @param dst new destination position computed by the path module
* @return
* - 0 if no path could be found ;
@@ -100,8 +122,19 @@ move_get_next_position (move_position_t *dst)
/* Update the path module */
path_update ();
+ /* If the path is found. */
+ if (path_get_next (&dst->x, &dst->y) != 0)
+ {
+ /* Position end. */
+ if (dst->x == move_data.final.x && dst->y == move_data.final.y)
+ return 1;
+ else if (main_always_stop_for_obstacle)
+ return 0;
+ else
+ return 1;
+ }
/* Retrieve next path coordinate */
- if (main_always_stop_for_obstacle || !path_get_next (&dst->x, &dst->y))
+ else if (main_always_stop_for_obstacle || !path_get_next (&dst->x, &dst->y))
{
DPRINTF ("Could not compute any path to avoid obstacle!\n");
return 0;
@@ -135,6 +168,7 @@ move_compute_obstacle_position (asserv_position_t cur,
MOVE_OBSTACLE_DISTANCE);
DPRINTF ("Computed obstacle (%d ; %d)\n", obstacle->x, obstacle->y);
}
+
/**
* Unique function to compute the obstacle position from here.
*/
@@ -147,8 +181,21 @@ move_obstacle_here (void)
/* Compute the obstacle position */
move_compute_obstacle_position (current, &move_data.obstacle);
/* Give it to the path module */
- path_obstacle (0, move_data.obstacle.x, move_data.obstacle.y,
- MOVE_OBSTACLE_RADIUS, MOVE_OBSTACLE_VALIDITY);
+ /* only id the obstacle is in the table */
+ if (move_obstacle_in_table ())
+ {
+ path_obstacle (0, move_data.obstacle.x, move_data.obstacle.y,
+ MOVE_OBSTACLE_RADIUS, MOVE_OBSTACLE_VALIDITY);
+ DPRINTF ("Obstacle pos x : %d, pos y : %d\n", move_data.obstacle.x,
+ move_data.obstacle.y);
+ }
+ else
+ {
+ main_sharp_ignore_event = 0xFF;
+ DPRINTF ("Obstacle Ignored pos x : %d, pos y : %d\n",
+ move_data.obstacle.x,
+ move_data.obstacle.y);
+ }
}
/**