From dba92215c528ceb41708d34f7dcf0aa8bd07ff21 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 05:15:14 +0200 Subject: digital/io/src: add shorten option to move FSM This will be used to collect corn. --- digital/io/src/ai_move_cb.c | 40 ++++++++++++++++++++++++++++++---------- digital/io/src/ai_top_cb.c | 10 +++++----- digital/io/src/move.c | 4 +++- digital/io/src/move.h | 4 +++- digital/io/src/top.c | 4 ++-- 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; } } -- cgit v1.2.3