summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-17 12:27:14 +0200
committerNicolas Schodet2012-05-17 14:18:23 +0200
commit10c98bbec0e066894d0432ca60bb170ee9e09b1d (patch)
treeee2771cf7b1e894bc6568324190aa836815d07f5
parentc206ca03d4e3e98d111605037c306e1f01589241 (diff)
digital/io-hub/src/guybrush: add clamp blocking handling in top
-rw-r--r--digital/io-hub/src/guybrush/strat.c7
-rw-r--r--digital/io-hub/src/guybrush/strat.h4
-rw-r--r--digital/io-hub/src/guybrush/top.c92
3 files changed, 103 insertions, 0 deletions
diff --git a/digital/io-hub/src/guybrush/strat.c b/digital/io-hub/src/guybrush/strat.c
index 63f3c124..640d8c62 100644
--- a/digital/io-hub/src/guybrush/strat.c
+++ b/digital/io-hub/src/guybrush/strat.c
@@ -283,3 +283,10 @@ strat_giveup (void)
assert (0);
}
}
+
+void
+strat_clamp_dead (void)
+{
+ /* TODO */
+}
+
diff --git a/digital/io-hub/src/guybrush/strat.h b/digital/io-hub/src/guybrush/strat.h
index 7dd24e1d..6c4a0255 100644
--- a/digital/io-hub/src/guybrush/strat.h
+++ b/digital/io-hub/src/guybrush/strat.h
@@ -61,4 +61,8 @@ strat_failure (void);
void
strat_giveup (void);
+/** Clamp is dead stop using it. */
+void
+strat_clamp_dead (void);
+
#endif /* strat_h */
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index aecbb7f7..8351e2ba 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -48,6 +48,9 @@
* impression of intelligence... Well...
*/
+/** Number of time to try to unblock clamp. */
+#define TOP_CLAMP_BLOCKED_THRESHOLD 5
+
FSM_INIT
FSM_STATES (
@@ -59,6 +62,8 @@ FSM_STATES (
TOP_INIT,
/* Transition state to take a new decision. */
TOP_DECISION,
+ /* Unblock clamp and take a new decision. */
+ TOP_CLAMP_ERROR_DECISION,
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
@@ -88,6 +93,8 @@ FSM_STATES (
TOP_TOTEM_ERROR_RELEASE,
/* Going back after an error. */
TOP_TOTEM_ERROR_GOING_BACK,
+ /* Going back after a clamp error. */
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK,
/* Going to push a bottle. */
TOP_BOTTLE_GOING,
@@ -226,10 +233,15 @@ FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
}
FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
+ clamp_blocked, TOP_CLAMP_ERROR_DECISION,
totem, TOP_TOTEM_GOING,
bottle, TOP_BOTTLE_GOING,
unload, TOP_UNLOAD_GOING)
{
+ if (FSM_HANDLE (AI, clamp_unblock))
+ {
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, clamp_blocked);
+ }
switch (top_decision ())
{
default:
@@ -243,6 +255,28 @@ FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
}
}
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamps_ready, TOP_DECISION)
+{
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamps_ready);
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamp_blocked,
+ unblock, TOP_CLAMP_ERROR_DECISION,
+ dead, TOP_DECISION)
+{
+ if (clamp_read_blocked_cpt () < TOP_CLAMP_BLOCKED_THRESHOLD)
+ {
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, unblock);
+ }
+ else
+ {
+ strat_clamp_dead ();
+ FSM_HANDLE (AI, clamp_is_dead);
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, dead);
+ }
+}
+
/** TOTEM */
FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLEAN_STARTING)
@@ -351,6 +385,35 @@ FSM_TRANS (TOP_TOTEM_GOING, move_failure, TOP_DECISION)
return FSM_NEXT (TOP_TOTEM_GOING, move_failure);
}
+FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamp_blocked, TOP_CLAMP_ERROR_DECISION)
+{
+ strat_failure ();
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked,
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK)
+{
+ strat_failure ();
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamp_blocked, TOP_CLAMP_ERROR_DECISION)
+{
+ strat_failure ();
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked, TOP_CLAMP_ERROR_DECISION)
+{
+ strat_failure ();
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked);
+}
+
FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
TOP_TOTEM_ERROR_GOING_BACK)
{
@@ -367,6 +430,14 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure,
return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure);
}
+FSM_TRANS (TOP_TOTEM_EMPTYING, clamp_blocked,
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK)
+{
+ strat_giveup ();
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_EMPTYING, clamp_blocked);
+}
+
FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_ERROR_RELEASE)
{
strat_giveup ();
@@ -374,6 +445,13 @@ FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_ERROR_RELEASE)
return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
}
+FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamp_blocked, TOP_CLAMP_ERROR_DECISION)
+{
+ strat_giveup ();
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamp_blocked);
+}
+
FSM_TRANS (TOP_TOTEM_ERROR_RELEASE, clamps_ready, TOP_TOTEM_ERROR_GOING_BACK)
{
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
@@ -392,6 +470,20 @@ FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_failure);
}
+FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success,
+ TOP_CLAMP_ERROR_DECISION)
+{
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure,
+ TOP_CLAMP_ERROR_DECISION)
+{
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure);
+}
+
/** BOTTLE */
FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING)