projects/microb2010/mainboard/strat_column_disp.c

   1 /*
   2  *  Copyright Droids, Microb Technology (2009)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: strat_column_disp.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26 #include <math.h>
  27
  28 #include <aversive/pgmspace.h>
  29 #include <aversive/queue.h>
  30 #include <aversive/wait.h>
  31 #include <aversive/error.h>
  32
  33 #include <ax12.h>
  34 #include <uart.h>
  35 #include <pwm_ng.h>
  36 #include <time.h>
  37 #include <spi.h>
  38
  39 #include <pid.h>
  40 #include <quadramp.h>
  41 #include <control_system_manager.h>
  42 #include <trajectory_manager.h>
  43 #include <vect_base.h>
  44 #include <lines.h>
  45 #include <polygon.h>
  46 #include <obstacle_avoidance.h>
  47 #include <blocking_detection_manager.h>
  48 #include <robot_system.h>
  49 #include <position_manager.h>
  50
  51 #include <rdline.h>
  52 #include <parse.h>
  53
  54 #include "../common/i2c_commands.h"
  55 #include "main.h"
  56 #include "actuator.h"
  57 #include "strat.h"
  58 #include "strat_base.h"
  59 #include "strat_avoid.h"
  60 #include "strat_utils.h"
  61 #include "sensor.h"
  62 #include "i2c_protocol.h"
  63
  64 #define ERROUT(e) do {                          \
  65                 err = e;                        \
  66                 goto end;                       \
  67         } while(0)
  68
  69 /* distance between the wheel axis and the IR sensor */
  70 #define IR_SHIFT_DISTANCE_RIGHT 85
  71 #define IR_SHIFT_DISTANCE_LEFT  95
  72
  73 /* return red or green sensor */
  74 #define COLOR_IR_SENSOR(left)                                           \
  75         ({                                                              \
  76                 uint8_t __ret = 0;                                      \
  77                 if (left)                                               \
  78                         __ret = sensor_get(S_DISP_LEFT);                \
  79                 else                                                    \
  80                         __ret = sensor_get(S_DISP_RIGHT);               \
  81                                                                         \
  82                 __ret;                                                  \
  83         })                                                              \
  84
  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)
  88 {
  89         uint8_t err;
  90
  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);
  94
  95         i2c_mechboard_mode_eject();
  96         time_wait_ms(600);
  97         trajectory_a_abs(&mainboard.traj, eject_a);
  98
  99         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
 100         i2c_mechboard_mode_clear();
 101         time_wait_ms(1000);
 102         trajectory_a_abs(&mainboard.traj, pickup_a);
 103
 104         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 105         return err;
 106 }
 107
 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)
 111 {
 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;
 117         microseconds us;
 118         uint8_t first_try = 1;
 119         uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
 120
 121         /* XXX set lazy pickup mode */
 122
 123         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 124
 125         strat_get_speed(&old_spdd, &old_spda);
 126
 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);
 132
 133         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 134
 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))
 140                 ERROUT(err);
 141
 142         /* go forward until blocking, then go back ~30mm */
 143
 144         pickup_wheels_on();
 145  retry:
 146         if (time_get_s() > 86) {
 147                 DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
 148                 return END_TIMER;
 149         }
 150
 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;
 158         }
 159         else {
 160                 pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
 161                 strat_infos.col_in_boobs = 0;
 162         }
 163
 164         if (first_try)
 165                 i2c_mechboard_mode_lazy_harvest();
 166         else
 167                 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
 168         first_try = 0;
 169
 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);
 174
 175         err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
 176
 177         trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
 178         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 179         if (!TRAJ_SUCCESS(err))
 180                 ERROUT(err);
 181
 182         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
 183                 strat_eject_col(eject_a, pickup_a);
 184                 goto retry;
 185         }
 186
 187         /* start to pickup with finger / arms */
 188
 189         DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__);
 190
 191         if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
 192                 i2c_mechboard_mode_pickup();
 193         else
 194                 i2c_mechboard_mode_lazy_pickup();
 195         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100);
 196         us = time_get_us2();
 197         cols = get_column_count();
 198         while (get_mechboard_mode() == pickup_mode) {
 199                 if (get_column_count() != cols) {
 200                         cols = get_column_count();
 201                         us = time_get_us2();
 202                 }
 203                 if ((get_column_count() - cols_count_before) >= disp->count) {
 204                         DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__);
 205                         break;
 206                 }
 207                 /* 1 second timeout */
 208                 if (time_get_us2() - us > 1000000L) {
 209                         DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__);
 210                         timeout = 1;
 211                         break;
 212                 }
 213         }
 214
 215         /* eject if we found a bad color column */
 216
 217         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
 218                 strat_eject_col(eject_a, pickup_a);
 219                 goto retry;
 220         }
 221
 222         /* only recalib if it was not a timeout or if we got at least
 223          * 2 cols. */
 224         if (timeout == 0 || (get_column_count() - cols_count_before >= 2))
 225                 strat_reset_pos(recalib_x, recalib_y, pickup_a);
 226         else {
 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,
 230                                         DO_NOT_SET_POS);
 231                 else
 232                         strat_reset_pos(recalib_x, DO_NOT_SET_POS,
 233                                         DO_NOT_SET_POS);
 234         }
 235
 236         /* go back */
 237
 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);
 241
 242         /* update dispenser count */
 243
 244         cols_count_after = get_column_count();
 245         cols = cols_count_after - cols_count_before;
 246         if (cols > 0) {
 247                 DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
 248                 disp->count -= cols;
 249                 if (disp->count < 0)
 250                         disp->count = 0;
 251         }
 252
 253         pickup_wheels_off();
 254         if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
 255                 i2c_mechboard_mode_clear();
 256         else
 257                 disp->count -= 2;
 258
 259         ERROUT(END_TRAJ);
 260
 261 end:
 262         strat_set_speed(old_spdd, old_spda);
 263         return err;
 264 }
 265
 266 /*
 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.
 269  */
 270 uint8_t strat_goto_col_disp(struct column_dispenser **pdisp)
 271 {
 272         uint8_t err;
 273         int16_t checkpoint_x, checkpoint_y;
 274         int16_t scan_a;
 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;
 279
 280         if (disp->count <= 0)
 281                 return END_ERROR;
 282
 283         if (disp->last_try_time >= time_get_s())
 284                 return END_ERROR;
 285
 286         disp->last_try_time = time_get_s();
 287
 288         strat_get_speed(&old_spdd, &old_spda);
 289
 290         i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
 291                                                I2C_MECHBOARD_MODE_CLEAR);
 292
 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);
 298
 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);
 304
 305 #if 0
 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),
 311                                      TRAJ_FLAGS_STD,
 312                                      TRAJ_FLAGS_STD);
 313                 if (!TRAJ_SUCCESS(err) && err != END_ERROR)
 314                         ERROUT(err);
 315         }
 316 #endif
 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))
 321                 ERROUT(err);
 322
 323         /* turn to correct angle to prepare scanning */
 324
 325         trajectory_a_abs(&mainboard.traj, scan_a);
 326         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 327         if (!TRAJ_SUCCESS(err))
 328                 ERROUT(err);
 329
 330         /* scan now */
 331
 332         DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__);
 333
 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),
 337                                     TRAJ_FLAGS_NO_NEAR);
 338         if (err) /* we should not reach end */
 339                 ERROUT(END_ERROR);
 340         pos1x = position_get_x_s16(&mainboard.pos);
 341         pos1y = position_get_y_s16(&mainboard.pos);
 342
 343         err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left),
 344                                     TRAJ_FLAGS_NO_NEAR);
 345         if (err)
 346                 ERROUT(END_ERROR);
 347         pos2x = position_get_x_s16(&mainboard.pos);
 348         pos2y = position_get_y_s16(&mainboard.pos);
 349
 350         dist = distance_between(pos1x, pos1y, pos2x, pos2y);
 351         DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist);
 352
 353         if (scan_left)
 354                 trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2);
 355         else
 356                 trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2);
 357         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 358
 359         if (disp == &strat_infos.c1)
 360                 ERROUT(END_TRAJ);
 361
 362         /* mark c2 or c3 as empty... */
 363         if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0)
 364                 ERROUT(END_TRAJ);
 365
 366         pos = (pos2y + pos1y) / 2;
 367         if (scan_a == 90) /* y is decreasing when scanning */
 368                 pos -= 80;
 369         else if (scan_a == -90) /* y is increasing when scanning */
 370                 pos += 80;
 371
 372         margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y));
 373         margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y));
 374
 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;
 381         }
 382         else {
 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;
 388         }
 389         ERROUT(END_TRAJ);
 390
 391 end:
 392         strat_set_speed(old_spdd, old_spda);
 393         return err;
 394 }
 395
 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)
 400 {
 401         uint8_t want_cols = 4 - get_column_count();
 402
 403         DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols);
 404
 405         /* an empty dispenser is not valid */
 406         if (a->count == 0)
 407                 return b;
 408         if (b->count == 0)
 409                 return a;
 410
 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) {
 414                 return a;
 415         }
 416         if (b->last_try_time < a->last_try_time) {
 417                 return b;
 418         }
 419
 420         /* take the one with the most columns */
 421         if (a->count >= want_cols && b->count < want_cols)
 422                 return a;
 423
 424         /* take the one with the most columns */
 425         if (b->count >= want_cols && a->count < want_cols)
 426                 return b;
 427
 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))) {
 431                 return a;
 432         }
 433         return b;
 434 }
 435
 436 /* choose the best dispenser */
 437 static struct column_dispenser *strat_get_best_col_disp(void)
 438 {
 439         struct column_dispenser *disp;
 440
 441         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 442
 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
 448
 449         DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__);
 450
 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);
 454
 455         if (disp->count == 0)
 456                 return NULL;
 457
 458         return disp;
 459 }
 460
 461 /* choose the best dispenser, depending on disp count, distance,
 462  * tries, ... and go pickup on it. */
 463 uint8_t strat_pickup_columns(void)
 464 {
 465         struct column_dispenser *disp;
 466         uint8_t err;
 467
 468         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 469         disp = strat_get_best_col_disp();
 470
 471         if (disp == NULL) {
 472                 DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__);
 473                 return END_ERROR;
 474         }
 475
 476         err = strat_goto_col_disp(&disp);
 477         if (!TRAJ_SUCCESS(err))
 478                 return err;
 479
 480         err = strat_pickup_col_disp(disp);
 481         if (!TRAJ_SUCCESS(err))
 482                 return err;
 483
 484         return END_TRAJ;
 485 }