projects/microb2009/mainboard/strat_static_columns.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_static_columns.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_utils.h"
  59 #include "strat_avoid.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 BIG_DIST 5000
  69
  70 /*
  71  * must be called from start area.
  72  * get 4 static columns and build a temple on the disc
  73  */
  74 uint8_t strat_static_columns(uint8_t configuration)
  75 {
  76         uint8_t err;
  77         uint8_t col1_present = 0, col4_present = 0;
  78         uint16_t old_spdd, old_spda;
  79
  80         DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration);
  81
  82         strat_get_speed(&old_spdd, &old_spda);
  83
  84         /* calibrate scanner */
  85         i2c_sensorboard_scanner_calib();
  86
  87         i2c_mechboard_mode_harvest();
  88
  89         /* go straight. total distance is less than 5 meters */
  90         strat_set_speed(1000, 1000);
  91         trajectory_d_rel(&mainboard.traj, BIG_DIST);
  92
  93         /* when y > 50, break */
  94         err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD);
  95         if (TRAJ_SUCCESS(err)) /* we should not reach end */
  96                 ERROUT(END_ERROR);
  97         else if (err)
  98                 ERROUT(err);
  99
 100         /* turn to 90° abs while going forward */
 101         DEBUG(E_USER_STRAT, "turn now");
 102         strat_set_speed(1000, 350);
 103         trajectory_only_a_abs(&mainboard.traj, COLOR_A(90));
 104
 105         /* when y > 100, check the presence of column 4 */
 106         err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD);
 107         if (TRAJ_SUCCESS(err)) /* we should not reach end */
 108                 ERROUT(END_ERROR);
 109         else if (err)
 110                 ERROUT(err);
 111         if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
 112                 col4_present = 1;
 113         if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
 114                 col4_present = 1;
 115
 116         /* when y > 120, check the presence of column 1 */
 117         err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD);
 118         if (TRAJ_SUCCESS(err)) /* we should not reach end */
 119                 ERROUT(END_ERROR);
 120         else if (err)
 121                 ERROUT(err);
 122         if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
 123                 col1_present = 1;
 124         if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
 125                 col1_present = 1;
 126
 127         /* when y > 130, break */
 128         err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD);
 129         if (TRAJ_SUCCESS(err)) /* we should not reach end */
 130                 ERROUT(END_ERROR);
 131         else if (err)
 132                 ERROUT(err);
 133
 134         strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE;
 135
 136         DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present);
 137         DEBUG(E_USER_STRAT, "have %d cols", get_column_count());
 138
 139         if (configuration == 0) {
 140                 if (get_column_count() > 2) {
 141                         configuration = 1;
 142                         if (col4_present || col1_present) {
 143                                 strat_infos.s_cols.flags |=
 144                                         STATIC_COL_LINE2_DONE;
 145                         }
 146                         else {
 147                                 strat_infos.s_cols.flags |=
 148                                         STATIC_COL_LINE1_DONE;
 149                         }
 150                 }
 151
 152                 /* only 2 colums on the first line */
 153                 else {
 154                         /* all other colums are on line 1 */
 155                         if (col4_present && col1_present) {
 156                                 configuration = 2;
 157                                 strat_infos.s_cols.flags |=
 158                                         STATIC_COL_LINE2_DONE;
 159                         }
 160
 161                         /* only 2 columns on line 1, so there are also
 162                          * 2 on line 2 */
 163                         else if (col4_present || col1_present) {
 164                                 configuration = 4;
 165                                 strat_infos.s_cols.flags |=
 166                                         STATIC_COL_LINE2_DONE;
 167                         }
 168
 169                         /* all other columns are on line 2 */
 170                         else {
 171                                 configuration = 3;
 172                                 strat_infos.s_cols.flags |=
 173                                         STATIC_COL_LINE1_DONE;
 174                         }
 175                 }
 176         }
 177
 178         strat_infos.s_cols.configuration = configuration;
 179         DEBUG(E_USER_STRAT, "use configuration %d", configuration);
 180
 181         if (configuration == 1) {
 182                 /* we already got 4 columns, go to the disc directly */
 183
 184                 strat_set_speed(1500, 900);
 185                 trajectory_only_a_abs(&mainboard.traj, COLOR_A(0));
 186                 err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD);
 187
 188                 if (TRAJ_SUCCESS(err)) /* we should not reach end */
 189                         ERROUT(END_ERROR);
 190                 else if (err)
 191                         ERROUT(err);
 192         }
 193         else if (configuration == 2 /* go from line 0 to line 1 */) {
 194                 strat_set_speed(800, 1000);
 195                 /* relative is needed here */
 196                 trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
 197                 err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD);
 198                 if (TRAJ_SUCCESS(err)) /* we should not reach end */
 199                         ERROUT(END_ERROR);
 200                 else if (err)
 201                         ERROUT(err);
 202                 strat_set_speed(1000, 600);
 203                 err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100),
 204                                             TRAJ_FLAGS_STD);
 205                 if (TRAJ_SUCCESS(err)) /* we should not reach end */
 206                         ERROUT(END_ERROR);
 207                 else if (err)
 208                         ERROUT(err);
 209         }
 210         else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns
 211                     on line 2*/) {
 212                 strat_set_speed(1000, 600);
 213                 /* relative is needed here */
 214                 trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
 215                 err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD);
 216                 if (TRAJ_SUCCESS(err)) /* we should not reach end */
 217                         ERROUT(END_ERROR);
 218                 else if (err)
 219                         ERROUT(err);
 220         }
 221         else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns
 222                       on line 2 */) {
 223                 strat_set_speed(1000, 600);
 224                 /* relative is needed here */
 225                 trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
 226                 err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD);
 227                 if (TRAJ_SUCCESS(err)) /* we should not reach end */
 228                         ERROUT(END_ERROR);
 229                 else if (err)
 230                         ERROUT(err);
 231         }
 232         else {
 233                 trajectory_stop(&mainboard.traj);
 234         }
 235
 236         ERROUT(END_TRAJ);
 237
 238  end:
 239         strat_set_speed(old_spdd, old_spda);
 240         return err;
 241 }
 242
 243 /*
 244  * get last 2 columns
 245  * must be called after the first temple building
 246  */
 247 uint8_t strat_static_columns_pass2(void)
 248 {
 249         uint16_t old_spdd, old_spda;
 250         uint8_t side, err, next_mode;
 251
 252         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 253
 254         strat_get_speed(&old_spdd, &old_spda);
 255
 256         if (get_color() == I2C_COLOR_RED)
 257                 side = I2C_RIGHT_SIDE;
 258         else
 259                 side = I2C_LEFT_SIDE;
 260
 261         if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
 262                 next_mode = I2C_MECHBOARD_MODE_STORE;
 263         else
 264                 next_mode = I2C_MECHBOARD_MODE_HARVEST;
 265
 266         switch (strat_infos.s_cols.configuration) {
 267
 268         /* configuration 1: 4 cols on line 0 */
 269         case 1:
 270                 if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) {
 271                         /* go on line 2 */
 272
 273                         strat_set_speed(2000, 700);
 274                         trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35));
 275                         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 276                         if (!TRAJ_SUCCESS(err))
 277                                 ERROUT(err);
 278
 279                         i2c_mechboard_mode_prepare_pickup_next(side,
 280                                                                next_mode);
 281
 282                         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
 283                         trajectory_goto_forward_xy_abs(&mainboard.traj,
 284                                                        LINE2_X,
 285                                                        COLOR_Y(400));
 286                         err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2,
 287                                                     TRAJ_FLAGS_NO_NEAR);
 288                         if (!TRAJ_SUCCESS(err))
 289                                 ERROUT(err);
 290                 }
 291                 else {
 292                         /* go on line 1 */
 293                         strat_set_speed(2000, 700);
 294                         trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55));
 295                         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 296                         if (!TRAJ_SUCCESS(err))
 297                                 ERROUT(err);
 298
 299                         i2c_mechboard_mode_prepare_pickup_next(side,
 300                                                                next_mode);
 301
 302                         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
 303
 304                         err = goto_and_avoid_forward(LINE1_X,
 305                                                      COLOR_Y(400),
 306                                                      TRAJ_FLAGS_NO_NEAR,
 307                                                      TRAJ_FLAGS_NO_NEAR);
 308                         if (!TRAJ_SUCCESS(err))
 309                                 ERROUT(err);
 310                 }
 311
 312                 ERROUT(END_TRAJ);
 313                 break;
 314
 315         /* configuration 2: 2 cols on line 0,
 316            all other colums are on line 1 */
 317         case 2:
 318                 /* go on line 1 */
 319                 strat_set_speed(2000, 700);
 320                 trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20));
 321                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 322                 if (!TRAJ_SUCCESS(err))
 323                         ERROUT(err);
 324
 325                 i2c_mechboard_mode_prepare_pickup_next(side,
 326                                                        next_mode);
 327
 328                 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
 329
 330                 err = goto_and_avoid_forward(COL10_X, COLOR_Y(400),
 331                                              TRAJ_FLAGS_NO_NEAR,
 332                                              TRAJ_FLAGS_NO_NEAR);
 333                 if (!TRAJ_SUCCESS(err))
 334                         ERROUT(err);
 335
 336                 ERROUT(END_TRAJ);
 337                 break;
 338
 339         /* configuration 3: 2 cols on line 0,
 340            all other colums are on line 2 */
 341         case 3:
 342                 /* go on line 2 */
 343                 strat_set_speed(2000, 700);
 344                 trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30));
 345                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 346                 if (!TRAJ_SUCCESS(err))
 347                         ERROUT(err);
 348
 349                 i2c_mechboard_mode_prepare_pickup_next(side,
 350                                                        next_mode);
 351
 352                 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
 353
 354                 trajectory_goto_forward_xy_abs(&mainboard.traj,
 355                                                LINE2_X,
 356                                                COLOR_Y(400));
 357                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 358                 if (!TRAJ_SUCCESS(err))
 359                         ERROUT(err);
 360
 361                 ERROUT(END_TRAJ);
 362                 break;
 363
 364         /* configuration 4: 2 cols on line 0,
 365            2 on line 1, 2 on line 2 */
 366         case 4:
 367                 /* go on line 1 */
 368                 strat_set_speed(600, 2000);
 369                 trajectory_d_a_rel(&mainboard.traj, -BIG_DIST,
 370                                    COLOR_A(-135));
 371                 err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900),
 372                                             TRAJ_FLAGS_STD);
 373                 if (TRAJ_SUCCESS(err)) /* we should not reach end */
 374                         ERROUT(END_ERROR);
 375                 else if (err)
 376                         ERROUT(err);
 377
 378                 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 379                 i2c_mechboard_mode_prepare_pickup_next(side,
 380                                                        next_mode);
 381
 382                 strat_set_speed(2000, 2000);
 383                 trajectory_d_rel(&mainboard.traj, -BIG_DIST);
 384                 err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1100),
 385                                             TRAJ_FLAGS_STD);
 386                 if (TRAJ_SUCCESS(err)) /* we should not reach end */
 387                         ERROUT(END_ERROR);
 388                 else if (err)
 389                         ERROUT(err);
 390
 391                 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 392                 trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40));
 393                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 394                 if (!TRAJ_SUCCESS(err))
 395                         ERROUT(err);
 396
 397                 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 398                 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
 399                 err = goto_and_avoid_forward(LINE1_X,
 400                                              COLOR_Y(400),
 401                                              TRAJ_FLAGS_NO_NEAR,
 402                                              TRAJ_FLAGS_NO_NEAR);
 403                 ERROUT(END_TRAJ);
 404                 break;
 405
 406         default:
 407                 break;
 408         }
 409
 410         /* should not reach this point */
 411         ERROUT(END_ERROR);
 412
 413  end:
 414         strat_set_speed(old_spdd, old_spda);
 415         return err;
 416 }