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: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
26 #include <aversive/wait.h>
27 #include <aversive/error.h>
32 #include <encoders_spi.h>
35 #include <scheduler.h>
40 #include <control_system_manager.h>
41 #include <blocking_detection_manager.h>
46 #include "../common/i2c_commands.h"
52 #include "arm_highlevel.h"
55 #define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args)
56 #define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args)
57 #define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args)
59 /* shorter aliases for this file */
60 #define MANUAL I2C_MECHBOARD_MODE_MANUAL
61 #define HARVEST I2C_MECHBOARD_MODE_HARVEST
62 #define PREPARE_PICKUP I2C_MECHBOARD_MODE_PREPARE_PICKUP
63 #define PICKUP I2C_MECHBOARD_MODE_PICKUP
64 #define PREPARE_BUILD I2C_MECHBOARD_MODE_PREPARE_BUILD
65 #define AUTOBUILD I2C_MECHBOARD_MODE_AUTOBUILD
66 #define WAIT I2C_MECHBOARD_MODE_WAIT
67 #define INIT I2C_MECHBOARD_MODE_INIT
68 #define PREPARE_GET_LINTEL I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL
69 #define GET_LINTEL I2C_MECHBOARD_MODE_GET_LINTEL
70 #define PUT_LINTEL I2C_MECHBOARD_MODE_PUT_LINTEL
71 #define PREPARE_EJECT I2C_MECHBOARD_MODE_PREPARE_EJECT
72 #define EJECT I2C_MECHBOARD_MODE_EJECT
73 #define CLEAR I2C_MECHBOARD_MODE_CLEAR
74 #define LAZY_HARVEST I2C_MECHBOARD_MODE_LAZY_HARVEST
75 #define LOADED I2C_MECHBOARD_MODE_LOADED
76 #define PREPARE_INSIDE I2C_MECHBOARD_MODE_PREPARE_INSIDE
77 #define STORE I2C_MECHBOARD_MODE_STORE
78 #define LAZY_PICKUP I2C_MECHBOARD_MODE_LAZY_PICKUP
79 #define MANIVELLE I2C_MECHBOARD_MODE_MANIVELLE
80 #define PUSH_TEMPLE I2C_MECHBOARD_MODE_PUSH_TEMPLE
81 #define PUSH_TEMPLE_DISC I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC
82 #define EXIT I2C_MECHBOARD_MODE_EXIT
84 static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode);
86 static struct i2c_cmd_mechboard_set_mode mainboard_command;
87 static struct vt100 local_vt100;
88 static volatile uint8_t prev_state;
89 static uint8_t pickup_side;
90 static volatile uint8_t changed = 0;
92 uint8_t state_debug = 0;
94 void state_dump_sensors(void)
96 uint16_t tmp = sensor_get_all();
97 prog_char *front = PSTR("no_front");
98 prog_char *left = PSTR("no_left");
99 prog_char *right = PSTR("no_right");
101 if (tmp & _BV(S_FRONT))
102 front = PSTR("FRONT");
103 if (tmp & _BV(S_LEFT)) {
104 if (tmp & _BV(S_COL_LEFT))
105 left = PSTR("LEFT(red)");
107 left = PSTR("LEFT(green)");
109 if (tmp & _BV(S_RIGHT)) {
110 if (tmp & _BV(S_COL_RIGHT))
111 right = PSTR("RIGHT(red)");
113 right = PSTR("RIGHT(green)");
116 STMCH_DEBUG("sensors = %S %S %S", front, left, right);
119 /* return 1 if column is there */
120 uint8_t arm_get_sensor(uint8_t arm_num)
122 if (arm_num == ARM_LEFT_NUM) {
123 return sensor_get(S_LEFT);
125 else if (arm_num == ARM_RIGHT_NUM) {
126 return sensor_get(S_RIGHT);
131 /* return 0 if color is correct, else return -1 */
132 int8_t arm_get_color_sensor(uint8_t arm_num)
135 if (arm_num == ARM_LEFT_NUM) {
136 col = sensor_get(S_COL_LEFT);
138 else if (arm_num == ARM_RIGHT_NUM) {
139 col = sensor_get(S_COL_RIGHT);
142 /* if col != 0, column is red */
144 if (mechboard.our_color == I2C_COLOR_RED)
149 if (mechboard.our_color == I2C_COLOR_GREEN)
155 void state_debug_wait_key_pressed(void)
159 printf_P(PSTR("press a key\r\n"));
160 while(!cmdline_keypressed());
163 /* set a new state, return 0 on success */
164 int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd)
167 prev_state = mainboard_command.mode;
168 memcpy(&mainboard_command, cmd, sizeof(mainboard_command));
169 STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode);
173 /* check that state is the one in parameter and that state did not
175 uint8_t state_check(uint8_t mode)
178 if (mode != mainboard_command.mode)
184 /* force quit when CTRL-C is typed */
185 c = cmdline_getchar();
188 if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) {
189 mainboard_command.mode = EXIT;
195 uint8_t state_get_mode(void)
197 return mainboard_command.mode;
200 void pump_reset_all(void)
203 for (i=0; i<4; i++) {
204 pump_set(i, PUMP_OFF);
209 void pump_check_all(void)
211 if (pump_is_busy(PUMP_LEFT1_NUM) &&
212 mechboard.pump_left1_current < I2C_MECHBOARD_CURRENT_COLUMN) {
213 STMCH_DEBUG("Mark l1 as free");
214 pump_mark_free(PUMP_LEFT1_NUM);
215 pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
218 if (pump_is_busy(PUMP_LEFT2_NUM) &&
219 mechboard.pump_left2_current < I2C_MECHBOARD_CURRENT_COLUMN) {
220 STMCH_DEBUG("Mark l2 as free");
221 pump_mark_free(PUMP_LEFT2_NUM);
222 pump_set(PUMP_LEFT2_NUM, PUMP_OFF);
225 if (pump_is_busy(PUMP_RIGHT1_NUM) &&
226 sensor_get_adc(ADC_CSENSE3) < I2C_MECHBOARD_CURRENT_COLUMN) {
227 STMCH_DEBUG("Mark r1 as free");
228 pump_mark_free(PUMP_RIGHT1_NUM);
229 pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
232 if (pump_is_busy(PUMP_RIGHT2_NUM) &&
233 sensor_get_adc(ADC_CSENSE4) < I2C_MECHBOARD_CURRENT_COLUMN) {
234 STMCH_DEBUG("Mark r2 as free");
235 pump_mark_free(PUMP_RIGHT2_NUM);
236 pump_set(PUMP_RIGHT2_NUM, PUMP_OFF);
240 uint8_t get_free_pump_count(void)
242 uint8_t i, free_pump_count = 0;
243 for (i=0; i<4; i++) {
247 return free_pump_count;
250 /* move finger if we are not in lazy harvest */
251 void state_finger_goto(uint8_t mode, uint16_t position)
253 if (mode == LAZY_HARVEST)
255 finger_goto(position);
258 void state_manivelle(int16_t step_deg)
263 double step = RAD(step_deg);
266 double ar = RAD(180);
274 arm_do_xy(&left_arm, add_d+l*sin(al), add_h+l*cos(al), 10);
275 arm_do_xy(&right_arm, add_d+l*sin(ar), add_h+l*cos(ar), 10);
277 if (time_get_us2() - us > (4000L * 1000L))
282 static void state_do_manivelle(void)
284 if (!state_check(MANIVELLE))
287 while (state_check(MANIVELLE));
290 /* common function for pickup/harvest */
291 static void state_pickup_or_harvest(uint8_t mode)
293 int8_t arm_num, pump_num;
294 int8_t other_arm_num, other_pump_num;
297 uint8_t flags, bad_color = 0, have_2cols = 0;
302 if (pickup_side == I2C_LEFT_SIDE) {
303 arm_num = ARM_LEFT_NUM;
304 other_arm_num = ARM_RIGHT_NUM;
307 arm_num = ARM_RIGHT_NUM;
308 other_arm_num = ARM_LEFT_NUM;
311 pump_num = arm_get_free_pump(arm_num);
312 other_pump_num = arm_get_free_pump(other_arm_num);
314 /* pump is not free... skip to other arm */
315 if (mode == HARVEST && pump_num == -1) {
316 STMCH_DEBUG("%s no free pump", __FUNCTION__);
317 if (arm_num == ARM_RIGHT_NUM) {
318 state_finger_goto(mode, FINGER_CENTER_RIGHT);
319 pickup_side = I2C_LEFT_SIDE;
322 state_finger_goto(mode, FINGER_CENTER_LEFT);
323 pickup_side = I2C_RIGHT_SIDE;
327 else if (mode == PICKUP && pump_num == -1) {
328 /* or exit when we are in pickup mode */
330 if (mainboard_command.mode == mode)
331 mainboard_command.mode = WAIT;
336 /* wait front sensor */
337 if (mode == HARVEST || mode == LAZY_HARVEST) {
338 STMCH_DEBUG("%s wait front", __FUNCTION__);
341 if (sensor_get(S_FRONT))
343 if (state_check(mode) == 0)
345 /* wait 500ms before reading other
347 if (time_get_us2() - us < (500 * 1000L))
349 if (arm_get_sensor(arm_num))
351 if (arm_get_sensor(other_arm_num)) {
354 arm_num = other_arm_num;
356 pump_num = arm_get_free_pump(arm_num);
357 other_pump_num = arm_get_free_pump(other_arm_num);
358 if (other_pump_num == -1)
366 STMCH_DEBUG("%s arm_num=%d pump_num=%d",
367 __FUNCTION__, arm_num, pump_num);
369 /* when ready, move finger */
370 if (arm_num == ARM_RIGHT_NUM)
371 state_finger_goto(mode, FINGER_RIGHT);
373 state_finger_goto(mode, FINGER_LEFT);
375 state_debug_wait_key_pressed();
378 arm = arm_num2ptr(arm_num);
380 /* prepare arm, should be already done */
381 arm_goto_prepare_get(arm_num, pump_num);
382 while (arm_test_traj_end(arm, ARM_TRAJ_ALL) &&
385 STMCH_DEBUG("%s arm pos ok", __FUNCTION__);
387 state_debug_wait_key_pressed();
389 /* wait to see the column on the sensor */
392 if (arm_get_sensor(arm_num))
394 if (state_check(mode) == 0)
396 if (mode == PICKUP) /* no timeout in pickup */
398 /* 500ms timeout in harvest, go back */
399 if (time_get_us2() - us > 500*1000L) {
400 STMCH_DEBUG("%s timeout", __FUNCTION__);
402 if (arm_num == ARM_RIGHT_NUM)
403 state_finger_goto(mode, FINGER_LEFT);
405 state_finger_goto(mode, FINGER_RIGHT);
407 if (sensor_get(S_FRONT))
410 pump_set(pump_num, PUMP_OFF);
415 state_dump_sensors();
417 pump_set(pump_num, PUMP_ON);
419 if (arm_get_color_sensor(arm_num) == -1) {
421 STMCH_DEBUG("%s prepare eject", __FUNCTION__);
422 mainboard_command.mode = PREPARE_EJECT;
423 state_do_eject(arm_num, pump_num, mode);
427 STMCH_DEBUG("%s sensor ok", __FUNCTION__);
429 /* by the way, prepare the other arm */
430 if (other_pump_num != -1)
431 arm_goto_prepare_get(other_arm_num, other_pump_num);
434 arm_goto_get_column(arm_num, pump_num);
439 if (time_get_us2() - us > 50*1000L)
443 /* if we still see the front sensor, it's because
444 * there are 2 columns instead of one or because there
445 * is another column, so send the arm on other
447 if (sensor_get(S_FRONT) && have_2cols == 0) {
448 STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__);
450 if (finger_get_side() == I2C_LEFT_SIDE)
451 state_finger_goto(mode, FINGER_RIGHT);
453 state_finger_goto(mode, FINGER_LEFT);
457 if (mode == HARVEST && have_2cols == 0) {
458 /* just release a bit of effort */
459 if (finger_get_side() == I2C_LEFT_SIDE) {
460 state_finger_goto(mode, FINGER_LEFT_RELAX);
463 state_finger_goto(mode, FINGER_RIGHT_RELAX);
466 else if (mode == PICKUP) {
467 /* no free pump on other arm */
468 if (other_pump_num == -1) {
469 if (finger_get_side() == I2C_LEFT_SIDE) {
470 state_finger_goto(mode, FINGER_LEFT_RELAX);
473 state_finger_goto(mode, FINGER_RIGHT_RELAX);
476 /* else send finger on the other side */
478 if (finger_get_side() == I2C_LEFT_SIDE) {
479 state_finger_goto(mode, FINGER_RIGHT);
482 state_finger_goto(mode, FINGER_LEFT);
490 if (time_get_us2() - us > 100*1000L)
494 /* if we still see the front sensor, it's because
495 * there are 2 columns instead of one or because there
496 * is another column, so send the arm on other
498 if (sensor_get(S_FRONT) && have_2cols == 0) {
499 STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__);
501 if (finger_get_side() == I2C_LEFT_SIDE)
502 state_finger_goto(mode, FINGER_RIGHT);
504 state_finger_goto(mode, FINGER_LEFT);
508 /* consider the column as taken */
509 pump_mark_busy(pump_num);
511 state_debug_wait_key_pressed();
513 arm_goto_intermediate_get(arm_num, pump_num);
514 arm_wait_traj_end(arm, ARM_TRAJ_ALL_NEAR);
517 pump_num = arm_get_free_pump(arm_num);
519 arm_goto_loaded(arm_num);
521 arm_goto_intermediate_get(arm_num, pump_num);
523 state_debug_wait_key_pressed();
525 /* switch to wait state */
526 if (get_free_pump_count() == 0) {
528 if (mainboard_command.mode == mode)
529 mainboard_command.mode = WAIT;
533 /* next pickup/harvest will be on the other side */
534 if (pickup_side == I2C_LEFT_SIDE)
535 pickup_side = I2C_RIGHT_SIDE;
537 pickup_side = I2C_LEFT_SIDE;
541 /* manual mode, arm position is sent from mainboard */
542 static void state_do_manual(void)
544 if (!state_check(MANUAL))
546 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
547 while (state_check(MANUAL));
551 static void state_do_wait(void)
553 if (!state_check(WAIT))
555 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
556 while (state_check(WAIT));
560 static void state_do_init(void)
562 if (!state_check(INIT))
565 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
566 while (state_check(INIT));
569 /* harvest columns elts from area */
570 static void state_do_harvest(void)
572 if (!state_check(HARVEST))
575 if (get_free_pump_count() == 0) {
576 mainboard_command.mode = WAIT;
580 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
582 state_pickup_or_harvest(HARVEST);
585 /* harvest columns elts from area without moving finger */
586 static void state_do_lazy_harvest(void)
588 if (!state_check(LAZY_HARVEST))
591 if (get_free_pump_count() == 0) {
592 mainboard_command.mode = WAIT;
596 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
598 state_pickup_or_harvest(LAZY_HARVEST);
601 /* eject a column. always called from pickup mode. */
602 static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode)
605 arm = arm_num2ptr(arm_num);
607 if (finger_get_side() == I2C_LEFT_SIDE) {
608 state_finger_goto(old_mode, FINGER_LEFT_RELAX);
611 state_finger_goto(old_mode, FINGER_RIGHT_RELAX);
614 /* wait mainboard to eject */
615 while (state_check(PREPARE_EJECT));
617 if (finger_get_side() == I2C_LEFT_SIDE) {
618 state_finger_goto(old_mode, FINGER_CENTER_LEFT);
621 state_finger_goto(old_mode, FINGER_CENTER_RIGHT);
624 arm_goto_get_column(arm_num, pump_num);
625 arm_wait_traj_end(arm, ARM_TRAJ_ALL);
628 state_debug_wait_key_pressed();
630 arm_goto_prepare_eject(arm_num, pump_num);
631 arm_wait_traj_end(arm, ARM_TRAJ_ALL);
633 state_debug_wait_key_pressed();
635 if (finger_get_side() == I2C_LEFT_SIDE) {
636 state_finger_goto(old_mode, FINGER_LEFT_RELAX);
639 state_finger_goto(old_mode, FINGER_RIGHT_RELAX);
642 state_debug_wait_key_pressed();
645 arm_goto_eject(arm_num, pump_num);
647 pump_set(pump_num, PUMP_REVERSE);
648 arm_wait_traj_end(arm, ARM_TRAJ_ALL);
650 arm_goto_intermediate_get(arm_num, pump_num);
651 pump_set(pump_num, PUMP_OFF);
655 /* prepare pickup in a dispenser, or harvest */
656 static void state_do_prepare_pickup(void)
658 uint8_t left_count = 0, right_count = 0;
659 int8_t pump_l, pump_r;
661 if (!state_check(PREPARE_PICKUP))
663 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
667 pump_l = arm_get_free_pump(ARM_LEFT_NUM);
669 arm_goto_loaded(ARM_LEFT_NUM);
672 arm_goto_intermediate_front_get(ARM_LEFT_NUM, pump_l);
675 pump_r = arm_get_free_pump(ARM_RIGHT_NUM);
677 arm_goto_loaded(ARM_RIGHT_NUM);
680 arm_goto_intermediate_front_get(ARM_RIGHT_NUM, pump_r);
683 arm_wait_both(ARM_TRAJ_ALL);
686 arm_goto_prepare_get(ARM_LEFT_NUM, pump_l);
688 arm_goto_prepare_get(ARM_RIGHT_NUM, pump_r);
690 if (mainboard_command.prep_pickup.side == I2C_AUTO_SIDE) {
691 left_count += pump_is_busy(PUMP_LEFT1_NUM);
692 left_count += pump_is_busy(PUMP_LEFT2_NUM);
693 right_count += pump_is_busy(PUMP_RIGHT1_NUM);
694 right_count += pump_is_busy(PUMP_RIGHT2_NUM);
695 if (left_count < right_count)
696 finger_goto(FINGER_RIGHT);
698 finger_goto(FINGER_LEFT);
700 else if (mainboard_command.prep_pickup.side == I2C_LEFT_SIDE)
701 finger_goto(FINGER_LEFT);
702 else if (mainboard_command.prep_pickup.side == I2C_RIGHT_SIDE)
703 finger_goto(FINGER_RIGHT);
704 else if (mainboard_command.prep_pickup.side == I2C_CENTER_SIDE)
705 finger_goto(FINGER_CENTER_LEFT);
707 /* try to know on which side we have to pickup */
708 if (finger_get_side() == I2C_RIGHT_SIDE) {
709 pickup_side = I2C_LEFT_SIDE;
712 pickup_side = I2C_RIGHT_SIDE;
715 arm_prepare_free_pumps();
717 mainboard_command.mode = mainboard_command.prep_pickup.next_mode;
719 while (state_check(PREPARE_PICKUP));
722 /* clear pickup zone, will switch to harvest if needed */
723 static void state_do_clear(void)
727 if (!state_check(CLEAR))
730 if (get_free_pump_count() == 0) {
731 mainboard_command.mode = WAIT;
735 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
737 finger_goto(FINGER_LEFT);
738 err = WAIT_COND_OR_TIMEOUT(sensor_get(S_LEFT), 500);
741 if (mainboard_command.mode == CLEAR)
742 mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
744 pickup_side = I2C_LEFT_SIDE;
748 finger_goto(FINGER_RIGHT);
749 err = WAIT_COND_OR_TIMEOUT(sensor_get(S_RIGHT), 500);
752 if (mainboard_command.mode == CLEAR)
753 mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
755 pickup_side = I2C_RIGHT_SIDE;
760 if (mainboard_command.mode == CLEAR)
761 mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
765 /* do a lazy pickup */
766 static void state_do_lazy_pickup(void)
768 int8_t flags, arm_num, pump_num;
771 if (!state_check(LAZY_PICKUP))
774 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
776 if (arm_get_sensor(ARM_LEFT_NUM) &&
777 arm_get_sensor(ARM_RIGHT_NUM)) {
779 if (mainboard_command.mode == LAZY_PICKUP) {
780 mainboard_command.mode = WAIT;
786 if (finger_get_side() == I2C_RIGHT_SIDE) {
787 finger_goto(FINGER_LEFT);
788 arm_num = ARM_LEFT_NUM;
791 finger_goto(FINGER_RIGHT);
792 arm_num = ARM_RIGHT_NUM;
797 if (state_check(LAZY_PICKUP) == 0)
799 if (arm_get_sensor(arm_num))
801 if (time_get_us2() - us > 500*1000L) {
802 if (finger_get_side() == I2C_RIGHT_SIDE)
803 finger_goto(FINGER_LEFT);
805 finger_goto(FINGER_RIGHT);
810 if (arm_get_color_sensor(arm_num) == -1) {
811 pump_num = arm_get_free_pump(arm_num);
814 pump_set(pump_num, PUMP_ON);
815 STMCH_DEBUG("%s prepare eject", __FUNCTION__);
816 mainboard_command.mode = PREPARE_EJECT;
817 state_do_eject(arm_num, pump_num, LAZY_PICKUP);
821 /* pickup from a dispenser automatically */
822 static void state_do_pickup(void)
824 if (!state_check(PICKUP))
827 if (get_free_pump_count() == 0) {
828 mainboard_command.mode = WAIT;
832 /* XXX check that finger is at correct place */
834 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
836 state_pickup_or_harvest(PICKUP);
839 /* store columns without using arms */
840 static void state_do_store(void)
843 int8_t other_arm_num;
846 if (!state_check(STORE))
849 if (get_free_pump_count() == 0) {
850 mainboard_command.mode = WAIT;
854 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
857 if (pickup_side == I2C_LEFT_SIDE) {
858 arm_num = ARM_LEFT_NUM;
859 other_arm_num = ARM_RIGHT_NUM;
862 arm_num = ARM_RIGHT_NUM;
863 other_arm_num = ARM_LEFT_NUM;
867 if (sensor_get(S_FRONT))
869 if (state_check(STORE) == 0)
873 /* when ready, move finger */
874 if (arm_num == ARM_RIGHT_NUM)
875 finger_goto(FINGER_RIGHT);
877 finger_goto(FINGER_LEFT);
879 /* wait to see the column on the sensor */
882 if (arm_get_sensor(arm_num))
884 if (state_check(STORE) == 0)
886 /* 500ms timeout in harvest, go back */
887 if (time_get_us2() - us > 500*1000L) {
888 STMCH_DEBUG("%s timeout", __FUNCTION__);
890 if (arm_num == ARM_RIGHT_NUM)
891 finger_goto(FINGER_LEFT);
893 finger_goto(FINGER_RIGHT);
898 if (arm_get_sensor(arm_num) && arm_get_sensor(other_arm_num)) {
899 STMCH_DEBUG("%s full", __FUNCTION__);
900 while (state_check(STORE));
904 /* next store will be on the other side */
905 if (pickup_side == I2C_LEFT_SIDE)
906 pickup_side = I2C_RIGHT_SIDE;
908 pickup_side = I2C_LEFT_SIDE;
911 /* prepare the building of a temple */
912 static void state_do_prepare_build(void)
914 int8_t pump_num, level;
915 if (!state_check(PREPARE_BUILD))
917 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
921 if (finger_get_side() == I2C_LEFT_SIDE)
922 finger_goto(FINGER_LEFT);
924 finger_goto(FINGER_RIGHT);
926 pump_num = arm_get_busy_pump(ARM_LEFT_NUM);
927 level = mainboard_command.prep_build.level_l;
928 if (pump_num != -1 && level != -1)
929 arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_num,
930 level, I2C_AUTOBUILD_DEFAULT_DIST);
932 pump_num = arm_get_busy_pump(ARM_RIGHT_NUM);
933 level = mainboard_command.prep_build.level_r;
934 if (pump_num != -1 && level != -1)
935 arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_num,
936 level, I2C_AUTOBUILD_DEFAULT_DIST);
938 while (state_check(PREPARE_BUILD));
941 /* prepare the building of a temple */
942 static void state_do_push_temple(void)
946 level = mainboard_command.push_temple.level;
948 if (!state_check(PUSH_TEMPLE))
950 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
952 if (finger_get_side() == I2C_LEFT_SIDE)
953 finger_goto(FINGER_LEFT);
955 finger_goto(FINGER_RIGHT);
957 arm_goto_prepare_push_temple(ARM_LEFT_NUM);
958 arm_goto_prepare_push_temple(ARM_RIGHT_NUM);
959 arm_wait_both(ARM_TRAJ_ALL);
961 arm_goto_push_temple(ARM_LEFT_NUM, level);
962 arm_goto_push_temple(ARM_RIGHT_NUM, level);
964 while (state_check(PUSH_TEMPLE));
967 /* prepare the building of a temple */
968 static void state_do_push_temple_disc(void)
973 if (!state_check(PUSH_TEMPLE_DISC))
975 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
977 side = mainboard_command.push_temple_disc.side;
979 if (side == I2C_LEFT_SIDE) {
980 arm = arm_num2ptr(ARM_LEFT_NUM);
981 arm_goto_prepare_push_temple_disc(ARM_LEFT_NUM);
982 arm_wait_traj_end(arm, ARM_TRAJ_ALL);
983 arm_goto_push_temple_disc(ARM_LEFT_NUM);
986 arm = arm_num2ptr(ARM_RIGHT_NUM);
987 arm_goto_prepare_push_temple_disc(ARM_RIGHT_NUM);
988 arm_wait_traj_end(arm, ARM_TRAJ_ALL);
989 arm_goto_push_temple_disc(ARM_RIGHT_NUM);
992 while (state_check(PUSH_TEMPLE_DISC));
995 /* prepare the building of a temple (mainly for columns) */
996 static void state_do_prepare_inside(void)
998 int8_t pump_num, level_l, level_r;
999 if (!state_check(PREPARE_INSIDE))
1002 level_l = mainboard_command.prep_inside.level_l;
1003 level_r = mainboard_command.prep_inside.level_r;
1004 STMCH_DEBUG("%s mode=%d level_l=%d, level_r=%d", __FUNCTION__,
1005 state_get_mode(), level_l, level_r);
1009 if (finger_get_side() == I2C_LEFT_SIDE)
1010 finger_goto(FINGER_LEFT);
1012 finger_goto(FINGER_RIGHT);
1014 pump_num = arm_get_busy_pump(ARM_LEFT_NUM);
1016 pump_num = PUMP_LEFT1_NUM;
1018 arm_goto_prepare_build_inside(ARM_LEFT_NUM, pump_num,
1021 pump_num = arm_get_busy_pump(ARM_RIGHT_NUM);
1023 pump_num = PUMP_RIGHT1_NUM;
1025 arm_goto_prepare_build_inside(ARM_RIGHT_NUM, pump_num,
1028 while (state_check(PREPARE_INSIDE));
1031 /* moving position */
1032 static void state_do_loaded(void)
1034 if (!state_check(LOADED))
1036 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1040 if (finger_get_side() == I2C_LEFT_SIDE)
1041 finger_goto(FINGER_LEFT);
1043 finger_goto(FINGER_RIGHT);
1045 arm_goto_loaded(ARM_LEFT_NUM);
1046 arm_goto_loaded(ARM_RIGHT_NUM);
1048 while (state_check(LOADED));
1051 static void state_do_build_lintel(uint8_t level)
1053 STMCH_DEBUG("%s() level=%d have_lintel=%d",
1054 __FUNCTION__, level, mechboard.lintel_count);
1058 arm_goto_prepare_get_lintel_inside1();
1059 arm_wait_both(ARM_TRAJ_ALL);
1060 state_debug_wait_key_pressed();
1062 pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE);
1063 pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE);
1064 arm_goto_prepare_get_lintel_inside2(mechboard.lintel_count);
1065 arm_wait_both(ARM_TRAJ_ALL);
1066 state_debug_wait_key_pressed();
1068 arm_goto_get_lintel_inside(mechboard.lintel_count);
1069 arm_wait_both(ARM_TRAJ_ALL);
1070 state_debug_wait_key_pressed();
1073 arm_goto_prepare_build_lintel1();
1074 arm_wait_both(ARM_TRAJ_ALL);
1075 state_debug_wait_key_pressed();
1077 arm_goto_prepare_build_lintel2(level);
1078 arm_wait_both(ARM_TRAJ_ALL);
1079 state_debug_wait_key_pressed();
1081 arm_goto_prepare_build_lintel3(level);
1082 arm_wait_both(ARM_TRAJ_ALL);
1083 state_debug_wait_key_pressed();
1085 if (mechboard.lintel_count == 1)
1086 servo_lintel_1lin();
1088 servo_lintel_2lin();
1090 arm_goto_build_lintel(level);
1091 arm_wait_both(ARM_TRAJ_ALL);
1093 pump_set(PUMP_LEFT1_NUM, PUMP_ON);
1094 time_wait_ms(50); /* right arm a bit after */
1095 pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
1097 pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1098 pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1100 mechboard.lintel_count --;
1103 /* Build one level of column. If pump_r or pump_l is -1, don't build
1105 static void state_do_build_column(uint8_t level_l, int8_t pump_l,
1107 uint8_t level_r, int8_t pump_r,
1110 STMCH_DEBUG("%s() level_l=%d pump_l=%d level_r=%d pump_r=%d",
1111 __FUNCTION__, level_l, pump_l, level_r, pump_r);
1114 if (pump_l == -1 && pump_r == -1)
1117 /* go above the selected level */
1119 arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_l, level_l, dist_l);
1121 arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_r, level_r, dist_r);
1122 STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL),
1123 arm_test_traj_end(&right_arm, ARM_TRAJ_ALL));
1124 arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL);
1125 STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL),
1126 arm_test_traj_end(&right_arm, ARM_TRAJ_ALL));
1128 state_debug_wait_key_pressed();
1130 /* drop columns of P2 */
1132 arm_goto_autobuild(ARM_LEFT_NUM, pump_l, level_l, dist_l);
1134 arm_goto_autobuild(ARM_RIGHT_NUM, pump_r, level_r, dist_r);
1135 arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL);
1137 state_debug_wait_key_pressed();
1141 pump_set(pump_l, PUMP_REVERSE);
1143 pump_set(pump_r, PUMP_REVERSE);
1146 pump_set(pump_l, PUMP_OFF);
1147 pump_mark_free(pump_l);
1150 pump_set(pump_r, PUMP_OFF);
1151 pump_mark_free(pump_r);
1154 state_debug_wait_key_pressed();
1157 /* autobuild columns elts from area */
1158 /* check level to avoid bad things ? */
1159 /* check if enough cols ? */
1160 static void state_do_autobuild(void)
1162 int8_t pump_l, pump_r;
1163 /* copy command into local data */
1164 int8_t level_l = mainboard_command.autobuild.level_left;
1165 int8_t level_r = mainboard_command.autobuild.level_right;
1166 uint8_t count_l = mainboard_command.autobuild.count_left;
1167 uint8_t count_r = mainboard_command.autobuild.count_right;
1168 uint8_t dist_l = mainboard_command.autobuild.distance_left;
1169 uint8_t dist_r = mainboard_command.autobuild.distance_right;
1170 uint8_t do_lintel = mainboard_command.autobuild.do_lintel;
1171 int8_t max_level = level_l;
1174 if (!state_check(AUTOBUILD))
1177 STMCH_DEBUG("%s mode=%d do_lintel=%d", __FUNCTION__,
1178 state_get_mode(), do_lintel);
1179 STMCH_DEBUG(" left: level=%d count=%d", level_l, count_l);
1180 STMCH_DEBUG(" right: level=%d count=%d", level_r, count_r);
1183 * build the first level of column if needed
1186 /* don't build with this arm if no pump or if we don't ask to */
1187 pump_l = arm_get_busy_pump(ARM_LEFT_NUM);
1190 pump_r = arm_get_busy_pump(ARM_RIGHT_NUM);
1194 if (pump_l == -1 && pump_r == -1)
1197 state_do_build_column(level_l, pump_l, dist_l,
1198 level_r, pump_r, dist_r);
1204 max_level = level_l;
1209 if (level_r > max_level)
1210 max_level = level_r;
1214 * build the second level of column if needed
1217 /* don't build with this arm if no pump or if we don't ask to */
1218 pump_l = arm_get_busy_pump(ARM_LEFT_NUM);
1221 pump_r = arm_get_busy_pump(ARM_RIGHT_NUM);
1225 state_do_build_column(level_l, pump_l, dist_l,
1226 level_r, pump_r, dist_r);
1232 max_level = level_l;
1237 if (level_r > max_level)
1238 max_level = level_r;
1241 state_debug_wait_key_pressed();
1243 if (mechboard.lintel_count != 0 && do_lintel != 0) {
1244 arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM,
1247 I2C_AUTOBUILD_DEFAULT_DIST);
1248 arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM,
1251 I2C_AUTOBUILD_DEFAULT_DIST);
1252 arm_wait_both(ARM_TRAJ_ALL_NEAR);
1253 state_debug_wait_key_pressed();
1255 arm_goto_prepare_autobuild_inside(ARM_LEFT_NUM,
1258 arm_goto_prepare_autobuild_inside(ARM_RIGHT_NUM,
1261 arm_wait_both(ARM_TRAJ_ALL_NEAR);
1262 state_debug_wait_key_pressed();
1266 if (mechboard.lintel_count == 0 || do_lintel == 0) {
1267 mainboard_command.mode = WAIT;
1271 state_do_build_lintel(max_level);
1272 mainboard_command.mode = WAIT;
1275 /* prepare to get the lintel */
1276 static void state_do_prepare_get_lintel(void)
1278 if (!state_check(PREPARE_GET_LINTEL))
1280 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1282 arm_goto_prepare_get_lintel_disp();
1283 arm_wait_both(ARM_TRAJ_ALL);
1285 pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1286 pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1288 /* go fully left or right */
1289 if (finger_get_side() == I2C_LEFT_SIDE)
1290 finger_goto(FINGER_LEFT);
1292 finger_goto(FINGER_RIGHT);
1294 while (state_check(PREPARE_GET_LINTEL));
1297 /* get the lintel from the dispenser */
1298 static void state_do_get_lintel(void)
1300 if (!state_check(GET_LINTEL))
1302 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1304 pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE);
1305 pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE);
1307 arm_goto_get_lintel_disp();
1308 arm_wait_both(ARM_TRAJ_ALL_NEAR);
1312 STMCH_DEBUG("%s left1=%d left2=%d", __FUNCTION__,
1313 mechboard.pump_left1_current,
1314 sensor_get_adc(ADC_CSENSE3));
1316 while (state_check(GET_LINTEL));
1318 /* mainboard asked to release lintel, so release pump first */
1319 if (state_get_mode() == PREPARE_GET_LINTEL) {
1320 pump_set(PUMP_LEFT1_NUM, PUMP_ON);
1321 pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
1323 pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1324 pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1328 /* put the lintel inside the robot */
1329 static void state_do_put_lintel(void)
1331 uint8_t prev_lin_count;
1333 if (!state_check(PUT_LINTEL))
1336 STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1337 prev_lin_count = mechboard.lintel_count;
1338 mechboard.lintel_count ++;
1340 arm_goto_prepare_get_lintel_disp();
1341 arm_wait_both(ARM_TRAJ_ALL);
1345 arm_goto_prepare_put_lintel();
1346 arm_wait_both(ARM_TRAJ_ALL_NEAR);
1348 arm_goto_put_lintel(prev_lin_count);
1349 arm_wait_both(ARM_TRAJ_ALL);
1351 pump_set(PUMP_LEFT1_NUM, PUMP_ON);
1352 pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
1354 if (mechboard.lintel_count == 1)
1355 servo_lintel_1lin();
1357 servo_lintel_2lin();
1361 pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1362 pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1364 arm_goto_prepare_put_lintel();
1365 arm_wait_both(ARM_TRAJ_ALL_NEAR);
1367 while (state_check(PUT_LINTEL));
1370 /* main state machine */
1371 void state_machine(void)
1373 while (state_get_mode() != EXIT) {
1378 state_do_lazy_harvest();
1379 state_do_prepare_pickup();
1381 state_do_prepare_inside();
1382 state_do_prepare_build();
1383 state_do_autobuild();
1384 state_do_prepare_get_lintel();
1385 state_do_get_lintel();
1386 state_do_put_lintel();
1389 state_do_lazy_pickup();
1392 state_do_manivelle();
1393 state_do_push_temple();
1394 state_do_push_temple_disc();
1398 void state_init(void)
1400 vt100_init(&local_vt100);
1401 mainboard_command.mode = WAIT;
1403 mechboard.lintel_count = 1;
1404 mechboard.column_flags = 0;
1405 servo_lintel_1lin();
1406 finger_goto(FINGER_LEFT);