summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c17
-rw-r--r--digital/io-hub/src/robospierre/logistic.c3
2 files changed, 18 insertions, 2 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index a9a1680b..979cc3c4 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -59,6 +59,10 @@ FSM_STATES (
CLAMP_INIT_FINDING_ROTATION_EDGE,
/* Initialisation sequence: finding top switch. */
CLAMP_INIT_FINDING_TOP,
+ /* Initialisation sequence: going to rest position. */
+ CLAMP_INIT_GOING_REST,
+ /* Clamp ready, waiting in rest position. */
+ CLAMP_INIT_READY,
/* Returning to idle position. */
CLAMP_GOING_IDLE,
@@ -434,12 +438,23 @@ FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success,
}
FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
- CLAMP_GOING_IDLE)
+ CLAMP_INIT_GOING_REST)
{
clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE);
return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success);
}
+FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY)
+{
+ return FSM_NEXT (CLAMP_INIT_GOING_REST, clamp_move_success);
+}
+
+FSM_TRANS (CLAMP_INIT_READY, init_start_round, CLAMP_GOING_IDLE)
+{
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_INIT_READY, init_start_round);
+}
+
FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE)
{
ctx.working = 0;
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
index a1b2a705..4fff865d 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -419,7 +419,8 @@ logistic_init (void)
ctx.slots[i] = 0;
ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB;
ctx.collect_direction = DIRECTION_FORWARD;
- ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE;
+ ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE;
ctx.construct_possible = 0;
ctx.prepare = 1;
ctx.ready = 0;