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_loader_cb.c | 49 +++++++++++++++++++++++++++++++++++++++++++ digital/io/src/ai_top_cb.c | 48 ++++++++++++++++++++++++++++++++++++++++++ digital/io/src/loader.fsm | 8 +++++++ digital/io/src/top.fsm | 12 +++++++++++ 4 files changed, 117 insertions(+) (limited to 'digital') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index b556c9eb..36bd96fe 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -363,6 +363,18 @@ ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void) return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed); } +/* + * LOADER_ERROR_DOWNING_OPEN =clamp_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_ERROR_DOWNING_OPEN__clamp_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_failed); +} + /* * LOADER_ERROR_UPING =elevator_succeed=> * => LOADER_ERROR_UPING_OPEN @@ -400,6 +412,18 @@ ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void) return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed); } +/* + * LOADER_ERROR_UPING_OPEN =clamp_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_ERROR_UPING_OPEN__clamp_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_ERROR_UPING_OPEN, clamp_failed); +} + /* * LOADER_LOAD_CLOSING =clamp_succeed=> * full => LOADER_LOAD_UPING @@ -513,6 +537,18 @@ ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void) } } +/* + * LOADER_LOAD_UNLOADING_OPEN =clamp_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_LOAD_UNLOADING_OPEN__clamp_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_failed); +} + /* * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=> * down => LOADER_DOWN @@ -532,3 +568,16 @@ ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void) return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down); } +/* + * LOADER_LOAD_EMPTY_OPEN =clamp_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_LOAD_EMPTY_OPEN__clamp_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_failed); +} + + 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 diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index fb37dc81..7cc192b3 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -149,6 +149,8 @@ LOADER_ERROR_DOWNING: LOADER_ERROR_DOWNING_OPEN: clamp_succeed -> LOADER_DOWN post loader_downed event + clamp_failed -> LOADER_ERROR + post loader_errored event LOADER_ERROR_UPING: elevator_succeed -> LOADER_ERROR_UPING_OPEN @@ -159,6 +161,8 @@ LOADER_ERROR_UPING: LOADER_ERROR_UPING_OPEN: clamp_succeed -> LOADER_UP post loader_uped event + clamp_failed -> LOADER_ERROR + post loader_errored event LOADER_DOWNING: elevator_succeed -> LOADER_DOWN @@ -205,9 +209,13 @@ LOADER_LOAD_UNLOADING_OPEN: move down clamp_succeed: up -> LOADER_UPING move up + clamp_failed -> LOADER_ERROR + post loader_errored event LOADER_LOAD_EMPTY_OPEN: clamp_succeed: down -> LOADER_DOWN clamp_succeed: up -> LOADER_UPING move up + clamp_failed -> LOADER_ERROR + post loader_errored event diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index c33923f6..4bebcdaa 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -19,6 +19,8 @@ States: move loader to up position UNLOAD_FACE_BIN orient robot to unload angle + UNLOAD_FACE_BIN_UNBLOCK + move backward to unblock UNLOAD_BACK_BIN move back to unload bin UNLOAD_UNLOAD[timeout=225] @@ -97,10 +99,20 @@ UNLOAD: UNLOAD_LOADER_UP: loader_uped -> UNLOAD_FACE_BIN turn toward bin + loader_errored -> UNLOAD_FACE_BIN + turn toward bin UNLOAD_FACE_BIN: bot_move_succeed -> UNLOAD_BACK_BIN go backward to bin + bot_move_failed -> UNLOAD_FACE_BIN_UNBLOCK + move backward + +UNLOAD_FACE_BIN_UNBLOCK: + bot_move_succeed -> UNLOAD_FACE_BIN + turn toward bin + bot_move_failed -> UNLOAD_FACE_BIN + turn toward bin UNLOAD_BACK_BIN: bot_move_succeed -> UNLOAD_UNLOAD -- cgit v1.2.3