From cfc72a8fea2d8ce7f442f7f7612e1b50eb5664fc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 13:06:29 +0200 Subject: digital/io/src: add blocking handling --- digital/io/src/ai_top_cb.c | 48 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) (limited to 'digital/io/src/ai_top_cb.c') diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 2041ab7b..65895afe 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -189,6 +189,18 @@ ai__UNLOAD_LOADER_UP__loader_uped (void) return ai_next (UNLOAD_LOADER_UP, loader_uped); } +/* + * UNLOAD_LOADER_UP =loader_errored=> + * => UNLOAD_FACE_BIN + * turn toward bin + */ +fsm_branch_t +ai__UNLOAD_LOADER_UP__loader_errored (void) +{ + asserv_goto_angle (PG_A_DEG (90)); + return ai_next (UNLOAD_LOADER_UP, loader_errored); +} + /* * UNLOAD_FACE_BIN =bot_move_succeed=> * => UNLOAD_BACK_BIN @@ -201,6 +213,42 @@ ai__UNLOAD_FACE_BIN__bot_move_succeed (void) return ai_next (UNLOAD_FACE_BIN, bot_move_succeed); } +/* + * UNLOAD_FACE_BIN =bot_move_failed=> + * => UNLOAD_FACE_BIN_UNBLOCK + * move backward + */ +fsm_branch_t +ai__UNLOAD_FACE_BIN__bot_move_failed (void) +{ + asserv_move_linearly (-40); + return ai_next (UNLOAD_FACE_BIN, bot_move_failed); +} + +/* + * UNLOAD_FACE_BIN_UNBLOCK =bot_move_succeed=> + * => UNLOAD_FACE_BIN + * turn toward bin + */ +fsm_branch_t +ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_succeed (void) +{ + asserv_goto_angle (PG_A_DEG (90)); + return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed); +} + +/* + * UNLOAD_FACE_BIN_UNBLOCK =bot_move_failed=> + * => UNLOAD_FACE_BIN + * turn toward bin + */ +fsm_branch_t +ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_failed (void) +{ + asserv_goto_angle (PG_A_DEG (90)); + return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed); +} + /* * UNLOAD_BACK_BIN =bot_move_succeed=> * => UNLOAD_UNLOAD -- cgit v1.2.3