summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-09-21 20:07:10 +0200
committerNicolas Schodet2011-09-21 20:07:10 +0200
commit456d1d6670a1a1cf6a01dbbf718a84cfa0a0f34c (patch)
tree92dd9b1e761149169436530ee91f1fa9db4d0982 /digital/io-hub/src/robospierre
parent7c2fd5bfc8e24005e791bc9536e40972b5a4737a (diff)
digital/io-hub/src/robospierre: go to green zone several times
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/element.c29
-rw-r--r--digital/io-hub/src/robospierre/element.h4
-rw-r--r--digital/io-hub/src/robospierre/top.c21
3 files changed, 42 insertions, 12 deletions
diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c
index 7319adbb..881e1be6 100644
--- a/digital/io-hub/src/robospierre/element.c
+++ b/digital/io-hub/src/robospierre/element.c
@@ -675,18 +675,6 @@ element_down (uint8_t element_id, uint8_t element_type)
element_set (i, ie);
}
}
-
- /* Remove our green zone score at first unload. */
- for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++)
- {
- element_t e = element_get (i);
- if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
- (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
- {
- e.bonus_load = 0;
- element_set (i, e);
- }
- }
}
void
@@ -821,3 +809,20 @@ element_i_like_green ()
}
}
}
+
+void
+element_no_more_green (void)
+{
+ uint8_t i;
+ /* Remove our green zone score at first unload. */
+ for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++)
+ {
+ element_t e = element_get (i);
+ if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
+ {
+ e.bonus_load = 0;
+ element_set (i, e);
+ }
+ }
+}
diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h
index 243f5b94..41985e95 100644
--- a/digital/io-hub/src/robospierre/element.h
+++ b/digital/io-hub/src/robospierre/element.h
@@ -206,4 +206,8 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab, uint8_t escape);
void
element_i_like_green ();
+/** Do not go to our green zone any more. */
+void
+element_no_more_green (void);
+
#endif /* element_h */
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 7732fb50..93fb447f 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -90,6 +90,8 @@ struct top_t
uint8_t broken;
/** Saved, direction when picking element. */
uint8_t go_to_element_direction;
+ /** Return to our green zone again. */
+ uint8_t green_again;
};
/** Global context. */
@@ -99,6 +101,7 @@ struct top_t top_global;
FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1)
{
element_init ();
+ ctx.green_again = 3;
asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100),
PG_Y (PG_LENGTH - 200), 0);
return FSM_NEXT (TOP_START, init_start_round);
@@ -305,6 +308,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
element_failure (ctx.target_element_id); /* Do not take this one again. */
if (clamp_working ())
return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
+ if (ctx.green_again)
+ {
+ ctx.green_again--;
+ if (ctx.green_again == 0)
+ element_no_more_green ();
+ }
switch (top_decision ())
{
default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop);
@@ -321,6 +330,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
element_failure (ctx.target_element_id);
if (clamp_working ())
return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working);
+ if (ctx.green_again)
+ {
+ ctx.green_again--;
+ if (ctx.green_again == 0)
+ element_no_more_green ();
+ }
switch (top_decision ())
{
default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop);
@@ -331,6 +346,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP)
{
move_stop ();
+ if (ctx.green_again)
+ {
+ ctx.green_again--;
+ if (ctx.green_again == 0)
+ element_no_more_green ();
+ }
return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working);
}