From 20d29c23e7b6fc011636d5f979f4b7261f213359 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 17 May 2012 06:10:38 +0200 Subject: digital/io-hub/src/guybrush: release goldbar when blocking --- digital/io-hub/src/guybrush/bottom_clamp.c | 9 +++++++++ digital/io-hub/src/guybrush/top.c | 21 +++++++++++++++------ 2 files changed, 24 insertions(+), 6 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c index ca44c98b..dd821bae 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.c +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -593,6 +593,15 @@ FSM_TRANS(CLAMP_CLOSE_ALL_CLAMPS, robot_is_back,CLAMP_REARRANGE_CD) } +FSM_TRANS (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach, CLAMP_READY_TO_EMPTY_TREE) +{ + IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE); + IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE); + IO_SET (OUTPUT_UPPER_CLAMP_OPEN); + fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); + return FSM_NEXT (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach); +} + FSM_TRANS_TIMEOUT (CLAMP_REARRANGE_CD, TIMEOUT_RECENTRAGE, CLAMP_BOTTOM_CLAMP_HIDE_POS2) { /*Hidding the clamp inside the robot.*/ diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 85015adb..8f80e174 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -84,6 +84,8 @@ FSM_STATES ( TOP_TOTEM_GOING_BACK, /* Put clamps up. */ TOP_TOTEM_CLAMP_UPPING, + /* Release a goldbar blocking the robot. */ + TOP_TOTEM_ERROR_RELEASE, /* Going back after an error. */ TOP_TOTEM_ERROR_GOING_BACK, @@ -333,12 +335,6 @@ FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING) return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success); } -FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING) -{ - FSM_HANDLE (AI, robot_is_back); - return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure); -} - FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, TOP_DECISION) { return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamps_ready); @@ -368,6 +364,19 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure, return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure); } +FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_ERROR_RELEASE) +{ + strat_failure (); + FSM_HANDLE (AI, stop_tree_approach); + return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure); +} + +FSM_TRANS (TOP_TOTEM_ERROR_RELEASE, clamps_ready, TOP_TOTEM_ERROR_GOING_BACK) +{ + move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0); + return FSM_NEXT (TOP_TOTEM_ERROR_RELEASE, clamps_ready); +} + FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING) { FSM_HANDLE (AI, stop_tree_approach); -- cgit v1.2.3