summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-01 12:51:46 +0200
committerNicolas Schodet2011-06-01 12:51:46 +0200
commitb2c12931a8378cdb2112da8b21b63099683516f5 (patch)
tree9d9805dc56300df74c59e2c273f27506128d5778 /digital/io-hub/src/robospierre/top.c
parent6d3e532a65e14fddb242ecc4841becec44253a6b (diff)
digital/io-hub: do not prepare in green zone
Diffstat (limited to 'digital/io-hub/src/robospierre/top.c')
-rw-r--r--digital/io-hub/src/robospierre/top.c54
1 files changed, 47 insertions, 7 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index b99e3baf..0af8ce80 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -51,7 +51,13 @@ FSM_STATES (
TOP_GOING_TO_DROP,
TOP_GOING_TO_ELEMENT,
/* Waiting clamp has finished its work. */
- TOP_WAITING_CLAMP)
+ TOP_WAITING_CLAMP,
+ /* Waiting construction is ready to drop. */
+ TOP_WAITING_READY,
+ /* Dropping, opening the doors. */
+ TOP_DROP_DROPPING,
+ /* Dropping, clearing so that doors can be closed. */
+ TOP_DROP_CLEARING)
FSM_START_WITH (TOP_START)
@@ -85,6 +91,11 @@ top_go_element (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
ctx.target_element_id = element_best (robot_pos);
+ element_t e = element_get (ctx.target_element_id);
+ if (e.attr & ELEMENT_GREEN)
+ logistic_global.prepare = 0;
+ else
+ logistic_global.prepare = 1;
vect_t element_pos = element_get_pos (ctx.target_element_id);
uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
? 0 : ASSERV_BACKWARD;
@@ -130,14 +141,18 @@ FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
}
FSM_TRANS (TOP_GOING_TO_DROP, move_success,
- drop, TOP_GOING_TO_DROP,
- element, TOP_GOING_TO_ELEMENT)
+ ready, TOP_DROP_DROPPING,
+ wait_clamp, TOP_WAITING_READY)
{
- clamp_drop (logistic_global.collect_direction);
- switch (top_decision ())
+ if (logistic_global.ready)
+ {
+ clamp_drop (logistic_global.collect_direction);
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready);
+ }
+ else
{
- default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop);
- case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element);
+ clamp_prepare (1);
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
}
}
@@ -166,3 +181,28 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element);
}
}
+
+FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING)
+{
+ clamp_drop (logistic_global.collect_direction);
+ return FSM_NEXT (TOP_WAITING_READY, clamp_done);
+}
+
+FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING)
+{
+ asserv_move_linearly (200);
+ return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting);
+}
+
+FSM_TRANS (TOP_DROP_CLEARING, robot_move_success,
+ 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_success, drop);
+ case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, element);
+ }
+}
+