From 953ff53ccf92e700c7c25136eabbe6699caa90f9 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 00:46:21 +0200 Subject: digital/io/src: make angle optional in move FSM --- digital/io/src/ai_move_cb.c | 8 ++++++-- digital/io/src/ai_top_cb.c | 12 ++++-------- digital/io/src/move.c | 15 ++++++++++++++- digital/io/src/move.h | 6 ++++++ digital/io/src/playground.h | 4 ++++ 5 files changed, 34 insertions(+), 11 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index 7a8ca9cd..eaa0aead 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -88,8 +88,12 @@ move_get_next_position (void) { /* Final position. */ move_data.final_move = 1; - /* Goto with angle. */ - asserv_goto_xya (dst.x, dst.y, move_data.final.a, + /* Goto with angle if requested. */ + if (move_data.with_angle) + asserv_goto_xya (dst.x, dst.y, move_data.final.a, + move_data.backward_movement_allowed); + else + asserv_goto (dst.x, dst.y, move_data.backward_movement_allowed); } move_data.step = dst; diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 745e0b6a..c057bebf 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -61,8 +61,7 @@ fsm_branch_t ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) { asserv_set_speed (BOT_MOVE_FAST); - position_t pos = PG_POSITION_DEG (375, 1503, -29); - move_start (pos, 0); + move_start_noangle (PG_VECT (375, 1503), 0); return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started); } @@ -75,8 +74,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) { - position_t pos = PG_POSITION_DEG (2625, 253, -29); - move_start (pos, 0); + move_start_noangle (PG_VECT (2625, 253), 0); loader_down (); return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed); } @@ -91,8 +89,7 @@ fsm_branch_t ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void) { asserv_set_speed (BOT_MOVE_SLOW); - position_t pos = PG_POSITION_DEG (375, 1503, -29); - move_start (pos, 0); + move_start_noangle (PG_VECT (375, 1503), 0); return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed); } @@ -156,8 +153,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) { - position_t pos = PG_POSITION_DEG (2625, 253, -29); - move_start (pos, 0); + move_start_noangle (PG_VECT (2625, 253), 0); return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed); } diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 24b8848d..77bbcf39 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -42,7 +42,20 @@ void move_start (position_t position, uint8_t backward) { /* Set parameters. */ - move_data.final= position; + move_data.final = position; + move_data.with_angle = 1; + move_data.backward_movement_allowed = backward; + move_data.final_move = 0; + /* Start the FSM. */ + fsm_handle_event (&ai_fsm, AI_EVENT_move_start); +} + +void +move_start_noangle (vect_t position, uint8_t backward) +{ + /* Set parameters. */ + move_data.final.v = position; + move_data.with_angle = 0; move_data.backward_movement_allowed = backward; move_data.final_move = 0; /* Start the FSM. */ diff --git a/digital/io/src/move.h b/digital/io/src/move.h index c504a0b2..214fddba 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -47,6 +47,8 @@ struct move_data_t { /** Final position. */ position_t final; + /** Use angle consign for final point. */ + uint8_t with_angle; /** Next step. */ vect_t step; /** Backward direction allowed flag. */ @@ -71,6 +73,10 @@ extern struct move_data_t move_data; void 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); + /** To be called when obstacles positions are computed. */ void move_obstacles_update (void); diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index 68e17ba0..5c251ce5 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -76,6 +76,10 @@ #define PG_POSITION_DEG(x, y, a) \ { { PG_X (x), PG_Y (y) }, PG_A_DEG (a) } +/** Initialiser for vect_t applying symmetry according to color. */ +#define PG_VECT(x, y) \ + (vect_t) { PG_X (x), PG_Y (y) } + /** * Start zone. */ -- cgit v1.2.3