X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_column_disp.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fstrat_column_disp.c;h=e0b481e10a6507eaca17fd3cf3f5814ae1761d2f;hp=0000000000000000000000000000000000000000;hb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae;hpb=9d2d9100592e18fed985730298215884127fc568 diff --git a/projects/microb2010/mainboard/strat_column_disp.c b/projects/microb2010/mainboard/strat_column_disp.c new file mode 100644 index 0000000..e0b481e --- /dev/null +++ b/projects/microb2010/mainboard/strat_column_disp.c @@ -0,0 +1,485 @@ +/* + * Copyright Droids, Microb Technology (2009) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: strat_column_disp.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "actuator.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_avoid.h" +#include "strat_utils.h" +#include "sensor.h" +#include "i2c_protocol.h" + +#define ERROUT(e) do { \ + err = e; \ + goto end; \ + } while(0) + +/* distance between the wheel axis and the IR sensor */ +#define IR_SHIFT_DISTANCE_RIGHT 85 +#define IR_SHIFT_DISTANCE_LEFT 95 + +/* return red or green sensor */ +#define COLOR_IR_SENSOR(left) \ + ({ \ + uint8_t __ret = 0; \ + if (left) \ + __ret = sensor_get(S_DISP_LEFT); \ + else \ + __ret = sensor_get(S_DISP_RIGHT); \ + \ + __ret; \ + }) \ + +/* eject one col, some error codes are ignored here: we want to be + * sure that the column is correctly ejected. */ +uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a) +{ + uint8_t err; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + i2c_mechboard_mode_eject(); + time_wait_ms(600); + trajectory_a_abs(&mainboard.traj, eject_a); + + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + i2c_mechboard_mode_clear(); + time_wait_ms(1000); + trajectory_a_abs(&mainboard.traj, pickup_a); + + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + return err; +} + +/* get columns from dispenser. Must be called when the robot is in + * front of the dispenser. */ +static uint8_t strat_pickup_col_disp(struct column_dispenser *disp) +{ + uint16_t old_spdd, old_spda; + int16_t recalib_x, recalib_y; + int16_t eject_a, pickup_a; + uint8_t err, timeout = 0; + int8_t cols_count_before, cols_count_after, cols; + microseconds us; + uint8_t first_try = 1; + uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP; + + /* XXX set lazy pickup mode */ + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + strat_get_speed(&old_spdd, &old_spda); + + cols_count_before = get_column_count(); + pickup_a = COLOR_A(disp->pickup_a); + eject_a = COLOR_A(disp->eject_a); + recalib_x = disp->recalib_x; + recalib_y = COLOR_Y(disp->recalib_y); + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + + /* turn to dispenser */ + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + trajectory_a_abs(&mainboard.traj, pickup_a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* go forward until blocking, then go back ~30mm */ + + pickup_wheels_on(); + retry: + if (time_get_s() > 86) { + DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__); + return END_TIMER; + } + + if ((strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE) && + strat_infos.col_in_boobs == 0 && + strat_infos.lazy_pickup_done == 0) { + DEBUG(E_USER_STRAT, "%s() mode lazy", __FUNCTION__); + pickup_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; + strat_infos.col_in_boobs = 1; + strat_infos.lazy_pickup_done = 1; + } + else { + pickup_mode = I2C_MECHBOARD_MODE_PICKUP; + strat_infos.col_in_boobs = 0; + } + + if (first_try) + i2c_mechboard_mode_lazy_harvest(); + else + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + first_try = 0; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + + err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST); + + trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(eject_a, pickup_a); + goto retry; + } + + /* start to pickup with finger / arms */ + + DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__); + + if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP) + i2c_mechboard_mode_pickup(); + else + i2c_mechboard_mode_lazy_pickup(); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100); + us = time_get_us2(); + cols = get_column_count(); + while (get_mechboard_mode() == pickup_mode) { + if (get_column_count() != cols) { + cols = get_column_count(); + us = time_get_us2(); + } + if ((get_column_count() - cols_count_before) >= disp->count) { + DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__); + break; + } + /* 1 second timeout */ + if (time_get_us2() - us > 1000000L) { + DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__); + timeout = 1; + break; + } + } + + /* eject if we found a bad color column */ + + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(eject_a, pickup_a); + goto retry; + } + + /* only recalib if it was not a timeout or if we got at least + * 2 cols. */ + if (timeout == 0 || (get_column_count() - cols_count_before >= 2)) + strat_reset_pos(recalib_x, recalib_y, pickup_a); + else { + /* else just update x or y depending on disp */ + if (disp == &strat_infos.c1) + strat_reset_pos(recalib_x, DO_NOT_SET_POS, + DO_NOT_SET_POS); + else + strat_reset_pos(recalib_x, DO_NOT_SET_POS, + DO_NOT_SET_POS); + } + + /* go back */ + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -300); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + + /* update dispenser count */ + + cols_count_after = get_column_count(); + cols = cols_count_after - cols_count_before; + if (cols > 0) { + DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols); + disp->count -= cols; + if (disp->count < 0) + disp->count = 0; + } + + pickup_wheels_off(); + if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP) + i2c_mechboard_mode_clear(); + else + disp->count -= 2; + + ERROUT(END_TRAJ); + +end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* + * Go in front of a dispenser. It will update the dispenser if it is + * c2 or c3 if we detect that this dispenser does not exist. + */ +uint8_t strat_goto_col_disp(struct column_dispenser **pdisp) +{ + uint8_t err; + int16_t checkpoint_x, checkpoint_y; + int16_t scan_a; + uint16_t old_spdd, old_spda, scan_left; + int16_t pos1x, pos1y, pos2x, pos2y, pos, dist; + int16_t margin_col2, margin_col3; + struct column_dispenser *disp = *pdisp; + + if (disp->count <= 0) + return END_ERROR; + + if (disp->last_try_time >= time_get_s()) + return END_ERROR; + + disp->last_try_time = time_get_s(); + + strat_get_speed(&old_spdd, &old_spda); + + i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, + I2C_MECHBOARD_MODE_CLEAR); + + /* set some useful variables */ + checkpoint_x = disp->checkpoint_x; + checkpoint_y = COLOR_Y(disp->checkpoint_y); + scan_a = COLOR_A(disp->scan_a); + scan_left = COLOR_INVERT(disp->scan_left); + + /* goto checkpoint */ + DEBUG(E_USER_STRAT, "%s(): goto %s (%d,%d) scan_left=%d", + __FUNCTION__, disp->name, checkpoint_x, + checkpoint_y, scan_left); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + +#if 0 + /* we have an intermediate checkpoint if we are on our + * side. If goto_and_avoid() returns END_ERROR, skip + * this checkpoint. */ + if (position_get_x_s16(&mainboard.pos) < 1500) { + err = goto_and_avoid(1000, COLOR_Y(1500), + TRAJ_FLAGS_STD, + TRAJ_FLAGS_STD); + if (!TRAJ_SUCCESS(err) && err != END_ERROR) + ERROUT(err); + } +#endif + /* go to checkpoint near the dispenser */ + err = goto_and_avoid(checkpoint_x, checkpoint_y, + TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* turn to correct angle to prepare scanning */ + + trajectory_a_abs(&mainboard.traj, scan_a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* scan now */ + + DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -1000); + err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(scan_left), + TRAJ_FLAGS_NO_NEAR); + if (err) /* we should not reach end */ + ERROUT(END_ERROR); + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + + err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left), + TRAJ_FLAGS_NO_NEAR); + if (err) + ERROUT(END_ERROR); + pos2x = position_get_x_s16(&mainboard.pos); + pos2y = position_get_y_s16(&mainboard.pos); + + dist = distance_between(pos1x, pos1y, pos2x, pos2y); + DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist); + + if (scan_left) + trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2); + else + trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + if (disp == &strat_infos.c1) + ERROUT(END_TRAJ); + + /* mark c2 or c3 as empty... */ + if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0) + ERROUT(END_TRAJ); + + pos = (pos2y + pos1y) / 2; + if (scan_a == 90) /* y is decreasing when scanning */ + pos -= 80; + else if (scan_a == -90) /* y is increasing when scanning */ + pos += 80; + + margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y)); + margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y)); + + if (margin_col3 > margin_col2) { + DEBUG(E_USER_STRAT, "%s(): delete disp c3 (scan_pos=%d)", __FUNCTION__, pos); + strat_infos.c3.count = 0; + *pdisp = &strat_infos.c2; + if (strat_infos.c3.last_try_time > strat_infos.c2.last_try_time) + strat_infos.c2.last_try_time = strat_infos.c3.last_try_time; + } + else { + DEBUG(E_USER_STRAT, "%s(): delete disp c2 (scan_pos=%d)", __FUNCTION__, pos); + strat_infos.c2.count = 0; + *pdisp = &strat_infos.c3; + if (strat_infos.c2.last_try_time > strat_infos.c3.last_try_time) + strat_infos.c3.last_try_time = strat_infos.c2.last_try_time; + } + ERROUT(END_TRAJ); + +end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* return the best dispenser between the 2 */ +static struct column_dispenser * +strat_disp_compare(struct column_dispenser *a, + struct column_dispenser *b) +{ + uint8_t want_cols = 4 - get_column_count(); + + DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols); + + /* an empty dispenser is not valid */ + if (a->count == 0) + return b; + if (b->count == 0) + return a; + + /* try to do a round robin: this is not optimal, but at least + * we will try another dispenser when one fails. */ + if (a->last_try_time < b->last_try_time) { + return a; + } + if (b->last_try_time < a->last_try_time) { + return b; + } + + /* take the one with the most columns */ + if (a->count >= want_cols && b->count < want_cols) + return a; + + /* take the one with the most columns */ + if (b->count >= want_cols && a->count < want_cols) + return b; + + /* the closer is the better */ + if (distance_from_robot(a->recalib_x, COLOR_Y(a->recalib_y)) < + distance_from_robot(b->recalib_x, COLOR_Y(b->recalib_y))) { + return a; + } + return b; +} + +/* choose the best dispenser */ +static struct column_dispenser *strat_get_best_col_disp(void) +{ + struct column_dispenser *disp; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + /* for the first call, use c3 */ + if (strat_infos.c1.last_try_time == 0 && + strat_infos.c2.last_try_time == 0 && + strat_infos.c3.last_try_time == 0) + return &strat_infos.c2; // XXX c3 + + DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__); + + /* else compare with standard conditions */ + disp = strat_disp_compare(&strat_infos.c1, &strat_infos.c2); + disp = strat_disp_compare(disp, &strat_infos.c3); + + if (disp->count == 0) + return NULL; + + return disp; +} + +/* choose the best dispenser, depending on disp count, distance, + * tries, ... and go pickup on it. */ +uint8_t strat_pickup_columns(void) +{ + struct column_dispenser *disp; + uint8_t err; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + disp = strat_get_best_col_disp(); + + if (disp == NULL) { + DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__); + return END_ERROR; + } + + err = strat_goto_col_disp(&disp); + if (!TRAJ_SUCCESS(err)) + return err; + + err = strat_pickup_col_disp(disp); + if (!TRAJ_SUCCESS(err)) + return err; + + return END_TRAJ; +}