summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-01 17:23:31 +0200
committerNicolas Schodet2011-06-01 21:48:47 +0200
commit8d758c96d257b15edc49dd4cb774ce67becf8cf1 (patch)
tree6805cd19b3b3267dbb4149af73e4d8186a856cf1 /digital
parente15457c84dc58e3433d66e2790fdd3a9368d7830 (diff)
digital/io-hub: drop in center of squares
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/robospierre/top.c22
1 files changed, 19 insertions, 3 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 0af8ce80..3a5ef25e 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -109,10 +109,25 @@ top_go_drop (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
uint8_t drop_pos_id = 43;
- vect_t drop_pos = element_get_pos (drop_pos_id);
+ position_t drop_pos;
+ drop_pos.v = element_get_pos (drop_pos_id);
uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
? 0 : ASSERV_BACKWARD;
- move_start_noangle (drop_pos, backward, 0);
+ /* Go above or below the drop point. */
+ if (drop_pos.v.y > PG_LENGTH / 2)
+ {
+ drop_pos.v.y -= 350 / 2;
+ drop_pos.a = PG_A_DEG (-90);
+ }
+ else
+ {
+ drop_pos.v.y += 350 / 2;
+ drop_pos.a = PG_A_DEG (90);
+ }
+ if (logistic_global.collect_direction == DIRECTION_BACKWARD)
+ drop_pos.a += PG_A_DEG (180);
+ /* Go! */
+ move_start (drop_pos, backward);
return 0;
}
@@ -190,7 +205,8 @@ FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING)
FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING)
{
- asserv_move_linearly (200);
+ asserv_move_linearly (logistic_global.collect_direction
+ == DIRECTION_FORWARD ? 200 : -200);
return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting);
}