X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fstate.c;h=9d082bacf7aabb9b606b654d00fd635bac4e0e12;hp=d02e54440a0aca434409f4748e1680fd5c1c6aad;hb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d;hpb=57b3e145f2ff46e1a7e7d522c85f3c56821dbf48 diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index d02e544..9d082ba 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 @@ -68,6 +68,7 @@ static uint8_t cob_count; #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)) uint8_t state_debug = 0; @@ -78,6 +79,11 @@ static void state_dump_sensors(void) } #endif +uint8_t state_get_cob_count(void) +{ + return cob_count; +} + static void state_debug_wait_key_pressed(void) { if (!state_debug) @@ -92,31 +98,43 @@ static uint8_t state_cob_present(uint8_t side) if (side == I2C_LEFT_SIDE) return sensor_get(S_LCOB); else - /* XXX */ - // return sensor_get(S_LCOB); - return 0; + return sensor_get(S_RCOB); } /* return the detected color of the cob (only valid if present) */ static uint8_t state_cob_color(uint8_t side) { + /* XXX no color sensor for now */ if (side == I2C_LEFT_SIDE) return I2C_COB_WHITE; else - /* XXX */ - // return sensor_get(S_LCOB); - return 0; + return I2C_COB_WHITE; +} + +/* 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); } /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) { state_mode = mode; - /* XXX synchrounous pack here */ - 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)); + +/* 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)); */ + return 0; } @@ -132,6 +150,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; } @@ -143,34 +162,75 @@ uint8_t state_get_mode(void) /* 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) return; + STMCH_DEBUG("start"); + /* eat the cob */ spickle_pack(side); - state_debug_wait_key_pressed(); - delay = spickle_get_pack_delay(side); - time_wait_ms(delay); + + time_wait_ms(250); + cobroller_on(side); + + if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) { + if (state_no_cob_inside()) { + printf_P(PSTR("NO COB\r\n")); + return; + } + printf_P(PSTR("BAD COB - press a key\r\n")); + while(!cmdline_keypressed()); + } + + /* cob is inside, switch off roller */ + cobroller_off(side); + cob_count ++; + + /* last cob, nothing to do */ + if (cob_count == 5) + return; /* redeploy the spickle */ 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); + + if (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { + BRAKE_ON(); + printf_P(PSTR("BLOCKED\r\n")); + while(!cmdline_keypressed()); + } + 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(); + + if (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) { + BRAKE_ON(); + printf_P(PSTR("BLOCKED\r\n")); + while(!cmdline_keypressed()); + } + + STMCH_DEBUG("end"); } /* eject cobs */ @@ -178,8 +238,12 @@ static void state_do_eject(void) { 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 */ @@ -187,21 +251,47 @@ void state_machine(void) { while (state_want_exit() == 0) { - /* pack spickles */ - if (!L_DEPLOY(state_mode)) + /* init */ + if (INIT(state_mode)) { + state_mode &= (~I2C_COBBOARD_MODE_INIT); + state_init(); + } + + cobroller_off(I2C_LEFT_SIDE); + cobroller_off(I2C_RIGHT_SIDE); + + /* pack/deply spickles, enable/disable roller */ + if (cob_count >= 5) + spickle_pack(I2C_LEFT_SIDE); + else if (L_DEPLOY(state_mode) && !L_HARVEST(state_mode)) + spickle_mid(I2C_LEFT_SIDE); + else if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) + spickle_deploy(I2C_LEFT_SIDE); + else spickle_pack(I2C_LEFT_SIDE); - if (!R_DEPLOY(state_mode)) + + if (cob_count >= 5) + spickle_pack(I2C_RIGHT_SIDE); + else if (R_DEPLOY(state_mode) && !R_HARVEST(state_mode)) + spickle_mid(I2C_RIGHT_SIDE); + else if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) + spickle_deploy(I2C_RIGHT_SIDE); + else spickle_pack(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 (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); + } /* eject */ - if (EJECT(state_mode)) + if (EJECT(state_mode)) { + state_mode &= (~I2C_COBBOARD_MODE_EJECT); state_do_eject(); + } } } @@ -209,6 +299,8 @@ 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;