summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-03 06:15:17 +0200
committerNicolas Schodet2011-06-03 06:53:45 +0200
commit002c8f8819a34cc8d589115cca433de716036d8c (patch)
treebf0298c4a80f37141998ab6f134b37d28386ce8d
parent2eca57339e792160fe21a17956a1257403296643 (diff)
digital/io-hub: little step backward in green zone
-rw-r--r--digital/io-hub/src/robospierre/bot.h2
-rw-r--r--digital/io-hub/src/robospierre/clamp.c3
-rw-r--r--digital/io-hub/src/robospierre/top.c13
3 files changed, 17 insertions, 1 deletions
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)