--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#include <math.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <vt100.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "cmdline.h"
+#include "sensor.h"
+#include "actuator.h"
+#include "arm_xy.h"
+#include "arm_highlevel.h"
+#include "state.h"
+
+#define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args)
+#define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args)
+#define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args)
+
+/* shorter aliases for this file */
+#define MANUAL I2C_MECHBOARD_MODE_MANUAL
+#define HARVEST I2C_MECHBOARD_MODE_HARVEST
+#define PREPARE_PICKUP I2C_MECHBOARD_MODE_PREPARE_PICKUP
+#define PICKUP I2C_MECHBOARD_MODE_PICKUP
+#define PREPARE_BUILD I2C_MECHBOARD_MODE_PREPARE_BUILD
+#define AUTOBUILD I2C_MECHBOARD_MODE_AUTOBUILD
+#define WAIT I2C_MECHBOARD_MODE_WAIT
+#define INIT I2C_MECHBOARD_MODE_INIT
+#define PREPARE_GET_LINTEL I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL
+#define GET_LINTEL I2C_MECHBOARD_MODE_GET_LINTEL
+#define PUT_LINTEL I2C_MECHBOARD_MODE_PUT_LINTEL
+#define PREPARE_EJECT I2C_MECHBOARD_MODE_PREPARE_EJECT
+#define EJECT I2C_MECHBOARD_MODE_EJECT
+#define CLEAR I2C_MECHBOARD_MODE_CLEAR
+#define LAZY_HARVEST I2C_MECHBOARD_MODE_LAZY_HARVEST
+#define LOADED I2C_MECHBOARD_MODE_LOADED
+#define PREPARE_INSIDE I2C_MECHBOARD_MODE_PREPARE_INSIDE
+#define STORE I2C_MECHBOARD_MODE_STORE
+#define LAZY_PICKUP I2C_MECHBOARD_MODE_LAZY_PICKUP
+#define MANIVELLE I2C_MECHBOARD_MODE_MANIVELLE
+#define PUSH_TEMPLE I2C_MECHBOARD_MODE_PUSH_TEMPLE
+#define PUSH_TEMPLE_DISC I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC
+#define EXIT I2C_MECHBOARD_MODE_EXIT
+
+static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode);
+
+static struct i2c_cmd_mechboard_set_mode mainboard_command;
+static struct vt100 local_vt100;
+static volatile uint8_t prev_state;
+static uint8_t pickup_side;
+static volatile uint8_t changed = 0;
+
+uint8_t state_debug = 0;
+
+void state_dump_sensors(void)
+{
+ uint16_t tmp = sensor_get_all();
+ prog_char *front = PSTR("no_front");
+ prog_char *left = PSTR("no_left");
+ prog_char *right = PSTR("no_right");
+
+ if (tmp & _BV(S_FRONT))
+ front = PSTR("FRONT");
+ if (tmp & _BV(S_LEFT)) {
+ if (tmp & _BV(S_COL_LEFT))
+ left = PSTR("LEFT(red)");
+ else
+ left = PSTR("LEFT(green)");
+ }
+ if (tmp & _BV(S_RIGHT)) {
+ if (tmp & _BV(S_COL_RIGHT))
+ right = PSTR("RIGHT(red)");
+ else
+ right = PSTR("RIGHT(green)");
+ }
+
+ STMCH_DEBUG("sensors = %S %S %S", front, left, right);
+}
+
+/* return 1 if column is there */
+uint8_t arm_get_sensor(uint8_t arm_num)
+{
+ if (arm_num == ARM_LEFT_NUM) {
+ return sensor_get(S_LEFT);
+ }
+ else if (arm_num == ARM_RIGHT_NUM) {
+ return sensor_get(S_RIGHT);
+ }
+ return 0;
+}
+
+/* return 0 if color is correct, else return -1 */
+int8_t arm_get_color_sensor(uint8_t arm_num)
+{
+ uint8_t col = 0;
+ if (arm_num == ARM_LEFT_NUM) {
+ col = sensor_get(S_COL_LEFT);
+ }
+ else if (arm_num == ARM_RIGHT_NUM) {
+ col = sensor_get(S_COL_RIGHT);
+ }
+
+ /* if col != 0, column is red */
+ if (col) {
+ if (mechboard.our_color == I2C_COLOR_RED)
+ return 0;
+ return -1;
+ }
+ else {
+ if (mechboard.our_color == I2C_COLOR_GREEN)
+ return 0;
+ return -1;
+ }
+}
+
+void state_debug_wait_key_pressed(void)
+{
+ if (!state_debug)
+ return;
+ printf_P(PSTR("press a key\r\n"));
+ while(!cmdline_keypressed());
+}
+
+/* set a new state, return 0 on success */
+int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd)
+{
+ changed = 1;
+ prev_state = mainboard_command.mode;
+ memcpy(&mainboard_command, cmd, sizeof(mainboard_command));
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode);
+ return 0;
+}
+
+/* check that state is the one in parameter and that state did not
+ * changed */
+uint8_t state_check(uint8_t mode)
+{
+ int16_t c;
+ if (mode != mainboard_command.mode)
+ return 0;
+
+ if (changed)
+ return 0;
+
+ /* force quit when CTRL-C is typed */
+ c = cmdline_getchar();
+ if (c == -1)
+ return 1;
+ if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) {
+ mainboard_command.mode = EXIT;
+ return 0;
+ }
+ return 1;
+}
+
+uint8_t state_get_mode(void)
+{
+ return mainboard_command.mode;
+}
+
+void pump_reset_all(void)
+{
+ uint8_t i;
+ for (i=0; i<4; i++) {
+ pump_set(i, PUMP_OFF);
+ pump_mark_free(i);
+ }
+}
+
+void pump_check_all(void)
+{
+ if (pump_is_busy(PUMP_LEFT1_NUM) &&
+ mechboard.pump_left1_current < I2C_MECHBOARD_CURRENT_COLUMN) {
+ STMCH_DEBUG("Mark l1 as free");
+ pump_mark_free(PUMP_LEFT1_NUM);
+ pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
+ }
+
+ if (pump_is_busy(PUMP_LEFT2_NUM) &&
+ mechboard.pump_left2_current < I2C_MECHBOARD_CURRENT_COLUMN) {
+ STMCH_DEBUG("Mark l2 as free");
+ pump_mark_free(PUMP_LEFT2_NUM);
+ pump_set(PUMP_LEFT2_NUM, PUMP_OFF);
+ }
+
+ if (pump_is_busy(PUMP_RIGHT1_NUM) &&
+ sensor_get_adc(ADC_CSENSE3) < I2C_MECHBOARD_CURRENT_COLUMN) {
+ STMCH_DEBUG("Mark r1 as free");
+ pump_mark_free(PUMP_RIGHT1_NUM);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
+ }
+
+ if (pump_is_busy(PUMP_RIGHT2_NUM) &&
+ sensor_get_adc(ADC_CSENSE4) < I2C_MECHBOARD_CURRENT_COLUMN) {
+ STMCH_DEBUG("Mark r2 as free");
+ pump_mark_free(PUMP_RIGHT2_NUM);
+ pump_set(PUMP_RIGHT2_NUM, PUMP_OFF);
+ }
+}
+
+uint8_t get_free_pump_count(void)
+{
+ uint8_t i, free_pump_count = 0;
+ for (i=0; i<4; i++) {
+ if (pump_is_free(i))
+ free_pump_count++;
+ }
+ return free_pump_count;
+}
+
+/* move finger if we are not in lazy harvest */
+void state_finger_goto(uint8_t mode, uint16_t position)
+{
+ if (mode == LAZY_HARVEST)
+ return;
+ finger_goto(position);
+}
+
+void state_manivelle(int16_t step_deg)
+{
+ double add_h = 0.;
+ double add_d = 160.;
+ double l = 70.;
+ double step = RAD(step_deg);
+ microseconds us;
+ double al = RAD(0);
+ double ar = RAD(180);
+
+ time_wait_ms(500);
+
+ us = time_get_us2();
+ while (1) {
+ al += step;
+ ar += step;
+ arm_do_xy(&left_arm, add_d+l*sin(al), add_h+l*cos(al), 10);
+ arm_do_xy(&right_arm, add_d+l*sin(ar), add_h+l*cos(ar), 10);
+ time_wait_ms(25);
+ if (time_get_us2() - us > (4000L * 1000L))
+ break;
+ }
+}
+
+static void state_do_manivelle(void)
+{
+ if (!state_check(MANIVELLE))
+ return;
+ state_manivelle(30);
+ while (state_check(MANIVELLE));
+}
+
+/* common function for pickup/harvest */
+static void state_pickup_or_harvest(uint8_t mode)
+{
+ int8_t arm_num, pump_num;
+ int8_t other_arm_num, other_pump_num;
+ struct arm *arm;
+ microseconds us;
+ uint8_t flags, bad_color = 0, have_2cols = 0;
+
+ pump_check_all();
+
+ /* get arm num */
+ if (pickup_side == I2C_LEFT_SIDE) {
+ arm_num = ARM_LEFT_NUM;
+ other_arm_num = ARM_RIGHT_NUM;
+ }
+ else {
+ arm_num = ARM_RIGHT_NUM;
+ other_arm_num = ARM_LEFT_NUM;
+ }
+
+ pump_num = arm_get_free_pump(arm_num);
+ other_pump_num = arm_get_free_pump(other_arm_num);
+
+ /* pump is not free... skip to other arm */
+ if (mode == HARVEST && pump_num == -1) {
+ STMCH_DEBUG("%s no free pump", __FUNCTION__);
+ if (arm_num == ARM_RIGHT_NUM) {
+ state_finger_goto(mode, FINGER_CENTER_RIGHT);
+ pickup_side = I2C_LEFT_SIDE;
+ }
+ else {
+ state_finger_goto(mode, FINGER_CENTER_LEFT);
+ pickup_side = I2C_RIGHT_SIDE;
+ }
+ return;
+ }
+ else if (mode == PICKUP && pump_num == -1) {
+ /* or exit when we are in pickup mode */
+ IRQ_LOCK(flags);
+ if (mainboard_command.mode == mode)
+ mainboard_command.mode = WAIT;
+ IRQ_UNLOCK(flags);
+ }
+
+ us = time_get_us2();
+ /* wait front sensor */
+ if (mode == HARVEST || mode == LAZY_HARVEST) {
+ STMCH_DEBUG("%s wait front", __FUNCTION__);
+
+ while (1) {
+ if (sensor_get(S_FRONT))
+ break;
+ if (state_check(mode) == 0)
+ return;
+ /* wait 500ms before reading other
+ sensors */
+ if (time_get_us2() - us < (500 * 1000L))
+ continue;
+ if (arm_get_sensor(arm_num))
+ break;
+ if (arm_get_sensor(other_arm_num)) {
+ uint8_t tmp;
+ tmp = arm_num;
+ arm_num = other_arm_num;
+ other_arm_num = tmp;
+ pump_num = arm_get_free_pump(arm_num);
+ other_pump_num = arm_get_free_pump(other_arm_num);
+ if (other_pump_num == -1)
+ return; // XXX
+ break;
+ }
+ }
+ }
+
+
+ STMCH_DEBUG("%s arm_num=%d pump_num=%d",
+ __FUNCTION__, arm_num, pump_num);
+
+ /* when ready, move finger */
+ if (arm_num == ARM_RIGHT_NUM)
+ state_finger_goto(mode, FINGER_RIGHT);
+ else
+ state_finger_goto(mode, FINGER_LEFT);
+
+ state_debug_wait_key_pressed();
+
+
+ arm = arm_num2ptr(arm_num);
+
+ /* prepare arm, should be already done */
+ arm_goto_prepare_get(arm_num, pump_num);
+ while (arm_test_traj_end(arm, ARM_TRAJ_ALL) &&
+ state_check(mode));
+
+ STMCH_DEBUG("%s arm pos ok", __FUNCTION__);
+
+ state_debug_wait_key_pressed();
+
+ /* wait to see the column on the sensor */
+ us = time_get_us2();
+ while (1) {
+ if (arm_get_sensor(arm_num))
+ break;
+ if (state_check(mode) == 0)
+ return;
+ if (mode == PICKUP) /* no timeout in pickup */
+ continue;
+ /* 500ms timeout in harvest, go back */
+ if (time_get_us2() - us > 500*1000L) {
+ STMCH_DEBUG("%s timeout", __FUNCTION__);
+
+ if (arm_num == ARM_RIGHT_NUM)
+ state_finger_goto(mode, FINGER_LEFT);
+ else
+ state_finger_goto(mode, FINGER_RIGHT);
+
+ if (sensor_get(S_FRONT))
+ time_wait_ms(500);
+
+ pump_set(pump_num, PUMP_OFF);
+ return;
+ }
+ }
+
+ state_dump_sensors();
+
+ pump_set(pump_num, PUMP_ON);
+ /* bad color */
+ if (arm_get_color_sensor(arm_num) == -1) {
+ bad_color = 1;
+ STMCH_DEBUG("%s prepare eject", __FUNCTION__);
+ mainboard_command.mode = PREPARE_EJECT;
+ state_do_eject(arm_num, pump_num, mode);
+ return;
+ }
+
+ STMCH_DEBUG("%s sensor ok", __FUNCTION__);
+
+ /* by the way, prepare the other arm */
+ if (other_pump_num != -1)
+ arm_goto_prepare_get(other_arm_num, other_pump_num);
+
+ /* get the column */
+ arm_goto_get_column(arm_num, pump_num);
+
+ us = time_get_us2();
+ while (1) {
+ /* wait 50 ms */
+ if (time_get_us2() - us > 50*1000L)
+ break;
+ if (mode != HARVEST)
+ continue;
+ /* if we still see the front sensor, it's because
+ * there are 2 columns instead of one or because there
+ * is another column, so send the arm on other
+ * side. */
+ if (sensor_get(S_FRONT) && have_2cols == 0) {
+ STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__);
+ have_2cols = 1;
+ if (finger_get_side() == I2C_LEFT_SIDE)
+ state_finger_goto(mode, FINGER_RIGHT);
+ else
+ state_finger_goto(mode, FINGER_LEFT);
+ }
+ }
+
+ if (mode == HARVEST && have_2cols == 0) {
+ /* just release a bit of effort */
+ if (finger_get_side() == I2C_LEFT_SIDE) {
+ state_finger_goto(mode, FINGER_LEFT_RELAX);
+ }
+ else {
+ state_finger_goto(mode, FINGER_RIGHT_RELAX);
+ }
+ }
+ else if (mode == PICKUP) {
+ /* no free pump on other arm */
+ if (other_pump_num == -1) {
+ if (finger_get_side() == I2C_LEFT_SIDE) {
+ state_finger_goto(mode, FINGER_LEFT_RELAX);
+ }
+ else {
+ state_finger_goto(mode, FINGER_RIGHT_RELAX);
+ }
+ }
+ /* else send finger on the other side */
+ else {
+ if (finger_get_side() == I2C_LEFT_SIDE) {
+ state_finger_goto(mode, FINGER_RIGHT);
+ }
+ else {
+ state_finger_goto(mode, FINGER_LEFT);
+ }
+ }
+ }
+
+ us = time_get_us2();
+ while (1) {
+ /* wait 100 ms */
+ if (time_get_us2() - us > 100*1000L)
+ break;
+ if (mode != HARVEST)
+ continue;
+ /* if we still see the front sensor, it's because
+ * there are 2 columns instead of one or because there
+ * is another column, so send the arm on other
+ * side. */
+ if (sensor_get(S_FRONT) && have_2cols == 0) {
+ STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__);
+ have_2cols = 1;
+ if (finger_get_side() == I2C_LEFT_SIDE)
+ state_finger_goto(mode, FINGER_RIGHT);
+ else
+ state_finger_goto(mode, FINGER_LEFT);
+ }
+ }
+
+ /* consider the column as taken */
+ pump_mark_busy(pump_num);
+
+ state_debug_wait_key_pressed();
+
+ arm_goto_intermediate_get(arm_num, pump_num);
+ arm_wait_traj_end(arm, ARM_TRAJ_ALL_NEAR);
+
+ /* prepare next */
+ pump_num = arm_get_free_pump(arm_num);
+ if (pump_num == -1)
+ arm_goto_loaded(arm_num);
+ else
+ arm_goto_intermediate_get(arm_num, pump_num);
+
+ state_debug_wait_key_pressed();
+
+ /* switch to wait state */
+ if (get_free_pump_count() == 0) {
+ IRQ_LOCK(flags);
+ if (mainboard_command.mode == mode)
+ mainboard_command.mode = WAIT;
+ IRQ_UNLOCK(flags);
+ }
+
+ /* next pickup/harvest will be on the other side */
+ if (pickup_side == I2C_LEFT_SIDE)
+ pickup_side = I2C_RIGHT_SIDE;
+ else
+ pickup_side = I2C_LEFT_SIDE;
+}
+
+
+/* manual mode, arm position is sent from mainboard */
+static void state_do_manual(void)
+{
+ if (!state_check(MANUAL))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+ while (state_check(MANUAL));
+}
+
+/* wait mode */
+static void state_do_wait(void)
+{
+ if (!state_check(WAIT))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+ while (state_check(WAIT));
+}
+
+/* init mode */
+static void state_do_init(void)
+{
+ if (!state_check(INIT))
+ return;
+ state_init();
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+ while (state_check(INIT));
+}
+
+/* harvest columns elts from area */
+static void state_do_harvest(void)
+{
+ if (!state_check(HARVEST))
+ return;
+
+ if (get_free_pump_count() == 0) {
+ mainboard_command.mode = WAIT;
+ return;
+ }
+
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ state_pickup_or_harvest(HARVEST);
+}
+
+/* harvest columns elts from area without moving finger */
+static void state_do_lazy_harvest(void)
+{
+ if (!state_check(LAZY_HARVEST))
+ return;
+
+ if (get_free_pump_count() == 0) {
+ mainboard_command.mode = WAIT;
+ return;
+ }
+
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ state_pickup_or_harvest(LAZY_HARVEST);
+}
+
+/* eject a column. always called from pickup mode. */
+static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode)
+{
+ struct arm *arm;
+ arm = arm_num2ptr(arm_num);
+
+ if (finger_get_side() == I2C_LEFT_SIDE) {
+ state_finger_goto(old_mode, FINGER_LEFT_RELAX);
+ }
+ else {
+ state_finger_goto(old_mode, FINGER_RIGHT_RELAX);
+ }
+
+ /* wait mainboard to eject */
+ while (state_check(PREPARE_EJECT));
+
+ if (finger_get_side() == I2C_LEFT_SIDE) {
+ state_finger_goto(old_mode, FINGER_CENTER_LEFT);
+ }
+ else {
+ state_finger_goto(old_mode, FINGER_CENTER_RIGHT);
+ }
+
+ arm_goto_get_column(arm_num, pump_num);
+ arm_wait_traj_end(arm, ARM_TRAJ_ALL);
+ time_wait_ms(150);
+
+ state_debug_wait_key_pressed();
+
+ arm_goto_prepare_eject(arm_num, pump_num);
+ arm_wait_traj_end(arm, ARM_TRAJ_ALL);
+
+ state_debug_wait_key_pressed();
+
+ if (finger_get_side() == I2C_LEFT_SIDE) {
+ state_finger_goto(old_mode, FINGER_LEFT_RELAX);
+ }
+ else {
+ state_finger_goto(old_mode, FINGER_RIGHT_RELAX);
+ }
+
+ state_debug_wait_key_pressed();
+
+ time_wait_ms(300);
+ arm_goto_eject(arm_num, pump_num);
+ time_wait_ms(200);
+ pump_set(pump_num, PUMP_REVERSE);
+ arm_wait_traj_end(arm, ARM_TRAJ_ALL);
+
+ arm_goto_intermediate_get(arm_num, pump_num);
+ pump_set(pump_num, PUMP_OFF);
+}
+
+
+/* prepare pickup in a dispenser, or harvest */
+static void state_do_prepare_pickup(void)
+{
+ uint8_t left_count = 0, right_count = 0;
+ int8_t pump_l, pump_r;
+
+ if (!state_check(PREPARE_PICKUP))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ pump_check_all();
+
+ pump_l = arm_get_free_pump(ARM_LEFT_NUM);
+ if (pump_l == -1) {
+ arm_goto_loaded(ARM_LEFT_NUM);
+ }
+ else {
+ arm_goto_intermediate_front_get(ARM_LEFT_NUM, pump_l);
+ }
+
+ pump_r = arm_get_free_pump(ARM_RIGHT_NUM);
+ if (pump_r == -1) {
+ arm_goto_loaded(ARM_RIGHT_NUM);
+ }
+ else {
+ arm_goto_intermediate_front_get(ARM_RIGHT_NUM, pump_r);
+ }
+
+ arm_wait_both(ARM_TRAJ_ALL);
+
+ if (pump_l != -1)
+ arm_goto_prepare_get(ARM_LEFT_NUM, pump_l);
+ if (pump_r != -1)
+ arm_goto_prepare_get(ARM_RIGHT_NUM, pump_r);
+
+ if (mainboard_command.prep_pickup.side == I2C_AUTO_SIDE) {
+ left_count += pump_is_busy(PUMP_LEFT1_NUM);
+ left_count += pump_is_busy(PUMP_LEFT2_NUM);
+ right_count += pump_is_busy(PUMP_RIGHT1_NUM);
+ right_count += pump_is_busy(PUMP_RIGHT2_NUM);
+ if (left_count < right_count)
+ finger_goto(FINGER_RIGHT);
+ else
+ finger_goto(FINGER_LEFT);
+ }
+ else if (mainboard_command.prep_pickup.side == I2C_LEFT_SIDE)
+ finger_goto(FINGER_LEFT);
+ else if (mainboard_command.prep_pickup.side == I2C_RIGHT_SIDE)
+ finger_goto(FINGER_RIGHT);
+ else if (mainboard_command.prep_pickup.side == I2C_CENTER_SIDE)
+ finger_goto(FINGER_CENTER_LEFT);
+
+ /* try to know on which side we have to pickup */
+ if (finger_get_side() == I2C_RIGHT_SIDE) {
+ pickup_side = I2C_LEFT_SIDE;
+ }
+ else {
+ pickup_side = I2C_RIGHT_SIDE;
+ }
+
+ arm_prepare_free_pumps();
+
+ mainboard_command.mode = mainboard_command.prep_pickup.next_mode;
+
+ while (state_check(PREPARE_PICKUP));
+}
+
+/* clear pickup zone, will switch to harvest if needed */
+static void state_do_clear(void)
+{
+ uint8_t flags, err;
+
+ if (!state_check(CLEAR))
+ return;
+
+ if (get_free_pump_count() == 0) {
+ mainboard_command.mode = WAIT;
+ return;
+ }
+
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ finger_goto(FINGER_LEFT);
+ err = WAIT_COND_OR_TIMEOUT(sensor_get(S_LEFT), 500);
+ if (err) {
+ IRQ_LOCK(flags);
+ if (mainboard_command.mode == CLEAR)
+ mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
+ IRQ_UNLOCK(flags);
+ pickup_side = I2C_LEFT_SIDE;
+ return;
+ }
+
+ finger_goto(FINGER_RIGHT);
+ err = WAIT_COND_OR_TIMEOUT(sensor_get(S_RIGHT), 500);
+ if (err) {
+ IRQ_LOCK(flags);
+ if (mainboard_command.mode == CLEAR)
+ mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
+ IRQ_UNLOCK(flags);
+ pickup_side = I2C_RIGHT_SIDE;
+ return;
+ }
+
+ IRQ_LOCK(flags);
+ if (mainboard_command.mode == CLEAR)
+ mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
+ IRQ_UNLOCK(flags);
+}
+
+/* do a lazy pickup */
+static void state_do_lazy_pickup(void)
+{
+ int8_t flags, arm_num, pump_num;
+ uint32_t us;
+
+ if (!state_check(LAZY_PICKUP))
+ return;
+
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ if (arm_get_sensor(ARM_LEFT_NUM) &&
+ arm_get_sensor(ARM_RIGHT_NUM)) {
+ IRQ_LOCK(flags);
+ if (mainboard_command.mode == LAZY_PICKUP) {
+ mainboard_command.mode = WAIT;
+ }
+ IRQ_UNLOCK(flags);
+ return;
+ }
+
+ if (finger_get_side() == I2C_RIGHT_SIDE) {
+ finger_goto(FINGER_LEFT);
+ arm_num = ARM_LEFT_NUM;
+ }
+ else {
+ finger_goto(FINGER_RIGHT);
+ arm_num = ARM_RIGHT_NUM;
+ }
+
+ us = time_get_us2();
+ while(1) {
+ if (state_check(LAZY_PICKUP) == 0)
+ return;
+ if (arm_get_sensor(arm_num))
+ break;
+ if (time_get_us2() - us > 500*1000L) {
+ if (finger_get_side() == I2C_RIGHT_SIDE)
+ finger_goto(FINGER_LEFT);
+ else
+ finger_goto(FINGER_RIGHT);
+ return;
+ }
+ }
+
+ if (arm_get_color_sensor(arm_num) == -1) {
+ pump_num = arm_get_free_pump(arm_num);
+ if (pump_num == -1)
+ return; /* XXX */
+ pump_set(pump_num, PUMP_ON);
+ STMCH_DEBUG("%s prepare eject", __FUNCTION__);
+ mainboard_command.mode = PREPARE_EJECT;
+ state_do_eject(arm_num, pump_num, LAZY_PICKUP);
+ }
+}
+
+/* pickup from a dispenser automatically */
+static void state_do_pickup(void)
+{
+ if (!state_check(PICKUP))
+ return;
+
+ if (get_free_pump_count() == 0) {
+ mainboard_command.mode = WAIT;
+ return;
+ }
+
+ /* XXX check that finger is at correct place */
+
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ state_pickup_or_harvest(PICKUP);
+}
+
+/* store columns without using arms */
+static void state_do_store(void)
+{
+ int8_t arm_num;
+ int8_t other_arm_num;
+ microseconds us;
+
+ if (!state_check(STORE))
+ return;
+
+ if (get_free_pump_count() == 0) {
+ mainboard_command.mode = WAIT;
+ return;
+ }
+
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ /* get arm num */
+ if (pickup_side == I2C_LEFT_SIDE) {
+ arm_num = ARM_LEFT_NUM;
+ other_arm_num = ARM_RIGHT_NUM;
+ }
+ else {
+ arm_num = ARM_RIGHT_NUM;
+ other_arm_num = ARM_LEFT_NUM;
+ }
+
+ while (1) {
+ if (sensor_get(S_FRONT))
+ break;
+ if (state_check(STORE) == 0)
+ return;
+ }
+
+ /* when ready, move finger */
+ if (arm_num == ARM_RIGHT_NUM)
+ finger_goto(FINGER_RIGHT);
+ else
+ finger_goto(FINGER_LEFT);
+
+ /* wait to see the column on the sensor */
+ us = time_get_us2();
+ while (1) {
+ if (arm_get_sensor(arm_num))
+ break;
+ if (state_check(STORE) == 0)
+ return;
+ /* 500ms timeout in harvest, go back */
+ if (time_get_us2() - us > 500*1000L) {
+ STMCH_DEBUG("%s timeout", __FUNCTION__);
+
+ if (arm_num == ARM_RIGHT_NUM)
+ finger_goto(FINGER_LEFT);
+ else
+ finger_goto(FINGER_RIGHT);
+ return;
+ }
+ }
+
+ if (arm_get_sensor(arm_num) && arm_get_sensor(other_arm_num)) {
+ STMCH_DEBUG("%s full", __FUNCTION__);
+ while (state_check(STORE));
+ return;
+ }
+
+ /* next store will be on the other side */
+ if (pickup_side == I2C_LEFT_SIDE)
+ pickup_side = I2C_RIGHT_SIDE;
+ else
+ pickup_side = I2C_LEFT_SIDE;
+}
+
+/* prepare the building of a temple */
+static void state_do_prepare_build(void)
+{
+ int8_t pump_num, level;
+ if (!state_check(PREPARE_BUILD))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ pump_check_all();
+
+ if (finger_get_side() == I2C_LEFT_SIDE)
+ finger_goto(FINGER_LEFT);
+ else
+ finger_goto(FINGER_RIGHT);
+
+ pump_num = arm_get_busy_pump(ARM_LEFT_NUM);
+ level = mainboard_command.prep_build.level_l;
+ if (pump_num != -1 && level != -1)
+ arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_num,
+ level, I2C_AUTOBUILD_DEFAULT_DIST);
+
+ pump_num = arm_get_busy_pump(ARM_RIGHT_NUM);
+ level = mainboard_command.prep_build.level_r;
+ if (pump_num != -1 && level != -1)
+ arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_num,
+ level, I2C_AUTOBUILD_DEFAULT_DIST);
+
+ while (state_check(PREPARE_BUILD));
+}
+
+/* prepare the building of a temple */
+static void state_do_push_temple(void)
+{
+ uint8_t level;
+
+ level = mainboard_command.push_temple.level;
+
+ if (!state_check(PUSH_TEMPLE))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ if (finger_get_side() == I2C_LEFT_SIDE)
+ finger_goto(FINGER_LEFT);
+ else
+ finger_goto(FINGER_RIGHT);
+
+ arm_goto_prepare_push_temple(ARM_LEFT_NUM);
+ arm_goto_prepare_push_temple(ARM_RIGHT_NUM);
+ arm_wait_both(ARM_TRAJ_ALL);
+
+ arm_goto_push_temple(ARM_LEFT_NUM, level);
+ arm_goto_push_temple(ARM_RIGHT_NUM, level);
+
+ while (state_check(PUSH_TEMPLE));
+}
+
+/* prepare the building of a temple */
+static void state_do_push_temple_disc(void)
+{
+ uint8_t side;
+ struct arm *arm;
+
+ if (!state_check(PUSH_TEMPLE_DISC))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ side = mainboard_command.push_temple_disc.side;
+
+ if (side == I2C_LEFT_SIDE) {
+ arm = arm_num2ptr(ARM_LEFT_NUM);
+ arm_goto_prepare_push_temple_disc(ARM_LEFT_NUM);
+ arm_wait_traj_end(arm, ARM_TRAJ_ALL);
+ arm_goto_push_temple_disc(ARM_LEFT_NUM);
+ }
+ else {
+ arm = arm_num2ptr(ARM_RIGHT_NUM);
+ arm_goto_prepare_push_temple_disc(ARM_RIGHT_NUM);
+ arm_wait_traj_end(arm, ARM_TRAJ_ALL);
+ arm_goto_push_temple_disc(ARM_RIGHT_NUM);
+ }
+
+ while (state_check(PUSH_TEMPLE_DISC));
+}
+
+/* prepare the building of a temple (mainly for columns) */
+static void state_do_prepare_inside(void)
+{
+ int8_t pump_num, level_l, level_r;
+ if (!state_check(PREPARE_INSIDE))
+ return;
+
+ level_l = mainboard_command.prep_inside.level_l;
+ level_r = mainboard_command.prep_inside.level_r;
+ STMCH_DEBUG("%s mode=%d level_l=%d, level_r=%d", __FUNCTION__,
+ state_get_mode(), level_l, level_r);
+
+ pump_check_all();
+
+ if (finger_get_side() == I2C_LEFT_SIDE)
+ finger_goto(FINGER_LEFT);
+ else
+ finger_goto(FINGER_RIGHT);
+
+ pump_num = arm_get_busy_pump(ARM_LEFT_NUM);
+ if (pump_num == -1)
+ pump_num = PUMP_LEFT1_NUM;
+ if (level_l != -1)
+ arm_goto_prepare_build_inside(ARM_LEFT_NUM, pump_num,
+ level_l);
+
+ pump_num = arm_get_busy_pump(ARM_RIGHT_NUM);
+ if (pump_num == -1)
+ pump_num = PUMP_RIGHT1_NUM;
+ if (level_r != -1)
+ arm_goto_prepare_build_inside(ARM_RIGHT_NUM, pump_num,
+ level_r);
+
+ while (state_check(PREPARE_INSIDE));
+}
+
+/* moving position */
+static void state_do_loaded(void)
+{
+ if (!state_check(LOADED))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ pump_check_all();
+
+ if (finger_get_side() == I2C_LEFT_SIDE)
+ finger_goto(FINGER_LEFT);
+ else
+ finger_goto(FINGER_RIGHT);
+
+ arm_goto_loaded(ARM_LEFT_NUM);
+ arm_goto_loaded(ARM_RIGHT_NUM);
+
+ while (state_check(LOADED));
+}
+
+static void state_do_build_lintel(uint8_t level)
+{
+ STMCH_DEBUG("%s() level=%d have_lintel=%d",
+ __FUNCTION__, level, mechboard.lintel_count);
+
+ servo_lintel_out();
+
+ arm_goto_prepare_get_lintel_inside1();
+ arm_wait_both(ARM_TRAJ_ALL);
+ state_debug_wait_key_pressed();
+
+ pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE);
+ arm_goto_prepare_get_lintel_inside2(mechboard.lintel_count);
+ arm_wait_both(ARM_TRAJ_ALL);
+ state_debug_wait_key_pressed();
+
+ arm_goto_get_lintel_inside(mechboard.lintel_count);
+ arm_wait_both(ARM_TRAJ_ALL);
+ state_debug_wait_key_pressed();
+
+ time_wait_ms(150);
+ arm_goto_prepare_build_lintel1();
+ arm_wait_both(ARM_TRAJ_ALL);
+ state_debug_wait_key_pressed();
+
+ arm_goto_prepare_build_lintel2(level);
+ arm_wait_both(ARM_TRAJ_ALL);
+ state_debug_wait_key_pressed();
+
+ arm_goto_prepare_build_lintel3(level);
+ arm_wait_both(ARM_TRAJ_ALL);
+ state_debug_wait_key_pressed();
+
+ if (mechboard.lintel_count == 1)
+ servo_lintel_1lin();
+ else
+ servo_lintel_2lin();
+
+ arm_goto_build_lintel(level);
+ arm_wait_both(ARM_TRAJ_ALL);
+ time_wait_ms(170);
+ pump_set(PUMP_LEFT1_NUM, PUMP_ON);
+ time_wait_ms(50); /* right arm a bit after */
+ pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
+ time_wait_ms(130);
+ pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
+
+ mechboard.lintel_count --;
+}
+
+/* Build one level of column. If pump_r or pump_l is -1, don't build
+ * with this arm. */
+static void state_do_build_column(uint8_t level_l, int8_t pump_l,
+ uint8_t dist_l,
+ uint8_t level_r, int8_t pump_r,
+ uint8_t dist_r)
+{
+ STMCH_DEBUG("%s() level_l=%d pump_l=%d level_r=%d pump_r=%d",
+ __FUNCTION__, level_l, pump_l, level_r, pump_r);
+
+ /* nothing to do */
+ if (pump_l == -1 && pump_r == -1)
+ return;
+
+ /* go above the selected level */
+ if (pump_l != -1)
+ arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_l, level_l, dist_l);
+ if (pump_r != -1)
+ arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_r, level_r, dist_r);
+ STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL),
+ arm_test_traj_end(&right_arm, ARM_TRAJ_ALL));
+ arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL);
+ STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL),
+ arm_test_traj_end(&right_arm, ARM_TRAJ_ALL));
+
+ state_debug_wait_key_pressed();
+
+ /* drop columns of P2 */
+ if (pump_l != -1)
+ arm_goto_autobuild(ARM_LEFT_NUM, pump_l, level_l, dist_l);
+ if (pump_r != -1)
+ arm_goto_autobuild(ARM_RIGHT_NUM, pump_r, level_r, dist_r);
+ arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL);
+
+ state_debug_wait_key_pressed();
+
+ time_wait_ms(150);
+ if (pump_l != -1)
+ pump_set(pump_l, PUMP_REVERSE);
+ if (pump_r != -1)
+ pump_set(pump_r, PUMP_REVERSE);
+ time_wait_ms(150);
+ if (pump_l != -1) {
+ pump_set(pump_l, PUMP_OFF);
+ pump_mark_free(pump_l);
+ }
+ if (pump_r != -1) {
+ pump_set(pump_r, PUMP_OFF);
+ pump_mark_free(pump_r);
+ }
+
+ state_debug_wait_key_pressed();
+}
+
+/* autobuild columns elts from area */
+/* check level to avoid bad things ? */
+/* check if enough cols ? */
+static void state_do_autobuild(void)
+{
+ int8_t pump_l, pump_r;
+ /* copy command into local data */
+ int8_t level_l = mainboard_command.autobuild.level_left;
+ int8_t level_r = mainboard_command.autobuild.level_right;
+ uint8_t count_l = mainboard_command.autobuild.count_left;
+ uint8_t count_r = mainboard_command.autobuild.count_right;
+ uint8_t dist_l = mainboard_command.autobuild.distance_left;
+ uint8_t dist_r = mainboard_command.autobuild.distance_right;
+ uint8_t do_lintel = mainboard_command.autobuild.do_lintel;
+ int8_t max_level = level_l;
+
+
+ if (!state_check(AUTOBUILD))
+ return;
+
+ STMCH_DEBUG("%s mode=%d do_lintel=%d", __FUNCTION__,
+ state_get_mode(), do_lintel);
+ STMCH_DEBUG(" left: level=%d count=%d", level_l, count_l);
+ STMCH_DEBUG(" right: level=%d count=%d", level_r, count_r);
+
+ /*
+ * build the first level of column if needed
+ */
+
+ /* don't build with this arm if no pump or if we don't ask to */
+ pump_l = arm_get_busy_pump(ARM_LEFT_NUM);
+ if (count_l == 0)
+ pump_l = -1;
+ pump_r = arm_get_busy_pump(ARM_RIGHT_NUM);
+ if (count_r == 0)
+ pump_r = -1;
+
+ if (pump_l == -1 && pump_r == -1)
+ goto lintel_only;
+
+ state_do_build_column(level_l, pump_l, dist_l,
+ level_r, pump_r, dist_r);
+
+ /* one level up */
+ if (pump_l != -1) {
+ count_l --;
+ level_l ++;
+ max_level = level_l;
+ }
+ if (pump_r != -1) {
+ count_r --;
+ level_r ++;
+ if (level_r > max_level)
+ max_level = level_r;
+ }
+
+ /*
+ * build the second level of column if needed
+ */
+
+ /* don't build with this arm if no pump or if we don't ask to */
+ pump_l = arm_get_busy_pump(ARM_LEFT_NUM);
+ if (count_l == 0)
+ pump_l = -1;
+ pump_r = arm_get_busy_pump(ARM_RIGHT_NUM);
+ if (count_r == 0)
+ pump_r = -1;
+
+ state_do_build_column(level_l, pump_l, dist_l,
+ level_r, pump_r, dist_r);
+
+ /* one level up */
+ if (pump_l != -1) {
+ count_l --;
+ level_l ++;
+ max_level = level_l;
+ }
+ if (pump_r != -1) {
+ count_r --;
+ level_r ++;
+ if (level_r > max_level)
+ max_level = level_r;
+ }
+
+ state_debug_wait_key_pressed();
+
+ if (mechboard.lintel_count != 0 && do_lintel != 0) {
+ arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM,
+ PUMP_LEFT1_NUM,
+ max_level,
+ I2C_AUTOBUILD_DEFAULT_DIST);
+ arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM,
+ PUMP_RIGHT1_NUM,
+ max_level,
+ I2C_AUTOBUILD_DEFAULT_DIST);
+ arm_wait_both(ARM_TRAJ_ALL_NEAR);
+ state_debug_wait_key_pressed();
+
+ arm_goto_prepare_autobuild_inside(ARM_LEFT_NUM,
+ PUMP_LEFT1_NUM,
+ max_level);
+ arm_goto_prepare_autobuild_inside(ARM_RIGHT_NUM,
+ PUMP_RIGHT1_NUM,
+ max_level);
+ arm_wait_both(ARM_TRAJ_ALL_NEAR);
+ state_debug_wait_key_pressed();
+ }
+
+ lintel_only:
+ if (mechboard.lintel_count == 0 || do_lintel == 0) {
+ mainboard_command.mode = WAIT;
+ return;
+ }
+
+ state_do_build_lintel(max_level);
+ mainboard_command.mode = WAIT;
+}
+
+/* prepare to get the lintel */
+static void state_do_prepare_get_lintel(void)
+{
+ if (!state_check(PREPARE_GET_LINTEL))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ arm_goto_prepare_get_lintel_disp();
+ arm_wait_both(ARM_TRAJ_ALL);
+
+ pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
+
+ /* go fully left or right */
+ if (finger_get_side() == I2C_LEFT_SIDE)
+ finger_goto(FINGER_LEFT);
+ else
+ finger_goto(FINGER_RIGHT);
+
+ while (state_check(PREPARE_GET_LINTEL));
+}
+
+/* get the lintel from the dispenser */
+static void state_do_get_lintel(void)
+{
+ if (!state_check(GET_LINTEL))
+ return;
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+
+ pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE);
+
+ arm_goto_get_lintel_disp();
+ arm_wait_both(ARM_TRAJ_ALL_NEAR);
+
+ time_wait_ms(200);
+
+ STMCH_DEBUG("%s left1=%d left2=%d", __FUNCTION__,
+ mechboard.pump_left1_current,
+ sensor_get_adc(ADC_CSENSE3));
+
+ while (state_check(GET_LINTEL));
+
+ /* mainboard asked to release lintel, so release pump first */
+ if (state_get_mode() == PREPARE_GET_LINTEL) {
+ pump_set(PUMP_LEFT1_NUM, PUMP_ON);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
+ time_wait_ms(200);
+ pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
+ }
+}
+
+/* put the lintel inside the robot */
+static void state_do_put_lintel(void)
+{
+ uint8_t prev_lin_count;
+
+ if (!state_check(PUT_LINTEL))
+ return;
+
+ STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
+ prev_lin_count = mechboard.lintel_count;
+ mechboard.lintel_count ++;
+
+ arm_goto_prepare_get_lintel_disp();
+ arm_wait_both(ARM_TRAJ_ALL);
+
+ servo_lintel_out();
+
+ arm_goto_prepare_put_lintel();
+ arm_wait_both(ARM_TRAJ_ALL_NEAR);
+
+ arm_goto_put_lintel(prev_lin_count);
+ arm_wait_both(ARM_TRAJ_ALL);
+
+ pump_set(PUMP_LEFT1_NUM, PUMP_ON);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
+
+ if (mechboard.lintel_count == 1)
+ servo_lintel_1lin();
+ else
+ servo_lintel_2lin();
+
+ time_wait_ms(300);
+
+ pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
+ pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
+
+ arm_goto_prepare_put_lintel();
+ arm_wait_both(ARM_TRAJ_ALL_NEAR);
+
+ while (state_check(PUT_LINTEL));
+}
+
+/* main state machine */
+void state_machine(void)
+{
+ while (state_get_mode() != EXIT) {
+ changed = 0;
+ state_do_init();
+ state_do_manual();
+ state_do_harvest();
+ state_do_lazy_harvest();
+ state_do_prepare_pickup();
+ state_do_pickup();
+ state_do_prepare_inside();
+ state_do_prepare_build();
+ state_do_autobuild();
+ state_do_prepare_get_lintel();
+ state_do_get_lintel();
+ state_do_put_lintel();
+ state_do_loaded();
+ state_do_clear();
+ state_do_lazy_pickup();
+ state_do_wait();
+ state_do_store();
+ state_do_manivelle();
+ state_do_push_temple();
+ state_do_push_temple_disc();
+ }
+}
+
+void state_init(void)
+{
+ vt100_init(&local_vt100);
+ mainboard_command.mode = WAIT;
+ pump_reset_all();
+ mechboard.lintel_count = 1;
+ mechboard.column_flags = 0;
+ servo_lintel_1lin();
+ finger_goto(FINGER_LEFT);
+}
git.droids-corp.org / aversive.git / blobdiff