summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 15:18:08 +0200
committerNicolas Schodet2011-06-02 16:03:50 +0200
commit4937eebb669121b5ee15a79267c6cbdc1ca88b97 (patch)
tree0c93f33fa3d9c8601f7c3a39930b08aad4f26165
parent79a1048f84c27da1dce7afb1266fa8423329a73b (diff)
digital/io-hub: drop single pawn when end of match approach
-rw-r--r--digital/io-hub/src/robospierre/top.c21
1 files changed, 17 insertions, 4 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 8ff69ee6..59a168ed 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -32,6 +32,7 @@
#include "logistic.h"
#include "move.h"
+#include "chrono.h"
/*
* Here is the top FSM. This FSM is suppose to give life to the robot with an
@@ -111,6 +112,17 @@ FSM_TRANS_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT, 250, TOP_GOING_OUT1)
}
static uint8_t
+top_prepare_level (void)
+{
+ if (ctx.broken)
+ return 3;
+ else if (logistic_global.need_prepare || chrono_remaining_time () < 45000l)
+ return 2;
+ else
+ return 1;
+}
+
+static uint8_t
top_go_element (void)
{
position_t robot_pos;
@@ -122,7 +134,7 @@ top_go_element (void)
if (e.attr & ELEMENT_GREEN)
logistic_global.prepare = 0;
else
- logistic_global.prepare = 1;
+ logistic_global.prepare = top_prepare_level ();
}
vect_t element_pos = element_get_pos (ctx.target_element_id);
uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
@@ -164,11 +176,12 @@ top_decision (void)
{
/* If we can make a tower. */
if (logistic_global.construct_possible == 1
- || (ctx.broken && logistic_global.construct_possible == 2))
+ || ((ctx.broken || logistic_global.prepare == 2)
+ && logistic_global.construct_possible == 2))
return top_go_drop ();
if (logistic_global.need_prepare)
{
- clamp_prepare (ctx.broken ? 3 : 2);
+ clamp_prepare (top_prepare_level ());
return top_go_drop ();
}
else
@@ -227,7 +240,7 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success,
}
else
{
- clamp_prepare (ctx.broken ? 3 : 1);
+ clamp_prepare (top_prepare_level ());
return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
}
}