summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/top.fsm
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/top.fsm')
-rw-r--r--digital/io/src/top.fsm26
1 files changed, 24 insertions, 2 deletions
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
+