projects/microb2010/mechboard/arm_highlevel.c

   1 /*
   2  *  Copyright Droids Corporation (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: arm_highlevel.c,v 1.4 2009-11-08 17:25:00 zer0 Exp $
  19  *
  20  */
  21
  22 #include <aversive.h>
  23 #include <aversive/wait.h>
  24 #include <aversive/pgmspace.h>
  25 #include <aversive/error.h>
  26
  27 #include <ax12.h>
  28 #include <uart.h>
  29 #include <spi.h>
  30 #include <encoders_spi.h>
  31 #include <pwm_ng.h>
  32 #include <timer.h>
  33 #include <scheduler.h>
  34 #include <time.h>
  35
  36 #include <pid.h>
  37 #include <quadramp.h>
  38 #include <control_system_manager.h>
  39 #include <blocking_detection_manager.h>
  40
  41 #include <rdline.h>
  42
  43 #include "../common/i2c_commands.h"
  44 #include "main.h"
  45 #include "arm_xy.h"
  46 #include "arm_highlevel.h"
  47
  48 #define WRIST_ANGLE_PUMP1 -2  /* in degree */
  49 #define WRIST_ANGLE_PUMP2 103 /* in degree */
  50
  51 struct arm *arm_num2ptr(uint8_t arm_num)
  52 {
  53         switch (arm_num) {
  54         case ARM_LEFT_NUM:
  55                 return &left_arm;
  56         case ARM_RIGHT_NUM:
  57                 return &right_arm;
  58         default:
  59                 return NULL;
  60         }
  61 }
  62
  63 #define ARM_MAX_H 110
  64 #define ARM_STRAIGHT_D 254
  65 #define ARM_STRAIGHT_H 0
  66
  67 void arm_goto_straight(uint8_t arm_num, uint8_t pump_num)
  68 {
  69         struct arm *arm = arm_num2ptr(arm_num);
  70         int16_t angle = pump_num2angle(pump_num);
  71         arm_do_xy(arm, ARM_STRAIGHT_D, ARM_STRAIGHT_H, angle);
  72 }
  73
  74 /* position to get a column */
  75 #define ARM_GET_D 60
  76 #define ARM_GET_H -140
  77
  78 void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num)
  79 {
  80         struct arm *arm = arm_num2ptr(arm_num);
  81         int16_t angle = pump_num2angle(pump_num);
  82         arm_do_xy(arm, ARM_GET_D, ARM_GET_H, angle);
  83 }
  84
  85 /* position to get a column */
  86 #define ARM_PREPARE_D 62
  87 #define ARM_PREPARE_H -133
  88
  89 void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num)
  90 {
  91         struct arm *arm = arm_num2ptr(arm_num);
  92         int16_t angle = pump_num2angle(pump_num);
  93         arm_do_xy(arm, ARM_PREPARE_D, ARM_PREPARE_H, angle);
  94 }
  95
  96 #define ARM_INTERMEDIATE_D (65)
  97 #define ARM_INTERMEDIATE_H (-115)
  98
  99 void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num)
 100 {
 101         struct arm *arm = arm_num2ptr(arm_num);
 102         int16_t angle = pump_num2angle(pump_num);
 103         arm_do_xy(arm, ARM_INTERMEDIATE_D, ARM_INTERMEDIATE_H, angle);
 104 }
 105
 106 /* used in prepare pickup */
 107 #define ARM_INTERMEDIATE_FRONT_D (90)
 108 #define ARM_INTERMEDIATE_FRONT_H (-105)
 109
 110 void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num)
 111 {
 112         struct arm *arm = arm_num2ptr(arm_num);
 113         int16_t angle = pump_num2angle(pump_num);
 114         arm_do_xy(arm, ARM_INTERMEDIATE_FRONT_D,
 115                   ARM_INTERMEDIATE_FRONT_H, angle);
 116 }
 117
 118 /* ****** */
 119
 120 #define ARM_PREPARE_EJECT_D (70)
 121 #define ARM_PREPARE_EJECT_H (-50)
 122
 123 void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num)
 124 {
 125         struct arm *arm = arm_num2ptr(arm_num);
 126         int16_t angle = pump_num2angle(pump_num);
 127         arm_do_xy(arm, ARM_PREPARE_EJECT_D, ARM_PREPARE_EJECT_H, angle);
 128 }
 129
 130 #define ARM_EJECT_D (200)
 131 #define ARM_EJECT_H (30)
 132
 133 void arm_goto_eject(uint8_t arm_num, uint8_t pump_num)
 134 {
 135         struct arm *arm = arm_num2ptr(arm_num);
 136         int16_t angle = pump_num2angle(pump_num);
 137         arm_do_xy(arm, ARM_EJECT_D, ARM_EJECT_H, angle);
 138 }
 139
 140 /* ****** */
 141
 142 #define ARM_PREPARE_GET_LINTEL_INSIDE1_D 90
 143 #define ARM_PREPARE_GET_LINTEL_INSIDE1_H -75
 144 #define ARM_PREPARE_GET_LINTEL_INSIDE1_A -30
 145 void arm_goto_prepare_get_lintel_inside1(void)
 146 {
 147         arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D,
 148                   ARM_PREPARE_GET_LINTEL_INSIDE1_H,
 149                   ARM_PREPARE_GET_LINTEL_INSIDE1_A);
 150         arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D,
 151                   ARM_PREPARE_GET_LINTEL_INSIDE1_H,
 152                   ARM_PREPARE_GET_LINTEL_INSIDE1_A);
 153 }
 154
 155 #define ARM_PREPARE_GET_LINTEL_INSIDE2_D 30
 156 #define ARM_PREPARE_GET_LINTEL_INSIDE2_H -75
 157 #define ARM_PREPARE_GET_LINTEL_INSIDE2_A -30
 158 void arm_goto_prepare_get_lintel_inside2(uint8_t lintel_count)
 159 {
 160         uint16_t d;
 161         d = ARM_PREPARE_GET_LINTEL_INSIDE2_D;
 162         if (lintel_count == 2)
 163                 d += 34;
 164         arm_do_xy(&left_arm, d,
 165                   ARM_PREPARE_GET_LINTEL_INSIDE2_H,
 166                   ARM_PREPARE_GET_LINTEL_INSIDE2_A);
 167         arm_do_xy(&right_arm, d,
 168                   ARM_PREPARE_GET_LINTEL_INSIDE2_H,
 169                   ARM_PREPARE_GET_LINTEL_INSIDE2_A);
 170 }
 171
 172 #define ARM_GET_LINTEL_INSIDE_D 10
 173 #define ARM_GET_LINTEL_INSIDE_H -75
 174 #define ARM_GET_LINTEL_INSIDE_A -30
 175 void arm_goto_get_lintel_inside(uint8_t lintel_count)
 176 {
 177         uint16_t d;
 178         d = ARM_GET_LINTEL_INSIDE_D;
 179         if (lintel_count == 2)
 180                 d += 34;
 181         arm_do_xy(&left_arm, d,
 182                   ARM_GET_LINTEL_INSIDE_H,
 183                   ARM_GET_LINTEL_INSIDE_A);
 184         arm_do_xy(&right_arm, d,
 185                   ARM_GET_LINTEL_INSIDE_H,
 186                   ARM_GET_LINTEL_INSIDE_A);
 187 }
 188
 189 #define ARM_PREPARE_BUILD_LINTEL1_D 30
 190 #define ARM_PREPARE_BUILD_LINTEL1_H -50
 191 #define ARM_PREPARE_BUILD_LINTEL1_A -30
 192 void arm_goto_prepare_build_lintel1(void)
 193 {
 194         arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL1_D,
 195                   ARM_PREPARE_BUILD_LINTEL1_H,
 196                   ARM_PREPARE_BUILD_LINTEL1_A);
 197         arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL1_D,
 198                   ARM_PREPARE_BUILD_LINTEL1_H,
 199                   ARM_PREPARE_BUILD_LINTEL1_A);
 200 }
 201
 202 #define ARM_PREPARE_BUILD_LINTEL2_D 80
 203 #define ARM_PREPARE_BUILD_LINTEL2_H -110
 204 #define ARM_PREPARE_BUILD_LINTEL2_A 60
 205 void arm_goto_prepare_build_lintel2(uint8_t level)
 206 {
 207         int16_t h;
 208         if (level < 3)
 209                 level = 3;
 210         h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL2_H;
 211         if (h > ARM_MAX_H)
 212                 h = ARM_MAX_H;
 213         arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL2_D,
 214                   h, ARM_PREPARE_BUILD_LINTEL2_A);
 215         arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL2_D,
 216                   h, ARM_PREPARE_BUILD_LINTEL2_A);
 217 }
 218
 219 #define ARM_PREPARE_BUILD_LINTEL3_D 205
 220 #define ARM_PREPARE_BUILD_LINTEL3_H -100
 221 #define ARM_PREPARE_BUILD_LINTEL3_A 50
 222 void arm_goto_prepare_build_lintel3(uint8_t level)
 223 {
 224         int16_t h;
 225         if (level < 2)
 226                 level = 2;
 227         h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL3_H;
 228         if (h > ARM_MAX_H)
 229                 h = ARM_MAX_H;
 230         arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL3_D,
 231                   h, ARM_PREPARE_BUILD_LINTEL3_A);
 232         arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL3_D,
 233                   h, ARM_PREPARE_BUILD_LINTEL3_A);
 234 }
 235
 236 #define ARM_BUILD_LINTEL_D 205
 237 #define ARM_BUILD_LINTEL_H -128
 238 #define ARM_BUILD_LINTEL_A 50
 239 void arm_goto_build_lintel(uint8_t level)
 240 {
 241         int16_t h;
 242         h = (int16_t)level * 30 + ARM_BUILD_LINTEL_H;
 243         if (h > ARM_MAX_H)
 244                 h = ARM_MAX_H;
 245         arm_do_xy(&left_arm, ARM_BUILD_LINTEL_D,
 246                   h, ARM_BUILD_LINTEL_A);
 247         arm_do_xy(&right_arm, ARM_BUILD_LINTEL_D,
 248                   h, ARM_BUILD_LINTEL_A);
 249 }
 250
 251 /* ****** */
 252
 253 #define ARM_PREPARE_GET_LINTEL_DISP_D 190
 254 #define ARM_PREPARE_GET_LINTEL_DISP_H -40
 255 #define ARM_PREPARE_GET_LINTEL_DISP_A 50
 256 void arm_goto_prepare_get_lintel_disp(void)
 257 {
 258         arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_DISP_D,
 259                   ARM_PREPARE_GET_LINTEL_DISP_H,
 260                   ARM_PREPARE_GET_LINTEL_DISP_A);
 261         arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_DISP_D,
 262                   ARM_PREPARE_GET_LINTEL_DISP_H,
 263                   ARM_PREPARE_GET_LINTEL_DISP_A);
 264 }
 265
 266 #define ARM_GET_LINTEL_DISP_D 190
 267 #define ARM_GET_LINTEL_DISP_H -70
 268 #define ARM_GET_LINTEL_DISP_A 40
 269 void arm_goto_get_lintel_disp(void)
 270 {
 271         arm_do_xy(&left_arm, ARM_GET_LINTEL_DISP_D,
 272                   ARM_GET_LINTEL_DISP_H,
 273                   ARM_GET_LINTEL_DISP_A);
 274         arm_do_xy(&right_arm, ARM_GET_LINTEL_DISP_D,
 275                   ARM_GET_LINTEL_DISP_H,
 276                   ARM_GET_LINTEL_DISP_A);
 277 }
 278
 279 #define ARM_PREPARE_PUT_LINTEL_DISP_D 130
 280 #define ARM_PREPARE_PUT_LINTEL_DISP_H 0
 281 #define ARM_PREPARE_PUT_LINTEL_DISP_A 0
 282 void arm_goto_prepare_put_lintel(void)
 283 {
 284         arm_do_xy(&left_arm, ARM_PREPARE_PUT_LINTEL_DISP_D,
 285                   ARM_PREPARE_PUT_LINTEL_DISP_H,
 286                   ARM_PREPARE_PUT_LINTEL_DISP_A);
 287         arm_do_xy(&right_arm, ARM_PREPARE_PUT_LINTEL_DISP_D,
 288                   ARM_PREPARE_PUT_LINTEL_DISP_H,
 289                   ARM_PREPARE_PUT_LINTEL_DISP_A);
 290 }
 291
 292 #define ARM_PUT_LINTEL_DISP_D 30
 293 #define ARM_PUT_LINTEL_DISP_H -60
 294 #define ARM_PUT_LINTEL_DISP_A -30
 295 void arm_goto_put_lintel(uint8_t lintel_count)
 296 {
 297         arm_do_xy(&left_arm,
 298                   ARM_PUT_LINTEL_DISP_D + lintel_count * 30,
 299                   ARM_PUT_LINTEL_DISP_H,
 300                   ARM_PUT_LINTEL_DISP_A);
 301         arm_do_xy(&right_arm,
 302                   ARM_PUT_LINTEL_DISP_D + lintel_count * 30,
 303                   ARM_PUT_LINTEL_DISP_H,
 304                   ARM_PUT_LINTEL_DISP_A);
 305 }
 306
 307 /* ****** */
 308
 309
 310 #define ARM_LOADED_D 100
 311 #define ARM_LOADED_H 0
 312
 313 void arm_goto_loaded(uint8_t arm_num)
 314 {
 315         struct arm *arm = arm_num2ptr(arm_num);
 316         arm_do_xy(arm, ARM_LOADED_D, ARM_LOADED_H, WRIST_ANGLE_PUMP2);
 317 }
 318
 319
 320 /* for columns */
 321 #define ARM_PREPARE_BUILD_INSIDE_D  90
 322 #define ARM_PREPARE_BUILD_INSIDE_H -45
 323
 324 void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level)
 325 {
 326         struct arm *arm = arm_num2ptr(arm_num);
 327         int16_t angle = pump_num2angle(pump_num);
 328         int16_t h;
 329         if (level < 2)
 330                 level = 2;
 331         h = (int16_t)level * 30 + ARM_PREPARE_BUILD_INSIDE_H;
 332         if (h > ARM_MAX_H)
 333                 h = ARM_MAX_H;
 334         arm_do_xy(arm, ARM_PREPARE_BUILD_INSIDE_D, h, angle);
 335 }
 336
 337 #define ARM_PREPARE_AUTOBUILD_INSIDE_D  90
 338 #define ARM_PREPARE_AUTOBUILD_INSIDE_H -70
 339
 340 void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level)
 341 {
 342         struct arm *arm = arm_num2ptr(arm_num);
 343         int16_t angle = pump_num2angle(pump_num);
 344         int16_t h;
 345         if (level < 2)
 346                 level = 2;
 347         h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_INSIDE_H;
 348         if (h > ARM_MAX_H)
 349                 h = ARM_MAX_H;
 350         arm_do_xy(arm, ARM_PREPARE_AUTOBUILD_INSIDE_D, h, angle);
 351 }
 352
 353 #define ARM_PREPARE_AUTOBUILD_OUTSIDE_D  210 /* not used, see dist below */
 354 #define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1 (-110)
 355 #define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2 (-90)
 356
 357 void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num,
 358                                         uint8_t level, uint8_t dist)
 359 {
 360         struct arm *arm = arm_num2ptr(arm_num);
 361         int16_t angle = pump_num2angle(pump_num);
 362         int16_t h;
 363         if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM)
 364                 h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1;
 365         else
 366                 h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2;
 367         if (h > ARM_MAX_H)
 368                 h = ARM_MAX_H;
 369         arm_do_xy(arm, dist, h, angle);
 370 }
 371
 372 #define ARM_AUTOBUILD_D_P1  208 /* not used, see dist below */
 373 #define ARM_AUTOBUILD_D_P2  210 /* not used, see dist below */
 374 #define ARM_AUTOBUILD_D_P1_OFFSET  (-2)
 375 #define ARM_AUTOBUILD_D_P2_OFFSET  (0)
 376 #define ARM_AUTOBUILD_H_P1 (-133)
 377 #define ARM_AUTOBUILD_H_P2 (-130)
 378
 379 void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num,
 380                         uint8_t level, uint8_t dist)
 381 {
 382         struct arm *arm = arm_num2ptr(arm_num);
 383         int16_t angle = pump_num2angle(pump_num);
 384         int16_t h;
 385         if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) {
 386                 h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P1;
 387                 if (h > ARM_MAX_H)
 388                         h = ARM_MAX_H;
 389                 arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P1_OFFSET, h, angle);
 390         }
 391         else {
 392                 h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P2;
 393                 if (h > ARM_MAX_H)
 394                         h = ARM_MAX_H;
 395                 arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P2_OFFSET, h, angle);
 396         }
 397 }
 398
 399 #define ARM_PUSH_TEMPLE_D 170
 400 #define ARM_PUSH_TEMPLE_H -165
 401
 402 void arm_goto_push_temple(uint8_t arm_num, uint8_t level)
 403 {
 404         struct arm *arm = arm_num2ptr(arm_num);
 405         int16_t h = ARM_PUSH_TEMPLE_H;
 406
 407         /* level can be 0 or 1 */
 408         if (level)
 409                 h += 30;
 410         arm_do_xy(arm, ARM_PUSH_TEMPLE_D, h, WRIST_ANGLE_PUMP1);
 411 }
 412
 413 #define ARM_PREPARE_PUSH_TEMPLE_D 120
 414 #define ARM_PREPARE_PUSH_TEMPLE_H -60
 415
 416 void arm_goto_prepare_push_temple(uint8_t arm_num)
 417 {
 418         struct arm *arm = arm_num2ptr(arm_num);
 419         arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_D,
 420                   ARM_PREPARE_PUSH_TEMPLE_H, WRIST_ANGLE_PUMP1);
 421 }
 422
 423 #define ARM_PUSH_TEMPLE_DISC_D1 215
 424 #define ARM_PUSH_TEMPLE_DISC_H1 -100
 425 #define ARM_PUSH_TEMPLE_DISC_D2 190
 426 #define ARM_PUSH_TEMPLE_DISC_H2 -65
 427
 428 void arm_goto_push_temple_disc(uint8_t arm_num)
 429 {
 430         struct arm *arm = arm_num2ptr(arm_num);
 431         int8_t pump_num;
 432
 433         pump_num = arm_get_busy_pump(arm_num);
 434         if (pump_num == -1)
 435                 arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D1,
 436                           ARM_PUSH_TEMPLE_DISC_H1, WRIST_ANGLE_PUMP1);
 437         else
 438                 arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D2,
 439                           ARM_PUSH_TEMPLE_DISC_H2, WRIST_ANGLE_PUMP1);
 440 }
 441
 442 #define ARM_PREPARE_PUSH_TEMPLE_DISC_D 100
 443 #define ARM_PREPARE_PUSH_TEMPLE_DISC_H -80
 444
 445 void arm_goto_prepare_push_temple_disc(uint8_t arm_num)
 446 {
 447         struct arm *arm = arm_num2ptr(arm_num);
 448         arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_DISC_D,
 449                   ARM_PREPARE_PUSH_TEMPLE_DISC_H, WRIST_ANGLE_PUMP1);
 450 }
 451
 452 void arm_prepare_free_pumps(void)
 453 {
 454         int8_t pump_num;
 455
 456         pump_num = arm_get_free_pump(ARM_LEFT_NUM);
 457         if (pump_num != -1)
 458                 arm_goto_prepare_get(ARM_LEFT_NUM, pump_num);
 459         pump_num = arm_get_free_pump(ARM_RIGHT_NUM);
 460         if (pump_num != -1)
 461                 arm_goto_prepare_get(ARM_RIGHT_NUM, pump_num);
 462 }
 463
 464
 465 /* return the id of a free pump on this arm */
 466 int8_t arm_get_free_pump(uint8_t arm_num)
 467 {
 468         switch (arm_num) {
 469         case ARM_LEFT_NUM:
 470                 if (pump_is_free(PUMP_LEFT1_NUM) &&
 471                     pump_is_free(PUMP_LEFT2_NUM))
 472                         return PUMP_LEFT1_NUM;
 473                 else if (pump_is_free(PUMP_LEFT2_NUM))
 474                         return PUMP_LEFT2_NUM;
 475                 return -1;
 476         case ARM_RIGHT_NUM:
 477                 if (pump_is_free(PUMP_RIGHT1_NUM) &&
 478                     pump_is_free(PUMP_RIGHT2_NUM))
 479                         return PUMP_RIGHT1_NUM;
 480                 else if (pump_is_free(PUMP_RIGHT2_NUM))
 481                         return PUMP_RIGHT2_NUM;
 482                 return -1;
 483         default:
 484                 return -1;
 485         }
 486 }
 487
 488 /* return the id of a busy pump on this arm */
 489 int8_t arm_get_busy_pump(uint8_t arm_num)
 490 {
 491         switch (arm_num) {
 492         case ARM_LEFT_NUM:
 493                 if (pump_is_busy(PUMP_LEFT2_NUM))
 494                         return PUMP_LEFT2_NUM;
 495                 else if (pump_is_busy(PUMP_LEFT1_NUM))
 496                         return PUMP_LEFT1_NUM;
 497                 return -1;
 498         case ARM_RIGHT_NUM:
 499                 if (pump_is_busy(PUMP_RIGHT2_NUM))
 500                         return PUMP_RIGHT2_NUM;
 501                 else if (pump_is_busy(PUMP_RIGHT1_NUM))
 502                         return PUMP_RIGHT1_NUM;
 503                 return -1;
 504         default:
 505                 return -1;
 506         }
 507 }
 508
 509 uint8_t arm_wait_both(uint8_t mask)
 510 {
 511         uint8_t ret;
 512         ret = arm_wait_traj_end(&left_arm, mask);
 513         if (ret != ARM_TRAJ_END && ret != ARM_TRAJ_NEAR)
 514                 return ret;
 515         return arm_wait_traj_end(&right_arm, mask);
 516 }
 517
 518 uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask)
 519 {
 520         if (left && right)
 521                 return arm_wait_both(mask);
 522         if (left)
 523                 return arm_wait_traj_end(&left_arm, mask);
 524         if (right)
 525                 return arm_wait_traj_end(&right_arm, mask);
 526         return ARM_TRAJ_END;
 527 }
 528
 529 /*********************/
 530
 531 int16_t *pump_num2ptr(uint8_t pump_num)
 532 {
 533         switch (pump_num) {
 534         case PUMP_LEFT1_NUM:
 535                 return &mechboard.pump_left1;
 536         case PUMP_RIGHT1_NUM:
 537                 return &mechboard.pump_right1;
 538         case PUMP_LEFT2_NUM:
 539                 return &mechboard.pump_left2;
 540         case PUMP_RIGHT2_NUM:
 541                 return &mechboard.pump_right2;
 542         default:
 543                 return NULL;
 544         }
 545 }
 546
 547 void pump_set(uint8_t pump_num, int16_t val)
 548 {
 549         int16_t *pump_ptr = pump_num2ptr(pump_num);
 550
 551         *pump_ptr = val;
 552
 553         switch (pump_num) {
 554         case PUMP_RIGHT1_NUM:
 555                 pwm_ng_set(RIGHT_PUMP1_PWM, val);
 556                 break;
 557         case PUMP_RIGHT2_NUM:
 558                 pwm_ng_set(RIGHT_PUMP2_PWM, val);
 559                 break;
 560
 561         /* no pwm, it's remote */
 562         case PUMP_LEFT1_NUM:
 563         case PUMP_LEFT2_NUM:
 564         default:
 565                 break;
 566         }
 567 }
 568
 569 int16_t pump_num2angle(uint8_t pump_num)
 570 {
 571         switch (pump_num) {
 572         case PUMP_LEFT1_NUM:
 573         case PUMP_RIGHT1_NUM:
 574                 return WRIST_ANGLE_PUMP1;
 575         case PUMP_LEFT2_NUM:
 576         case PUMP_RIGHT2_NUM:
 577                 return WRIST_ANGLE_PUMP2;
 578         default:
 579                 return 0;
 580         }
 581 }
 582
 583 void pump_mark_busy(uint8_t pump_num)
 584 {
 585         switch (pump_num) {
 586         case PUMP_LEFT1_NUM:
 587                 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L1;
 588                 break;
 589         case PUMP_RIGHT1_NUM:
 590                 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R1;
 591                 break;
 592         case PUMP_LEFT2_NUM:
 593                 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L2;
 594                 break;
 595         case PUMP_RIGHT2_NUM:
 596                 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R2;
 597                 break;
 598         default:
 599                 break;
 600         }
 601
 602 }
 603
 604 void pump_mark_free(uint8_t pump_num)
 605 {
 606         switch (pump_num) {
 607         case PUMP_LEFT1_NUM:
 608                 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L1);
 609                 break;
 610         case PUMP_RIGHT1_NUM:
 611                 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R1);
 612                 break;
 613         case PUMP_LEFT2_NUM:
 614                 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L2);
 615                 break;
 616         case PUMP_RIGHT2_NUM:
 617                 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R2);
 618                 break;
 619         default:
 620                 break;
 621         }
 622
 623 }
 624
 625 uint8_t pump_is_free(uint8_t pump_num)
 626 {
 627         switch (pump_num) {
 628         case PUMP_LEFT1_NUM:
 629                 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1);
 630         case PUMP_RIGHT1_NUM:
 631                 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1);
 632         case PUMP_LEFT2_NUM:
 633                 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2);
 634         case PUMP_RIGHT2_NUM:
 635                 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2);
 636         default:
 637                 return 0;
 638         }
 639 }
 640
 641 uint8_t pump_is_busy(uint8_t pump_num)
 642 {
 643         return !pump_is_free(pump_num);
 644 }