summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-14 00:33:48 +0200
committerNicolas Schodet2012-05-14 00:34:58 +0200
commitbef31893e6ad6f62c4c9bdd9b6c62451b1b8fd51 (patch)
tree836026de5e55159f9290bd9097be961b91de509d
parentcffa0218d0511a4e7a540cad9e09d52f52d27e7a (diff)
digital/io-hub/src/guybrush: top clean area
-rw-r--r--digital/io-hub/src/guybrush/top.c60
1 files changed, 57 insertions, 3 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 36c193bc..945b6998 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -57,6 +57,16 @@ FSM_STATES (
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
+ /* Cleaning: waiting clamp FSM. */
+ TOP_TOTEM_CLEAN_WAITING,
+ /* Cleaning: approaching totem. */
+ TOP_TOTEM_CLEAN_APPROACHING,
+ /* Cleaning; closing clamps. */
+ TOP_TOTEM_CLEAN_CATCH_WAITING,
+ /* Cleaning: going back. */
+ TOP_TOTEM_CLEAN_GOING_BACK,
+ /* Cleaning: loading. */
+ TOP_TOTEM_CLEAN_LOADING,
/* Waiting clamp FSM to put clamps down. */
TOP_TOTEM_CLAMP_WAITING,
/* Put clamps down. */
@@ -193,17 +203,61 @@ FSM_TRANS (TOP_INIT, init_start_round,
/** TOTEM */
FSM_TRANS (TOP_TOTEM_GOING, move_success,
+ wait, TOP_TOTEM_CLEAN_WAITING,
+ ready, TOP_TOTEM_CLEAN_APPROACHING)
+{
+ if (!FSM_CAN_HANDLE (AI, clean_start))
+ {
+ return FSM_NEXT (TOP_TOTEM_GOING, move_success, wait);
+ }
+ else
+ {
+ FSM_HANDLE (AI, clean_start);
+ asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 130);
+ return FSM_NEXT (TOP_TOTEM_GOING, move_success, ready);
+ }
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_WAITING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
+{
+ FSM_HANDLE (AI, clean_start);
+ asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 130);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_WAITING, clamps_ready);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success,
+ TOP_TOTEM_CLEAN_CATCH_WAITING)
+{
+ FSM_HANDLE (AI, clean_catch);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready,
+ TOP_TOTEM_CLEAN_GOING_BACK)
+{
+ asserv_move_linearly (-100);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success,
+ TOP_TOTEM_CLEAN_LOADING)
+{
+ FSM_HANDLE (AI, clean_load);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready,
wait, TOP_TOTEM_CLAMP_WAITING,
ready, TOP_TOTEM_CLAMP_DOWNING)
{
if (!FSM_CAN_HANDLE (AI, tree_detected))
{
- return FSM_NEXT (TOP_TOTEM_GOING, move_success, wait);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready, wait);
}
else
{
FSM_HANDLE (AI, tree_detected);
- return FSM_NEXT (TOP_TOTEM_GOING, move_success, ready);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready, ready);
}
}
@@ -215,7 +269,7 @@ FSM_TRANS (TOP_TOTEM_CLAMP_WAITING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING)
{
- asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 30);
+ asserv_move_linearly (200);
return FSM_NEXT (TOP_TOTEM_CLAMP_DOWNING, clamps_ready);
}