projects/microb2010/mainboard/strat_lintel.c

   1 /*
   2  *  Copyright Droids, Microb Technology (2008)
   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_lintel.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 "strat.h"
  57 #include "strat_base.h"
  58 #include "strat_avoid.h"
  59 #include "strat_utils.h"
  60 #include "sensor.h"
  61 #include "i2c_protocol.h"
  62
  63 #define ERROUT(e) do {                          \
  64                 err = e;                        \
  65                 goto end;                       \
  66         } while(0)
  67
  68 #define X_PRE_MARGIN 20
  69 #define X_POST_MARGIN 10
  70
  71 /*
  72  * goto lintel disp. Return END_TRAJ if success or if there is nothing
  73  * to do. Return END_ERROR if dest cannot be reached, else, it may
  74  * return END_OBSTACLE or END_BLOCKING.
  75  */
  76 uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp)
  77 {
  78         uint8_t err, first_try = 1, right_ok, left_ok;
  79         uint16_t old_spdd, old_spda;
  80         int16_t left_cur, right_cur, a;
  81
  82         if (disp->count == 0)
  83                 return END_ERROR;
  84
  85         if (get_lintel_count() == 2)
  86                 return END_ERROR;
  87
  88         if (disp->last_try_time >= time_get_s())
  89                 return END_ERROR;
  90
  91         disp->last_try_time = time_get_s();
  92
  93         strat_get_speed(&old_spdd, &old_spda);
  94         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
  95
  96         DEBUG(E_USER_STRAT, "%s(): goto %s", __FUNCTION__, disp->name);
  97         i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
  98
  99         err = goto_and_avoid_backward(disp->x, COLOR_Y(400),
 100                                       TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
 101         if (!TRAJ_SUCCESS(err))
 102                 ERROUT(err);
 103
 104         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 105         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 106         if (!TRAJ_SUCCESS(err))
 107                 ERROUT(err);
 108
 109         if (time_get_s() > 86) {
 110                 DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
 111                 return END_TIMER;
 112         }
 113
 114         i2c_mechboard_mode_prepare_get_lintel();
 115  retry:
 116         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 117         err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
 118         if (err == END_BLOCKING) {
 119                 a = position_get_a_deg_s16(&mainboard.pos);
 120                 /* only reset pos if angle is not too different */
 121                 if (ABS(a - COLOR_A(-90)) < 5)
 122                         strat_reset_pos(DO_NOT_SET_POS,
 123                                         COLOR_Y(ROBOT_LENGTH/2),
 124                                         COLOR_A(-90));
 125         }
 126         else if (!TRAJ_SUCCESS(err))
 127                 ERROUT(err);
 128
 129         i2c_mechboard_mode_get_lintel();
 130         time_wait_ms(500);
 131
 132         left_cur = sensor_get_adc(ADC_CSENSE3);
 133         left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
 134         right_cur = mechboard.pump_right1_current;
 135         right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
 136
 137         DEBUG(E_USER_STRAT, "%s left_ok=%d (%d), right_ok=%d (%d)", __FUNCTION__,
 138               left_ok, left_cur, right_ok, right_cur);
 139         if (first_try) {
 140                 if (!right_ok && !left_ok) {
 141                         i2c_mechboard_mode_prepare_get_lintel();
 142                         time_wait_ms(300);
 143                 }
 144                 /* XXX recalib x ? */
 145                 else if (right_ok && !left_ok) {
 146                         i2c_mechboard_mode_prepare_get_lintel();
 147                         time_wait_ms(300);
 148                         strat_set_speed(500, 500);
 149                         trajectory_d_a_rel(&mainboard.traj, -200, 30);
 150                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 151                         trajectory_d_a_rel(&mainboard.traj, 190, -30);
 152                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 153                         first_try = 0;
 154                         goto retry;
 155                 }
 156                 else if (!right_ok && left_ok) {
 157                         i2c_mechboard_mode_prepare_get_lintel();
 158                         time_wait_ms(300);
 159                         strat_set_speed(500, 500);
 160                         trajectory_d_a_rel(&mainboard.traj, -200, -30);
 161                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 162                         trajectory_d_a_rel(&mainboard.traj, 190, 30);
 163                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 164                         first_try = 0;
 165                         goto retry;
 166                 }
 167                 /* else, lintel is ok */
 168                 else {
 169                         strat_infos.taken_lintel ++;
 170                         i2c_mechboard_mode_put_lintel();
 171                 }
 172         }
 173         else {
 174                 if (right_ok && left_ok) {
 175                         /* lintel is ok */
 176                         strat_infos.taken_lintel ++;
 177                         i2c_mechboard_mode_put_lintel();
 178                 }
 179                 else {
 180                         i2c_mechboard_mode_prepare_get_lintel();
 181                         time_wait_ms(300);
 182                 }
 183         }
 184         disp->count--;
 185
 186         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 187         trajectory_d_rel(&mainboard.traj, -250);
 188         err = wait_traj_end(TRAJ_FLAGS_STD);
 189
 190         ERROUT(err);
 191
 192 end:
 193         strat_set_speed(old_spdd, old_spda);
 194         return err;
 195 }
 196
 197 /* go pickup lintels on dispensers. Return END_TRAJ on success or if
 198
 199  * there is nothing to do, else return the error status. */
 200 uint8_t strat_pickup_lintels(void)
 201 {
 202         uint8_t err;
 203
 204         if (get_column_count() != 0)
 205                 return END_ERROR;
 206
 207         if (strat_infos.l1.count == 0 && strat_infos.l2.count == 0)
 208                 return END_TRAJ;
 209
 210         /* skip if it's too early */
 211         if (time_get_s() < strat_infos.conf.lintel_min_time)
 212                 return END_TRAJ;
 213
 214         /* skip next lintel if we want only one */
 215         if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
 216                 if (strat_infos.taken_lintel)
 217                         return END_TRAJ;
 218         }
 219
 220         /* don't take lintel now if we already have one and if there
 221          * is not much time */
 222         if (get_lintel_count() && time_get_s() > 75)
 223                 return END_TRAJ;
 224
 225         /* take lintel 1 */
 226         err = strat_goto_lintel_disp(&strat_infos.l1);
 227         if (!TRAJ_SUCCESS(err) && err != END_ERROR)
 228                 return err;
 229
 230         /* skip next lintel if we want only one */
 231         if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
 232                 if (strat_infos.taken_lintel)
 233                         return END_TRAJ;
 234         }
 235
 236         /* don't take lintel now if we already have one and if there
 237          * is not much time */
 238         if (get_lintel_count() && time_get_s() > 75)
 239                 return END_TRAJ;
 240
 241         /* take lintel 2 */
 242         err = strat_goto_lintel_disp(&strat_infos.l2);
 243         if (!TRAJ_SUCCESS(err) && err != END_ERROR)
 244                 return err;
 245
 246         return END_TRAJ;
 247 }