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 ++++++++++++++++++ digital/io-hub/src/robospierre/logistic.h | 4 ++++ digital/io-hub/src/robospierre/test_element.c | 2 ++ 3 files changed, 24 insertions(+) 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 diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 334b9db4..f27c082b 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -88,6 +88,10 @@ struct logistic_t /* Inform TOP that we can't take any more elements and needs to put * construction somewhere. */ uint8_t need_prepare; + /** Filtered side slot sensor. */ + uint8_t side_state; + /** Filtered side slot sensor counter. */ + uint16_t side_filter; }; /** Global context. */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index b26db210..8c93e876 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -35,6 +35,8 @@ int test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK; position_t test_robot_pos_ = {{0, 2100}, 1}; +uint8_t PINE; + enum team_color_e team_color; /** Stubbed chrono. */ -- cgit v1.2.3