From 002c8f8819a34cc8d589115cca433de716036d8c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 06:15:17 +0200 Subject: digital/io-hub: little step backward in green zone --- digital/io-hub/src/robospierre/top.c | 13 +++++++++++++ 1 file changed, 13 insertions(+) (limited to 'digital/io-hub/src/robospierre/top.c') diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 286e819c..9cdefc3e 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -81,6 +81,8 @@ struct top_t uint8_t chaos; /** Broken clamp. */ uint8_t broken; + /** Saved, direction when picking element. */ + uint8_t go_to_element_direction; }; /** Global context. */ @@ -140,6 +142,7 @@ top_go_element (void) logistic_global.prepare = top_prepare_level (); } vect_t element_pos = element_get_pos (ctx.target_element_id); + ctx.go_to_element_direction = logistic_global.collect_direction; uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; move_start_noangle (element_pos, backward, 0); @@ -325,6 +328,16 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); } +FSM_TRANS (TOP_WAITING_CLAMP, clamp_taken, TOP_WAITING_CLAMP) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + if (robot_pos.v.x < 400 || robot_pos.v.x > PG_WIDTH - 400) + asserv_move_linearly (ctx.go_to_element_direction + == DIRECTION_FORWARD ? -50 : 50); + return FSM_NEXT (TOP_WAITING_CLAMP, clamp_taken); +} + FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250, try_again, TOP_UNBLOCKING_SHAKE, tryout, TOP_WAITING_CLAMP) -- cgit v1.2.3