2 * Copyright Droids, Microb Technology (2009)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat_column_disp.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
28 #include <aversive/pgmspace.h>
29 #include <aversive/queue.h>
30 #include <aversive/wait.h>
31 #include <aversive/error.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <vect_base.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
54 #include "../common/i2c_commands.h"
58 #include "strat_base.h"
59 #include "strat_avoid.h"
60 #include "strat_utils.h"
62 #include "i2c_protocol.h"
64 #define ERROUT(e) do { \
69 /* distance between the wheel axis and the IR sensor */
70 #define IR_SHIFT_DISTANCE_RIGHT 85
71 #define IR_SHIFT_DISTANCE_LEFT 95
73 /* return red or green sensor */
74 #define COLOR_IR_SENSOR(left) \
78 __ret = sensor_get(S_DISP_LEFT); \
80 __ret = sensor_get(S_DISP_RIGHT); \
85 /* eject one col, some error codes are ignored here: we want to be
86 * sure that the column is correctly ejected. */
87 uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a)
91 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
92 trajectory_d_rel(&mainboard.traj, -300);
93 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
95 i2c_mechboard_mode_eject();
97 trajectory_a_abs(&mainboard.traj, eject_a);
99 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
100 i2c_mechboard_mode_clear();
102 trajectory_a_abs(&mainboard.traj, pickup_a);
104 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
108 /* get columns from dispenser. Must be called when the robot is in
109 * front of the dispenser. */
110 static uint8_t strat_pickup_col_disp(struct column_dispenser *disp)
112 uint16_t old_spdd, old_spda;
113 int16_t recalib_x, recalib_y;
114 int16_t eject_a, pickup_a;
115 uint8_t err, timeout = 0;
116 int8_t cols_count_before, cols_count_after, cols;
118 uint8_t first_try = 1;
119 uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
121 /* XXX set lazy pickup mode */
123 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
125 strat_get_speed(&old_spdd, &old_spda);
127 cols_count_before = get_column_count();
128 pickup_a = COLOR_A(disp->pickup_a);
129 eject_a = COLOR_A(disp->eject_a);
130 recalib_x = disp->recalib_x;
131 recalib_y = COLOR_Y(disp->recalib_y);
133 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
135 /* turn to dispenser */
136 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
137 trajectory_a_abs(&mainboard.traj, pickup_a);
138 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
139 if (!TRAJ_SUCCESS(err))
142 /* go forward until blocking, then go back ~30mm */
146 if (time_get_s() > 86) {
147 DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
151 if ((strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE) &&
152 strat_infos.col_in_boobs == 0 &&
153 strat_infos.lazy_pickup_done == 0) {
154 DEBUG(E_USER_STRAT, "%s() mode lazy", __FUNCTION__);
155 pickup_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
156 strat_infos.col_in_boobs = 1;
157 strat_infos.lazy_pickup_done = 1;
160 pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
161 strat_infos.col_in_boobs = 0;
165 i2c_mechboard_mode_lazy_harvest();
167 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
170 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
171 trajectory_d_rel(&mainboard.traj, 120);
172 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
173 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
175 err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
177 trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
178 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
179 if (!TRAJ_SUCCESS(err))
182 if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
183 strat_eject_col(eject_a, pickup_a);
187 /* start to pickup with finger / arms */
189 DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__);
191 if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
192 i2c_mechboard_mode_pickup();
194 i2c_mechboard_mode_lazy_pickup();
195 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100);
197 cols = get_column_count();
198 while (get_mechboard_mode() == pickup_mode) {
199 if (get_column_count() != cols) {
200 cols = get_column_count();
203 if ((get_column_count() - cols_count_before) >= disp->count) {
204 DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__);
207 /* 1 second timeout */
208 if (time_get_us2() - us > 1000000L) {
209 DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__);
215 /* eject if we found a bad color column */
217 if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
218 strat_eject_col(eject_a, pickup_a);
222 /* only recalib if it was not a timeout or if we got at least
224 if (timeout == 0 || (get_column_count() - cols_count_before >= 2))
225 strat_reset_pos(recalib_x, recalib_y, pickup_a);
227 /* else just update x or y depending on disp */
228 if (disp == &strat_infos.c1)
229 strat_reset_pos(recalib_x, DO_NOT_SET_POS,
232 strat_reset_pos(recalib_x, DO_NOT_SET_POS,
238 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
239 trajectory_d_rel(&mainboard.traj, -300);
240 wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
242 /* update dispenser count */
244 cols_count_after = get_column_count();
245 cols = cols_count_after - cols_count_before;
247 DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
254 if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
255 i2c_mechboard_mode_clear();
262 strat_set_speed(old_spdd, old_spda);
267 * Go in front of a dispenser. It will update the dispenser if it is
268 * c2 or c3 if we detect that this dispenser does not exist.
270 uint8_t strat_goto_col_disp(struct column_dispenser **pdisp)
273 int16_t checkpoint_x, checkpoint_y;
275 uint16_t old_spdd, old_spda, scan_left;
276 int16_t pos1x, pos1y, pos2x, pos2y, pos, dist;
277 int16_t margin_col2, margin_col3;
278 struct column_dispenser *disp = *pdisp;
280 if (disp->count <= 0)
283 if (disp->last_try_time >= time_get_s())
286 disp->last_try_time = time_get_s();
288 strat_get_speed(&old_spdd, &old_spda);
290 i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
291 I2C_MECHBOARD_MODE_CLEAR);
293 /* set some useful variables */
294 checkpoint_x = disp->checkpoint_x;
295 checkpoint_y = COLOR_Y(disp->checkpoint_y);
296 scan_a = COLOR_A(disp->scan_a);
297 scan_left = COLOR_INVERT(disp->scan_left);
299 /* goto checkpoint */
300 DEBUG(E_USER_STRAT, "%s(): goto %s (%d,%d) scan_left=%d",
301 __FUNCTION__, disp->name, checkpoint_x,
302 checkpoint_y, scan_left);
303 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
306 /* we have an intermediate checkpoint if we are on our
307 * side. If goto_and_avoid() returns END_ERROR, skip
308 * this checkpoint. */
309 if (position_get_x_s16(&mainboard.pos) < 1500) {
310 err = goto_and_avoid(1000, COLOR_Y(1500),
313 if (!TRAJ_SUCCESS(err) && err != END_ERROR)
317 /* go to checkpoint near the dispenser */
318 err = goto_and_avoid(checkpoint_x, checkpoint_y,
319 TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
320 if (!TRAJ_SUCCESS(err))
323 /* turn to correct angle to prepare scanning */
325 trajectory_a_abs(&mainboard.traj, scan_a);
326 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
327 if (!TRAJ_SUCCESS(err))
332 DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__);
334 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
335 trajectory_d_rel(&mainboard.traj, -1000);
336 err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(scan_left),
338 if (err) /* we should not reach end */
340 pos1x = position_get_x_s16(&mainboard.pos);
341 pos1y = position_get_y_s16(&mainboard.pos);
343 err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left),
347 pos2x = position_get_x_s16(&mainboard.pos);
348 pos2y = position_get_y_s16(&mainboard.pos);
350 dist = distance_between(pos1x, pos1y, pos2x, pos2y);
351 DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist);
354 trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2);
356 trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2);
357 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
359 if (disp == &strat_infos.c1)
362 /* mark c2 or c3 as empty... */
363 if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0)
366 pos = (pos2y + pos1y) / 2;
367 if (scan_a == 90) /* y is decreasing when scanning */
369 else if (scan_a == -90) /* y is increasing when scanning */
372 margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y));
373 margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y));
375 if (margin_col3 > margin_col2) {
376 DEBUG(E_USER_STRAT, "%s(): delete disp c3 (scan_pos=%d)", __FUNCTION__, pos);
377 strat_infos.c3.count = 0;
378 *pdisp = &strat_infos.c2;
379 if (strat_infos.c3.last_try_time > strat_infos.c2.last_try_time)
380 strat_infos.c2.last_try_time = strat_infos.c3.last_try_time;
383 DEBUG(E_USER_STRAT, "%s(): delete disp c2 (scan_pos=%d)", __FUNCTION__, pos);
384 strat_infos.c2.count = 0;
385 *pdisp = &strat_infos.c3;
386 if (strat_infos.c2.last_try_time > strat_infos.c3.last_try_time)
387 strat_infos.c3.last_try_time = strat_infos.c2.last_try_time;
392 strat_set_speed(old_spdd, old_spda);
396 /* return the best dispenser between the 2 */
397 static struct column_dispenser *
398 strat_disp_compare(struct column_dispenser *a,
399 struct column_dispenser *b)
401 uint8_t want_cols = 4 - get_column_count();
403 DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols);
405 /* an empty dispenser is not valid */
411 /* try to do a round robin: this is not optimal, but at least
412 * we will try another dispenser when one fails. */
413 if (a->last_try_time < b->last_try_time) {
416 if (b->last_try_time < a->last_try_time) {
420 /* take the one with the most columns */
421 if (a->count >= want_cols && b->count < want_cols)
424 /* take the one with the most columns */
425 if (b->count >= want_cols && a->count < want_cols)
428 /* the closer is the better */
429 if (distance_from_robot(a->recalib_x, COLOR_Y(a->recalib_y)) <
430 distance_from_robot(b->recalib_x, COLOR_Y(b->recalib_y))) {
436 /* choose the best dispenser */
437 static struct column_dispenser *strat_get_best_col_disp(void)
439 struct column_dispenser *disp;
441 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
443 /* for the first call, use c3 */
444 if (strat_infos.c1.last_try_time == 0 &&
445 strat_infos.c2.last_try_time == 0 &&
446 strat_infos.c3.last_try_time == 0)
447 return &strat_infos.c2; // XXX c3
449 DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__);
451 /* else compare with standard conditions */
452 disp = strat_disp_compare(&strat_infos.c1, &strat_infos.c2);
453 disp = strat_disp_compare(disp, &strat_infos.c3);
455 if (disp->count == 0)
461 /* choose the best dispenser, depending on disp count, distance,
462 * tries, ... and go pickup on it. */
463 uint8_t strat_pickup_columns(void)
465 struct column_dispenser *disp;
468 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
469 disp = strat_get_best_col_disp();
472 DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__);
476 err = strat_goto_col_disp(&disp);
477 if (!TRAJ_SUCCESS(err))
480 err = strat_pickup_col_disp(disp);
481 if (!TRAJ_SUCCESS(err))