2 * Copyright Droids Corporation (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: arm_highlevel.c,v 1.4 2009-11-08 17:25:00 zer0 Exp $
23 #include <aversive/wait.h>
24 #include <aversive/pgmspace.h>
25 #include <aversive/error.h>
30 #include <encoders_spi.h>
33 #include <scheduler.h>
38 #include <control_system_manager.h>
39 #include <blocking_detection_manager.h>
43 #include "../common/i2c_commands.h"
46 #include "arm_highlevel.h"
48 #define WRIST_ANGLE_PUMP1 -2 /* in degree */
49 #define WRIST_ANGLE_PUMP2 103 /* in degree */
51 struct arm *arm_num2ptr(uint8_t arm_num)
64 #define ARM_STRAIGHT_D 254
65 #define ARM_STRAIGHT_H 0
67 void arm_goto_straight(uint8_t arm_num, uint8_t pump_num)
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);
74 /* position to get a column */
76 #define ARM_GET_H -140
78 void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num)
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);
85 /* position to get a column */
86 #define ARM_PREPARE_D 62
87 #define ARM_PREPARE_H -133
89 void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num)
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);
96 #define ARM_INTERMEDIATE_D (65)
97 #define ARM_INTERMEDIATE_H (-115)
99 void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num)
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);
106 /* used in prepare pickup */
107 #define ARM_INTERMEDIATE_FRONT_D (90)
108 #define ARM_INTERMEDIATE_FRONT_H (-105)
110 void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num)
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);
120 #define ARM_PREPARE_EJECT_D (70)
121 #define ARM_PREPARE_EJECT_H (-50)
123 void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num)
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);
130 #define ARM_EJECT_D (200)
131 #define ARM_EJECT_H (30)
133 void arm_goto_eject(uint8_t arm_num, uint8_t pump_num)
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);
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)
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);
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)
161 d = ARM_PREPARE_GET_LINTEL_INSIDE2_D;
162 if (lintel_count == 2)
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);
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)
178 d = ARM_GET_LINTEL_INSIDE_D;
179 if (lintel_count == 2)
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);
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)
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);
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)
210 h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL2_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);
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)
227 h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL3_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);
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)
242 h = (int16_t)level * 30 + ARM_BUILD_LINTEL_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);
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)
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);
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)
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);
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)
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);
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)
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);
310 #define ARM_LOADED_D 100
311 #define ARM_LOADED_H 0
313 void arm_goto_loaded(uint8_t arm_num)
315 struct arm *arm = arm_num2ptr(arm_num);
316 arm_do_xy(arm, ARM_LOADED_D, ARM_LOADED_H, WRIST_ANGLE_PUMP2);
321 #define ARM_PREPARE_BUILD_INSIDE_D 90
322 #define ARM_PREPARE_BUILD_INSIDE_H -45
324 void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level)
326 struct arm *arm = arm_num2ptr(arm_num);
327 int16_t angle = pump_num2angle(pump_num);
331 h = (int16_t)level * 30 + ARM_PREPARE_BUILD_INSIDE_H;
334 arm_do_xy(arm, ARM_PREPARE_BUILD_INSIDE_D, h, angle);
337 #define ARM_PREPARE_AUTOBUILD_INSIDE_D 90
338 #define ARM_PREPARE_AUTOBUILD_INSIDE_H -70
340 void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level)
342 struct arm *arm = arm_num2ptr(arm_num);
343 int16_t angle = pump_num2angle(pump_num);
347 h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_INSIDE_H;
350 arm_do_xy(arm, ARM_PREPARE_AUTOBUILD_INSIDE_D, h, angle);
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)
357 void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num,
358 uint8_t level, uint8_t dist)
360 struct arm *arm = arm_num2ptr(arm_num);
361 int16_t angle = pump_num2angle(pump_num);
363 if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM)
364 h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1;
366 h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2;
369 arm_do_xy(arm, dist, h, angle);
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)
379 void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num,
380 uint8_t level, uint8_t dist)
382 struct arm *arm = arm_num2ptr(arm_num);
383 int16_t angle = pump_num2angle(pump_num);
385 if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) {
386 h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P1;
389 arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P1_OFFSET, h, angle);
392 h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P2;
395 arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P2_OFFSET, h, angle);
399 #define ARM_PUSH_TEMPLE_D 170
400 #define ARM_PUSH_TEMPLE_H -165
402 void arm_goto_push_temple(uint8_t arm_num, uint8_t level)
404 struct arm *arm = arm_num2ptr(arm_num);
405 int16_t h = ARM_PUSH_TEMPLE_H;
407 /* level can be 0 or 1 */
410 arm_do_xy(arm, ARM_PUSH_TEMPLE_D, h, WRIST_ANGLE_PUMP1);
413 #define ARM_PREPARE_PUSH_TEMPLE_D 120
414 #define ARM_PREPARE_PUSH_TEMPLE_H -60
416 void arm_goto_prepare_push_temple(uint8_t arm_num)
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);
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
428 void arm_goto_push_temple_disc(uint8_t arm_num)
430 struct arm *arm = arm_num2ptr(arm_num);
433 pump_num = arm_get_busy_pump(arm_num);
435 arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D1,
436 ARM_PUSH_TEMPLE_DISC_H1, WRIST_ANGLE_PUMP1);
438 arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D2,
439 ARM_PUSH_TEMPLE_DISC_H2, WRIST_ANGLE_PUMP1);
442 #define ARM_PREPARE_PUSH_TEMPLE_DISC_D 100
443 #define ARM_PREPARE_PUSH_TEMPLE_DISC_H -80
445 void arm_goto_prepare_push_temple_disc(uint8_t arm_num)
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);
452 void arm_prepare_free_pumps(void)
456 pump_num = arm_get_free_pump(ARM_LEFT_NUM);
458 arm_goto_prepare_get(ARM_LEFT_NUM, pump_num);
459 pump_num = arm_get_free_pump(ARM_RIGHT_NUM);
461 arm_goto_prepare_get(ARM_RIGHT_NUM, pump_num);
465 /* return the id of a free pump on this arm */
466 int8_t arm_get_free_pump(uint8_t arm_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;
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;
488 /* return the id of a busy pump on this arm */
489 int8_t arm_get_busy_pump(uint8_t arm_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;
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;
509 uint8_t arm_wait_both(uint8_t mask)
512 ret = arm_wait_traj_end(&left_arm, mask);
513 if (ret != ARM_TRAJ_END && ret != ARM_TRAJ_NEAR)
515 return arm_wait_traj_end(&right_arm, mask);
518 uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask)
521 return arm_wait_both(mask);
523 return arm_wait_traj_end(&left_arm, mask);
525 return arm_wait_traj_end(&right_arm, mask);
529 /*********************/
531 int16_t *pump_num2ptr(uint8_t pump_num)
535 return &mechboard.pump_left1;
536 case PUMP_RIGHT1_NUM:
537 return &mechboard.pump_right1;
539 return &mechboard.pump_left2;
540 case PUMP_RIGHT2_NUM:
541 return &mechboard.pump_right2;
547 void pump_set(uint8_t pump_num, int16_t val)
549 int16_t *pump_ptr = pump_num2ptr(pump_num);
554 case PUMP_RIGHT1_NUM:
555 pwm_ng_set(RIGHT_PUMP1_PWM, val);
557 case PUMP_RIGHT2_NUM:
558 pwm_ng_set(RIGHT_PUMP2_PWM, val);
561 /* no pwm, it's remote */
569 int16_t pump_num2angle(uint8_t pump_num)
573 case PUMP_RIGHT1_NUM:
574 return WRIST_ANGLE_PUMP1;
576 case PUMP_RIGHT2_NUM:
577 return WRIST_ANGLE_PUMP2;
583 void pump_mark_busy(uint8_t pump_num)
587 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L1;
589 case PUMP_RIGHT1_NUM:
590 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R1;
593 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L2;
595 case PUMP_RIGHT2_NUM:
596 mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R2;
604 void pump_mark_free(uint8_t pump_num)
608 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L1);
610 case PUMP_RIGHT1_NUM:
611 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R1);
614 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L2);
616 case PUMP_RIGHT2_NUM:
617 mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R2);
625 uint8_t pump_is_free(uint8_t pump_num)
629 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1);
630 case PUMP_RIGHT1_NUM:
631 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1);
633 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2);
634 case PUMP_RIGHT2_NUM:
635 return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2);
641 uint8_t pump_is_busy(uint8_t pump_num)
643 return !pump_is_free(pump_num);