summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 04:29:28 +0200
committerNicolas Schodet2011-06-02 04:29:28 +0200
commit0d1aaa87a85b7306c66d217c345596c84fd9f80f (patch)
treec2569636e1361a40b20be09a6f01cc3719187778
parentf8c5a9371f9d77a99a8a95033fd8d747d21a0356 (diff)
digital/io-hub: handle blocking situations
-rw-r--r--digital/io-hub/src/robospierre/top.c82
1 files changed, 81 insertions, 1 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 43eaf255..4554c12b 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -45,8 +45,12 @@ FSM_STATES (
TOP_START,
/* Going out of start area. */
TOP_GOING_OUT1,
+ /* Problem going out, wait before retry. */
+ TOP_GOING_OUT1_BLOCK_WAIT,
/* Going out, first pawn emplacement. */
TOP_GOING_OUT2,
+ /* Problem going out, wait before retry. */
+ TOP_GOING_OUT2_BLOCK_WAIT,
TOP_GOING_TO_DROP,
TOP_GOING_TO_ELEMENT,
@@ -66,6 +70,8 @@ struct top_t
{
/** Target element. */
uint8_t target_element_id;
+ /** Chaos counter. */
+ uint8_t chaos;
};
/** Global context. */
@@ -75,7 +81,8 @@ struct top_t top_global;
FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1)
{
element_init ();
- asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), PG_Y (PG_LENGTH - 200), 0);
+ asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100),
+ PG_Y (PG_LENGTH - 200), 0);
return FSM_NEXT (TOP_START, init_start_round);
}
@@ -85,6 +92,18 @@ FSM_TRANS (TOP_GOING_OUT1, robot_move_success, TOP_GOING_OUT2)
return FSM_NEXT (TOP_GOING_OUT1, robot_move_success);
}
+FSM_TRANS (TOP_GOING_OUT1, robot_move_failure, TOP_GOING_OUT1_BLOCK_WAIT)
+{
+ return FSM_NEXT (TOP_GOING_OUT1, robot_move_failure);
+}
+
+FSM_TRANS_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT, 250, TOP_GOING_OUT1)
+{
+ asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100),
+ PG_Y (PG_LENGTH - 200 - (++ctx.chaos % 4) * 10), 0);
+ return FSM_NEXT_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT);
+}
+
static uint8_t
top_go_element (void)
{
@@ -155,6 +174,25 @@ FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
}
}
+FSM_TRANS (TOP_GOING_OUT2, robot_move_failure, TOP_GOING_OUT2_BLOCK_WAIT)
+{
+ return FSM_NEXT (TOP_GOING_OUT2, robot_move_failure);
+}
+
+FSM_TRANS_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, 250,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ if (clamp_working ())
+ return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, clamp_working);
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, drop);
+ case 1: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, element);
+ }
+}
+
FSM_TRANS (TOP_GOING_TO_DROP, move_success,
ready, TOP_DROP_DROPPING,
wait_clamp, TOP_WAITING_READY)
@@ -171,6 +209,21 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success,
}
}
+FSM_TRANS (TOP_GOING_TO_DROP, move_failure,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ element_failure (ctx.target_element_id);
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, clamp_working);
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, drop);
+ case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, element);
+ }
+}
+
FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
clamp_working, TOP_WAITING_CLAMP,
drop, TOP_GOING_TO_DROP,
@@ -186,6 +239,21 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
}
}
+FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ element_failure (ctx.target_element_id);
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working);
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop);
+ case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, element);
+ }
+}
+
FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
drop, TOP_GOING_TO_DROP,
element, TOP_GOING_TO_ELEMENT)
@@ -223,3 +291,15 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success,
}
}
+FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ clamp_drop_clear ();
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop);
+ case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, element);
+ }
+}
+