X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fstate.c;h=cbbc8293b5d28ffeb83d3afaf095711cd2799fab;hp=8e8affc60c131f03469b39e386744e73b2f307c0;hb=ba4ad67d2bd3457a12615439a7e2f748cfb7bcde;hpb=c5092ef0e45256f7e5ff2ceea8e1aa1cb33027db diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 8e8affc..cbbc829 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -57,28 +57,18 @@ #define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args) static struct vt100 local_vt100; -static volatile uint8_t state_mode; +static volatile uint8_t state_mode, lspickle, rspickle; +static volatile uint8_t state_status; +static volatile uint8_t state_i2c_ignore = 0; static uint8_t cob_count; /* short aliases */ -#define L_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_L_DEPLOY)) -#define R_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_R_DEPLOY)) -#define DEPLOY(side, mode) ((side) == I2C_LEFT_SIDE ? L_DEPLOY(mode) : R_DEPLOY(mode)) -#define L_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_L_HARVEST)) -#define R_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_R_HARVEST)) -#define HARVEST(side, mode) ((side) == I2C_LEFT_SIDE ? L_HARVEST(mode) : R_HARVEST(mode)) -#define EJECT(mode) (!!((mode) & I2C_COBBOARD_MODE_EJECT)) -#define INIT(mode) (!!((mode) & I2C_COBBOARD_MODE_INIT)) +#define INIT(mode) ((mode) == I2C_COBBOARD_MODE_INIT) +#define HARVEST(mode) ((mode) == I2C_COBBOARD_MODE_HARVEST) +#define EJECT(mode) ((mode) == I2C_COBBOARD_MODE_EJECT) uint8_t state_debug = 0; -#if 0 -static void state_dump_sensors(void) -{ - STMCH_DEBUG("TODO\n"); -} -#endif - uint8_t state_get_cob_count(void) { return cob_count; @@ -92,39 +82,93 @@ static void state_debug_wait_key_pressed(void) while(!cmdline_keypressed()); } -/* return true if cob is present */ -static uint8_t state_cob_present(uint8_t side) +/* return true if the cob is correctly inside */ +static uint8_t state_cob_inside(void) +{ + return sensor_get(S_COB_INSIDE_L) && + sensor_get(S_COB_INSIDE_R); +} + +/* return true if there is no cob correctly inside */ +static uint8_t state_no_cob_inside(void) +{ + return !sensor_get(S_COB_INSIDE_L) && + !sensor_get(S_COB_INSIDE_R); +} + +static uint8_t state_spicklemode_deployed(uint8_t side) { if (side == I2C_LEFT_SIDE) - return sensor_get(S_LCOB); + return lspickle & I2C_COBBOARD_SPK_DEPLOY; else - /* XXX */ - // return sensor_get(S_LCOB); - return 0; + return rspickle & I2C_COBBOARD_SPK_DEPLOY; } -/* return the detected color of the cob (only valid if present) */ -static uint8_t state_cob_color(uint8_t side) +static uint8_t state_spicklemode_autoharvest(uint8_t side) { if (side == I2C_LEFT_SIDE) - return I2C_COB_WHITE; + return lspickle & I2C_COBBOARD_SPK_AUTOHARVEST; else - /* XXX */ - // return sensor_get(S_LCOB); - return 0; + return rspickle & I2C_COBBOARD_SPK_AUTOHARVEST; +} + +static uint8_t state_spicklemode_nomove(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + return lspickle & I2C_COBBOARD_SPK_NO_MOVE; + else + return rspickle & I2C_COBBOARD_SPK_NO_MOVE; +} + +/* pack/deploy spickles depending on mode */ +static void spickle_prepare(uint8_t side) +{ + /* we do nothing in mode no out */ + if (state_spicklemode_nomove(side)) + return; + + if (state_spicklemode_deployed(side)) + spickle_deploy(side); + else + spickle_pack(side); +} + +void state_set_spickle(uint8_t side, uint8_t flags) +{ + if (side == I2C_LEFT_SIDE) { + /* the PACK command preempts the current action if the + * arm is not busy */ + if (lspickle != 0 && flags == 0 && + state_status != I2C_COBBOARD_STATUS_LBUSY) + spickle_prepare(I2C_LEFT_SIDE); + lspickle = flags; + } + else { + /* the PACK command preempts the current action if the + * arm is not busy */ + if (rspickle != 0 && flags == 0 && + state_status != I2C_COBBOARD_STATUS_RBUSY) + spickle_prepare(I2C_RIGHT_SIDE); + rspickle = flags; + } } /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) { state_mode = mode; + STMCH_DEBUG("%s(): mode=%x", __FUNCTION__, mode); + return 0; +} -/* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */ -/* "r_deploy=%d r_harvest=%d eject=%d", */ -/* __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */ -/* R_DEPLOY(mode), R_HARVEST(mode), EJECT(mode)); */ +void state_set_i2c_ignore(uint8_t val) +{ + state_i2c_ignore = val; +} - return 0; +uint8_t state_get_i2c_ignore(void) +{ + return state_i2c_ignore; } /* check that state is the one in parameter and that state did not @@ -139,6 +183,7 @@ static uint8_t state_want_exit(void) return 0; if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) return 1; + printf_P(PSTR("CTRL-C\r\n")); return 0; } @@ -147,48 +192,128 @@ uint8_t state_get_mode(void) return state_mode; } +uint8_t state_get_status(void) +{ + return state_status; +} + /* harvest cobs from area */ static void state_do_harvest(uint8_t side) { - uint16_t delay; - /* if there is no cob, return */ - if (state_cob_present(side)) - return; - - /* if it is black, nothing to do */ - if (state_cob_color(side) == I2C_COB_BLACK) + if (cob_falling_edge(side) == 0) return; + if (side == I2C_LEFT_SIDE) + state_status = I2C_COBBOARD_STATUS_LBUSY; + else + state_status = I2C_COBBOARD_STATUS_RBUSY; + + STMCH_DEBUG("start"); + /* eat the cob */ spickle_pack(side); + + time_wait_ms(200); + cobroller_on(side); + + /* check that cob is correctly in place */ + if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) { + if (state_no_cob_inside()) { + STMCH_DEBUG("no cob"); + return; + } + STMCH_DEBUG("bad cob state"); + + /* while cob is not correctly placed try to extract + * it as much as we can */ + while (state_cob_inside() == 0 && + state_no_cob_inside() == 0) { + uint8_t cpt = 0; + if (cpt == 0) + cobroller_reverse(side); + else if (cpt == 1) + cobroller_off(side); + else if (cpt == 2) + cobroller_on(side); + cpt ++; + cpt %= 3; + shovel_mid(); + time_wait_ms(250); + shovel_down(); + time_wait_ms(250); + } + + STMCH_DEBUG("cob removed"); + if (state_no_cob_inside()) { + STMCH_DEBUG("no cob"); + return; + } + } + + /* cob is inside, switch off roller */ + cobroller_off(side); + cob_count ++; + state_debug_wait_key_pressed(); - delay = spickle_get_pack_delay(side); - time_wait_ms(delay); - /* redeploy the spickle */ - spickle_deploy(side); + /* last cob, nothing to do */ + if (cob_count == 5) + return; + + /* redeploy the spickle if there is a black cob */ + if (!state_spicklemode_nomove(side)) + spickle_deploy(side); state_debug_wait_key_pressed(); - cob_count ++; + /* let the loaded cobs go */ + servo_door_block(); + servo_carry_open(); + time_wait_ms(100); + state_debug_wait_key_pressed(); /* store it */ shovel_up(); - wait_ms(200); + + while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 600) == 0) { + STMCH_DEBUG("shovel blocked"); + shovel_down(); + time_wait_ms(250); + shovel_up(); + } + state_debug_wait_key_pressed(); - shovel_down(); + + /* close the carry servos */ + servo_carry_close(); + servo_door_close(); + time_wait_ms(200); state_debug_wait_key_pressed(); + + shovel_down(); + + while (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) { + STMCH_DEBUG("shovel blocked"); + shovel_up(); + time_wait_ms(250); + shovel_down(); + } + + STMCH_DEBUG("end"); } /* eject cobs */ static void state_do_eject(void) { + state_status = I2C_COBBOARD_STATUS_EJECT; cob_count = 0; shovel_mid(); + servo_carry_open(); servo_door_open(); time_wait_ms(2000); shovel_down(); servo_door_close(); + servo_carry_close(); } /* main state machine */ @@ -196,42 +321,37 @@ void state_machine(void) { while (state_want_exit() == 0) { + state_status = I2C_COBBOARD_STATUS_READY; + /* init */ if (INIT(state_mode)) { - state_mode &= (~I2C_COBBOARD_MODE_INIT); state_init(); + state_mode = I2C_COBBOARD_MODE_HARVEST; } /* pack/deply spickles, enable/disable roller */ - if (L_DEPLOY(state_mode)) { - spickle_deploy(I2C_LEFT_SIDE); - left_cobroller_on(); - } - else { - spickle_pack(I2C_LEFT_SIDE); - left_cobroller_off(); - } - if (R_DEPLOY(state_mode)) { - spickle_deploy(I2C_RIGHT_SIDE); - right_cobroller_on(); - } - else { - spickle_pack(I2C_RIGHT_SIDE); - right_cobroller_off(); - } + cobroller_off(I2C_LEFT_SIDE); + cobroller_off(I2C_RIGHT_SIDE); + spickle_prepare(I2C_LEFT_SIDE); + spickle_prepare(I2C_RIGHT_SIDE); /* harvest */ - if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) - state_do_harvest(I2C_LEFT_SIDE); - if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) - state_do_harvest(I2C_RIGHT_SIDE); + if (cob_count < 5) { + if (state_spicklemode_deployed(I2C_LEFT_SIDE) && + state_spicklemode_autoharvest(I2C_LEFT_SIDE)) + state_do_harvest(I2C_LEFT_SIDE); + if (state_spicklemode_deployed(I2C_RIGHT_SIDE) && + state_spicklemode_autoharvest(I2C_RIGHT_SIDE)) + state_do_harvest(I2C_RIGHT_SIDE); + } /* eject */ if (EJECT(state_mode)) { - state_mode &= (~I2C_COBBOARD_MODE_EJECT); + state_mode = I2C_COBBOARD_MODE_HARVEST; state_do_eject(); } } + state_status = I2C_COBBOARD_STATUS_OFF; } void state_init(void) @@ -239,8 +359,10 @@ void state_init(void) vt100_init(&local_vt100); shovel_down(); servo_door_close(); + servo_carry_close(); spickle_pack(I2C_LEFT_SIDE); spickle_pack(I2C_RIGHT_SIDE); state_mode = 0; cob_count = 0; + state_status = I2C_COBBOARD_STATUS_OFF; }