From 3bf1b19e94a94909cb3366962a870bcbe0cdc122 Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 8 May 2010 01:42:32 +0200 Subject: [PATCH] forbid strat_eject() when we are not on correct line --- projects/microb2010/mainboard/strat.c | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 152c173..ccd1f02 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -285,17 +285,37 @@ void strat_event(void *dummy) //strat_limit_speed(); } +/* check that we are on an eject line */ +static uint8_t robot_is_on_eject_line(void) +{ + int16_t x, y; + uint8_t i, j; + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); + + if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) + return 0; + + if (!wp_belongs_to_line(i, j, 5, LINE_UP) && + !wp_belongs_to_line(i, j, 2, LINE_R_UP)) + return 0; + + return 1; +} /* must be called from a terminal line */ static uint8_t strat_eject(void) { uint8_t err; - /* XXX MUST be a on the line !! */ - //XXX return vals - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + if (!robot_is_on_eject_line()) { + DEBUG(E_USER_STRAT, "%s() not on eject line", __FUNCTION__); + return END_ERROR; + } + /* go to eject point */ trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); err = WAIT_COND_OR_TRAJ_END(get_cob_count() >= 5, -- 2.39.5