summaryrefslogtreecommitdiffhomepage
path: root/digital
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
parent734b5b06dd0421c3c0ab9fbc282510801869ab4c (diff)
digital/io/src: add shorten option to move FSM
This will be used to collect corn.
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_move_cb.c40
-rw-r--r--digital/io/src/ai_top_cb.c10
-rw-r--r--digital/io/src/move.c4
-rw-r--r--digital/io/src/move.h4
-rw-r--r--digital/io/src/top.c4
5 files changed, 43 insertions, 19 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);
}
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index e42ac4b1..f8596c1f 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -56,7 +56,7 @@ fsm_branch_t
ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
{
asserv_set_speed (BOT_MOVE_FAST);
- move_start_noangle (PG_VECT (375, 1503), 0);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started);
}
@@ -69,7 +69,7 @@ ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
fsm_branch_t
ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_succeed (void)
{
- move_start_noangle (PG_VECT (2625, 253), 0);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
loader_down ();
return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
}
@@ -84,7 +84,7 @@ fsm_branch_t
ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void)
{
asserv_set_speed (BOT_MOVE_SLOW);
- move_start_noangle (PG_VECT (375, 1503), 0);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
}
@@ -148,7 +148,7 @@ ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void)
fsm_branch_t
ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void)
{
- move_start_noangle (PG_VECT (2625, 253), 0);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
}
@@ -172,7 +172,7 @@ ai__UNLOAD__move_fsm_succeed (void)
fsm_branch_t
ai__UNLOAD__move_fsm_failed (void)
{
- move_start_noangle (PG_VECT (2625, 253), 0);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
return ai_next (UNLOAD, move_fsm_failed);
}
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 77bbcf39..27fc93fb 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -46,18 +46,20 @@ move_start (position_t position, uint8_t backward)
move_data.with_angle = 1;
move_data.backward_movement_allowed = backward;
move_data.final_move = 0;
+ move_data.shorten = 0;
/* Start the FSM. */
fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
}
void
-move_start_noangle (vect_t position, uint8_t backward)
+move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
{
/* Set parameters. */
move_data.final.v = position;
move_data.with_angle = 0;
move_data.backward_movement_allowed = backward;
move_data.final_move = 0;
+ move_data.shorten = shorten;
/* Start the FSM. */
fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
}
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 7e52c6b9..7f860802 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -66,6 +66,8 @@ struct move_data_t
uint8_t try_again_counter;
/** Dirty fix to know this is the final move. */
uint8_t final_move;
+ /** Distance to remove from path. */
+ int16_t shorten;
};
/**
@@ -84,7 +86,7 @@ move_start (position_t position, uint8_t backward);
/** Go to a position, with no angle consign. */
void
-move_start_noangle (vect_t position, uint8_t backward);
+move_start_noangle (vect_t position, uint8_t backward, int16_t shorten);
/** To be called when obstacles positions are computed. */
void
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index 6f7fd8db..f68a71b7 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -57,12 +57,12 @@ top_collect (uint8_t force)
return 0;
vect_t food_v;
food_pos (food, &food_v);
- move_start_noangle (food_v, 0);
+ move_start_noangle (food_v, 0, 0);
return 1;
}
else
{
- move_start_noangle (PG_VECT (2625, 253), 0);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
return 0;
}
}