From f335b6c0c4fa4315229e9cc7ee01cb200207acdf Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 07:42:13 +0200 Subject: digital/io-hub: detect loss of side slot element --- 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 46f8736f..eacecbe1 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -28,6 +28,9 @@ #include "clamp.h" #include "defs.h" +#include "contact.h" +#include "io.h" + #include "debug.host.h" /** Handle elements stored inside the robot. */ @@ -441,6 +444,21 @@ logistic_init (void) void logistic_update (void) { + uint8_t side_now = !IO_GET (CONTACT_SIDE); + /* Filter side contact. */ + if (side_now) + { + ctx.side_filter = 0; + ctx.side_state = 1; + } + else if (ctx.side_filter++ == 2 * 250) + { + ctx.side_state = 0; + ctx.side_filter = 0; + } + /* Side slot element can be lost. */ + if (ctx.moving_from != CLAMP_SLOT_SIDE && !ctx.side_state) + ctx.slots[CLAMP_SLOT_SIDE] = 0; } void -- cgit v1.2.3