From 4232368383074bfa9fcb98d96b6a20f0a4ef2736 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 20:42:57 +0200 Subject: digital/io-hub: handle clamp locking When the clamp is in a bay with an element at the middle level, it can no longer move. --- digital/io-hub/src/robospierre/logistic.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) (limited to 'digital/io-hub/src/robospierre/logistic.c') diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 8c2a759f..0dbbaa6c 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -221,3 +221,21 @@ logistic_drop (uint8_t direction) logistic_decision (); } +static uint8_t +logistic_slot_clear (uint8_t slot) +{ + if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot) + && ctx.slots[CLAMP_SLOT_FRONT_MIDDLE]) + return 0; + if (CLAMP_IS_SLOT_IN_BACK_BAY (slot) + && ctx.slots[CLAMP_SLOT_BACK_MIDDLE]) + return 0; + return 1; +} + +uint8_t +logistic_path_clear (uint8_t slot1, uint8_t slot2) +{ + return logistic_slot_clear (slot1) && logistic_slot_clear (slot2); +} + -- cgit v1.2.3