X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_lintel.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fstrat_lintel.c;h=72ab29904156d3c0701d17f8d5b6cdf102b4aee4;hp=0000000000000000000000000000000000000000;hb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae;hpb=9d2d9100592e18fed985730298215884127fc568 diff --git a/projects/microb2010/mainboard/strat_lintel.c b/projects/microb2010/mainboard/strat_lintel.c new file mode 100644 index 0000000..72ab299 --- /dev/null +++ b/projects/microb2010/mainboard/strat_lintel.c @@ -0,0 +1,247 @@ +/* + * Copyright Droids, Microb Technology (2008) + * + * 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_lintel.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 "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) + +#define X_PRE_MARGIN 20 +#define X_POST_MARGIN 10 + +/* + * goto lintel disp. Return END_TRAJ if success or if there is nothing + * to do. Return END_ERROR if dest cannot be reached, else, it may + * return END_OBSTACLE or END_BLOCKING. + */ +uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp) +{ + uint8_t err, first_try = 1, right_ok, left_ok; + uint16_t old_spdd, old_spda; + int16_t left_cur, right_cur, a; + + if (disp->count == 0) + return END_ERROR; + + if (get_lintel_count() == 2) + 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); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + DEBUG(E_USER_STRAT, "%s(): goto %s", __FUNCTION__, disp->name); + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + + err = goto_and_avoid_backward(disp->x, COLOR_Y(400), + TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + trajectory_a_abs(&mainboard.traj, COLOR_A(-90)); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (time_get_s() > 86) { + DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__); + return END_TIMER; + } + + i2c_mechboard_mode_prepare_get_lintel(); + retry: + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { + a = position_get_a_deg_s16(&mainboard.pos); + /* only reset pos if angle is not too different */ + if (ABS(a - COLOR_A(-90)) < 5) + strat_reset_pos(DO_NOT_SET_POS, + COLOR_Y(ROBOT_LENGTH/2), + COLOR_A(-90)); + } + else if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + i2c_mechboard_mode_get_lintel(); + time_wait_ms(500); + + left_cur = sensor_get_adc(ADC_CSENSE3); + left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN); + right_cur = mechboard.pump_right1_current; + right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN); + + DEBUG(E_USER_STRAT, "%s left_ok=%d (%d), right_ok=%d (%d)", __FUNCTION__, + left_ok, left_cur, right_ok, right_cur); + if (first_try) { + if (!right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + /* XXX recalib x ? */ + else if (right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -200, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, 190, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + else if (!right_ok && left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -200, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, 190, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + /* else, lintel is ok */ + else { + strat_infos.taken_lintel ++; + i2c_mechboard_mode_put_lintel(); + } + } + else { + if (right_ok && left_ok) { + /* lintel is ok */ + strat_infos.taken_lintel ++; + i2c_mechboard_mode_put_lintel(); + } + else { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + } + disp->count--; + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -250); + err = wait_traj_end(TRAJ_FLAGS_STD); + + ERROUT(err); + +end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* go pickup lintels on dispensers. Return END_TRAJ on success or if + + * there is nothing to do, else return the error status. */ +uint8_t strat_pickup_lintels(void) +{ + uint8_t err; + + if (get_column_count() != 0) + return END_ERROR; + + if (strat_infos.l1.count == 0 && strat_infos.l2.count == 0) + return END_TRAJ; + + /* skip if it's too early */ + if (time_get_s() < strat_infos.conf.lintel_min_time) + return END_TRAJ; + + /* skip next lintel if we want only one */ + if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) { + if (strat_infos.taken_lintel) + return END_TRAJ; + } + + /* don't take lintel now if we already have one and if there + * is not much time */ + if (get_lintel_count() && time_get_s() > 75) + return END_TRAJ; + + /* take lintel 1 */ + err = strat_goto_lintel_disp(&strat_infos.l1); + if (!TRAJ_SUCCESS(err) && err != END_ERROR) + return err; + + /* skip next lintel if we want only one */ + if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) { + if (strat_infos.taken_lintel) + return END_TRAJ; + } + + /* don't take lintel now if we already have one and if there + * is not much time */ + if (get_lintel_count() && time_get_s() > 75) + return END_TRAJ; + + /* take lintel 2 */ + err = strat_goto_lintel_disp(&strat_infos.l2); + if (!TRAJ_SUCCESS(err) && err != END_ERROR) + return err; + + return END_TRAJ; +}