summaryrefslogtreecommitdiff
path: root/digital/io/src/ai_move_cb.c
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-14 05:15:14 +0200
committerNicolas Schodet2010-05-14 05:15:14 +0200
commitdba92215c528ceb41708d34f7dcf0aa8bd07ff21 (patch)
treed55a38cc0faf6e9b0af39532718bad94978092da /digital/io/src/ai_move_cb.c
parent734b5b06dd0421c3c0ab9fbc282510801869ab4c (diff)
digital/io/src: add shorten option to move FSM
This will be used to collect corn.
Diffstat (limited to 'digital/io/src/ai_move_cb.c')
-rw-r--r--digital/io/src/ai_move_cb.c40
1 files changed, 30 insertions, 10 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index 5544eaf6..a61a1058 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -49,6 +49,34 @@
#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
+/** Go to current step, low level function. */
+static void
+move_go (void)
+{
+ vect_t dst = move_data.step;
+ /* Modify final point if requested. */
+ if (move_data.final_move && move_data.shorten)
+ {
+ /* Compute a vector from destination to robot with lenght
+ * 'shorten'. */
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ vect_t v = robot_position.v;
+ vect_sub (&v, &move_data.step);
+ int16_t d = vect_norm (&v);
+ if (d > move_data.shorten)
+ {
+ vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
+ vect_translate (&dst, &v);
+ }
+ }
+ if (move_data.step_with_angle)
+ asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
+ move_data.step_backward);
+ else
+ asserv_goto (dst.x, dst.y, move_data.step_backward);
+}
+
/** Go or rotate toward position, returns 1 for linear move, 2 for angular
* move. */
static uint8_t
@@ -75,10 +103,7 @@ move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
if (UTILS_ABS (diff) < 0x1000)
{
loader_down ();
- if (with_angle)
- asserv_goto_xya (dst.x, dst.y, angle, backward);
- else
- asserv_goto (dst.x, dst.y, backward);
+ move_go ();
return 1;
}
else
@@ -204,12 +229,7 @@ fsm_branch_t
ai__MOVE_ROTATING__bot_move_succeed (void)
{
loader_down ();
- if (move_data.step_with_angle)
- asserv_goto_xya (move_data.step.x, move_data.step.y,
- move_data.step_angle, move_data.step_backward);
- else
- asserv_goto (move_data.step.x, move_data.step.y,
- move_data.step_backward);
+ move_go ();
return ai_next (MOVE_ROTATING, bot_move_succeed);
}