summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 07:42:13 +0200
committerNicolas Schodet2011-06-02 09:21:46 +0200
commitf335b6c0c4fa4315229e9cc7ee01cb200207acdf (patch)
treeff7676695bd687dbce349d41abac053171c4904d
parent463cf200cf2fdfd5abd0359240e45526746ee588 (diff)
digital/io-hub: detect loss of side slot element
-rw-r--r--digital/io-hub/src/robospierre/logistic.c18
-rw-r--r--digital/io-hub/src/robospierre/logistic.h4
-rw-r--r--digital/io-hub/src/robospierre/test_element.c2
3 files changed, 24 insertions, 0 deletions
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. */