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/bot.h | 2 +- digital/io-hub/src/robospierre/clamp.c | 3 +++ digital/io-hub/src/robospierre/top.c | 13 +++++++++++++ 3 files changed, 17 insertions(+), 1 deletion(-) (limited to 'digital') diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index a2f8b820..fbbeeb7e 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -60,7 +60,7 @@ #define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600 /** Distance from border to go to capture a green element. */ #define BOT_GREEN_ELEMENT_DISTANCE_MM \ - (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 25) + (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 10) /** Speed used for initialisation. */ #ifdef HOST diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 6b48c56a..50afda1e 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -103,6 +103,8 @@ FSM_EVENTS ( clamp_new_element, /* Order to prepare tower. */ clamp_prepare, + /* Sent when an element has just been taken (door closed). */ + clamp_taken, /* Sent when clamp is working. */ clamp_working, /* Sent when clamp return to idle state. */ @@ -614,6 +616,7 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { + fsm_queue_post_event (FSM_EVENT (AI, clamp_taken)); logistic_element_new (ctx.pos_new, ctx.new_element_type); if (logistic_global.moving_from != CLAMP_SLOT_NB) { 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