From b924ab5f66538c6c2f2f19f02bff755a3ddd266d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 12 May 2010 21:34:15 +0200 Subject: digital/io/src: homologation unloading --- digital/io/src/ai_top_cb.c | 59 ++++++++++++++++++++++++++++++++++++++++++---- digital/io/src/top.fsm | 26 ++++++++++++++++++-- 2 files changed, 79 insertions(+), 6 deletions(-) diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 2e9a824d..745e0b6a 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -75,7 +75,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, -90); + position_t pos = PG_POSITION_DEG (2625, 253, -29); move_start (pos, 0); loader_down (); return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed); @@ -110,8 +110,9 @@ ai__FIRST_GO_END_OF_LINE_FAST__in_field (void) /* * FIRST_GO_END_OF_LINE_FAST =move_fsm_succeed=> - * => IDLE + * => UNLOAD_LOADER_UP * set slow speed + * move loader up */ fsm_branch_t ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_succeed (void) @@ -137,11 +138,13 @@ ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void) /* * FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=> - * => IDLE + * => UNLOAD_LOADER_UP + * move loader up */ fsm_branch_t ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void) { + loader_up (); return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed); } @@ -153,8 +156,56 @@ 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, -90); + position_t pos = PG_POSITION_DEG (2625, 253, -29); move_start (pos, 0); return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed); } +/* + * UNLOAD_LOADER_UP =loader_uped=> + * => UNLOAD_FACE_BIN + * turn toward bin + */ +fsm_branch_t +ai__UNLOAD_LOADER_UP__loader_uped (void) +{ + asserv_goto_angle (PG_A_DEG (90)); + return ai_next (UNLOAD_LOADER_UP, loader_uped); +} + +/* + * UNLOAD_FACE_BIN =bot_move_succeed=> + * => UNLOAD_BACK_BIN + * go backward to bin + */ +fsm_branch_t +ai__UNLOAD_FACE_BIN__bot_move_succeed (void) +{ + asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50)); + return ai_next (UNLOAD_FACE_BIN, bot_move_succeed); +} + +/* + * UNLOAD_BACK_BIN =bot_move_succeed=> + * => IDLE + * unload + */ +fsm_branch_t +ai__UNLOAD_BACK_BIN__bot_move_succeed (void) +{ + asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); + return ai_next (UNLOAD_BACK_BIN, bot_move_succeed); +} + +/* + * UNLOAD_BACK_BIN =bot_move_failed=> + * => IDLE + * unload + */ +fsm_branch_t +ai__UNLOAD_BACK_BIN__bot_move_failed (void) +{ + ai__UNLOAD_BACK_BIN__bot_move_succeed (); + return ai_next (UNLOAD_BACK_BIN, bot_move_failed); +} + diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index a46929f0..9a61d00c 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -13,6 +13,12 @@ States: first sequence, go to end of line FIRST_GO_END_OF_LINE_SLOW slow down to take elements + UNLOAD_LOADER_UP + move loader to up position + UNLOAD_FACE_BIN + orient robot to unload angle + UNLOAD_BACK_BIN + move back to unload bin Events: start @@ -59,14 +65,30 @@ FIRST_GO_BEGIN_OF_LINE_FAST: FIRST_GO_END_OF_LINE_FAST: in_field -> FIRST_GO_END_OF_LINE_SLOW set slow speed - move_fsm_succeed -> IDLE + move_fsm_succeed -> UNLOAD_LOADER_UP set slow speed + move loader up move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW set slow speed retry FIRST_GO_END_OF_LINE_SLOW: - move_fsm_succeed -> IDLE + move_fsm_succeed -> UNLOAD_LOADER_UP + move loader up move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW retry +UNLOAD_LOADER_UP: + loader_uped -> UNLOAD_FACE_BIN + turn toward bin + +UNLOAD_FACE_BIN: + bot_move_succeed -> UNLOAD_BACK_BIN + go backward to bin + +UNLOAD_BACK_BIN: + bot_move_succeed -> IDLE + unload + bot_move_failed -> IDLE + unload + -- cgit v1.2.3