From f073e6ad9d968bd0ae02fceb848f7473e4bb81f8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 15 May 2012 23:39:51 +0200 Subject: digital/io-hub/src/guybrush: prepare first decision in advance --- digital/io-hub/src/guybrush/strat.c | 19 +++++++++++++++++++ digital/io-hub/src/guybrush/strat.h | 5 +++++ digital/io-hub/src/guybrush/top.c | 3 ++- 3 files changed, 26 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/guybrush/strat.c b/digital/io-hub/src/guybrush/strat.c index bf81e462..7608ff65 100644 --- a/digital/io-hub/src/guybrush/strat.c +++ b/digital/io-hub/src/guybrush/strat.c @@ -92,6 +92,10 @@ struct strat_t uint8_t last_decision; /** Place of last decision. */ uint8_t last_place; + /** Decision is prepared in advance. */ + uint8_t prepared; + /** Prepared position. */ + vect_t prepared_pos; /** Robot content estimation. */ uint8_t load; /** Places information. */ @@ -161,6 +165,14 @@ strat_decision (vect_t *pos) int32_t best_score = -1; uint8_t best_place = 0; uint8_t i; + /* If decision was prepared, use it now. */ + if (strat.prepared) + { + strat.prepared = 0; + *pos = strat.prepared_pos; + return strat.last_decision; + } + /* Else compute the best decision. */ if (strat.load > 0) { strat.last_decision = STRAT_DECISION_UNLOAD; @@ -188,6 +200,13 @@ strat_decision (vect_t *pos) return -1; } +void +strat_prepare (void) +{ + strat_decision (&strat.prepared_pos); + strat.prepared = 1; +} + void strat_success (void) { diff --git a/digital/io-hub/src/guybrush/strat.h b/digital/io-hub/src/guybrush/strat.h index e20a44b8..36bed598 100644 --- a/digital/io-hub/src/guybrush/strat.h +++ b/digital/io-hub/src/guybrush/strat.h @@ -44,6 +44,11 @@ strat_init (void); uint8_t strat_decision (vect_t *pos); +/** Take a decision in advance so that next strat_decision call is + * immediate. */ +void +strat_prepare (void); + /** Report a success of last decision. */ void strat_success (void); diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 9ff8b18b..187edcab 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -213,6 +213,8 @@ FSM_TRANS (TOP_START, init_actuators, TOP_INIT_DOOR) main_demo = !IO_GET (CONTACT_STRAT); IO_SET (OUTPUT_DOOR_OPEN); IO_CLR (OUTPUT_DOOR_CLOSE); + strat_init (); + strat_prepare (); return FSM_NEXT (TOP_START, init_actuators); } @@ -228,7 +230,6 @@ FSM_TRANS (TOP_INIT, init_start_round, bottle, TOP_BOTTLE_GOING, unload, TOP_UNLOAD_GOING) { - strat_init (); main_usdist = 1; RETURN_TOP_DECISION_SWITCH (TOP_INIT, init_start_round); } -- cgit v1.2.3