summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/guybrush/main.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-16 21:30:58 +0200
committerNicolas Schodet2012-05-16 21:30:58 +0200
commit80164e9198ab46811509f7e8c7a06982bd5c7541 (patch)
tree1075a00458123ca47476d44c0ef35ee6a7e566ce /digital/io-hub/src/guybrush/main.c
parent28ea3c171495aef1b13ef6af28e3b65c9277b006 (diff)
digital/io-hub/src/guybrush: add demo mode control for totem
Diffstat (limited to 'digital/io-hub/src/guybrush/main.c')
-rw-r--r--digital/io-hub/src/guybrush/main.c42
1 files changed, 42 insertions, 0 deletions
diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c
index 63366c13..bc953471 100644
--- a/digital/io-hub/src/guybrush/main.c
+++ b/digital/io-hub/src/guybrush/main.c
@@ -54,6 +54,7 @@
#include "path.h"
#include "move.h"
#include "top.h"
+#include "bottom_clamp.h"
#include "bot.h"
@@ -154,6 +155,44 @@ main_init (void)
proto_send0 ('z');
}
+/** Generate demo mode events. */
+static uint8_t
+main_demo_events (void)
+{
+ static uint8_t color_switch_last = 0xff;
+ static uint8_t tree_step;
+ static uint8_t sleep;
+ /* Bounce detection. */
+ if (sleep)
+ {
+ sleep--;
+ return 0;
+ }
+ /* Look at color switch to control totem picking code. */
+ if (color_switch_last == 0xff)
+ color_switch_last = contact_get_color ();
+ if (color_switch_last != contact_get_color ())
+ {
+ color_switch_last = contact_get_color ();
+ sleep = 125;
+ switch (tree_step)
+ {
+ case 0:
+ clamp_request (FSM_EVENT (AI, tree_detected));
+ break;
+ case 1:
+ clamp_request (FSM_EVENT (AI, empty_tree));
+ break;
+ case 2:
+ clamp_request (FSM_EVENT (AI, robot_is_back));
+ break;
+ }
+ tree_step = (tree_step + 1) % 3;
+ return 1;
+ }
+ return 0;
+}
+
/** Main events management. */
uint8_t
main_event_to_fsm (void)
@@ -198,6 +237,9 @@ main_event_to_fsm (void)
FSM_HANDLE_E (AI,upper_set_down);
if (!IO_GET(CONTACT_UPPER_CLAMP_UP))
FSM_HANDLE_E (AI,upper_set_up);
+ /* Demo mode. */
+ if (main_demo && main_demo_events ())
+ return 1;
/* Events from the event queue. */
if (fsm_queue_poll ())
{