From d10eb76208e401ae2c4dc44de2f96cdcf2e7e4ac Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 24 Apr 2010 23:16:38 +0200 Subject: [PATCH] cobboard: better actions + i2c --- .../microb2010/cobboard/commands_cobboard.c | 6 ++- projects/microb2010/cobboard/cs.c | 8 ++-- projects/microb2010/cobboard/i2c_protocol.c | 6 ++- projects/microb2010/cobboard/shovel.c | 22 ++++++++++ projects/microb2010/cobboard/shovel.h | 21 ++------- projects/microb2010/cobboard/state.c | 44 +++++++++++-------- projects/microb2010/cobboard/state.h | 3 ++ 7 files changed, 67 insertions(+), 43 deletions(-) diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index dbbdd79..515f3d5 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -193,13 +193,17 @@ static void cmd_state1_parsed(void *parsed_result, state_init(); else if (!strcmp_P(res->arg1, PSTR("eject"))) state_set_mode(I2C_COBBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("ignore_i2c"))) + state_set_i2c_ignore(1); + else if (!strcmp_P(res->arg1, PSTR("care_i2c"))) + state_set_i2c_ignore(0); /* other commands */ } prog_char str_state1_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0); -prog_char str_state1_arg1[] = "init#eject"; +prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c"; parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); prog_char help_state1[] = "set cobboard mode"; diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index 0ed2f1a..7eb186e 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -78,9 +78,9 @@ static void do_cs(__attribute__((unused)) void *dummy) ls = (cobboard.left_spickle.prev - cobboard.left_spickle.cs.filtered_feedback_value); rs = (cobboard.right_spickle.prev - cobboard.right_spickle.cs.filtered_feedback_value); sh = (cobboard.shovel.prev - cobboard.shovel.cs.filtered_feedback_value); - if (ls < -2000 || ls > 2000 || - rs < -2000 || rs > 2000 || - sh < -2000 || sh > 2000) { + if (ls < -3000 || ls > 3000 || + rs < -3000 || rs > 3000 || + sh < -3000 || sh > 3000) { printf_P(PSTR("left_spickle %ld "), ls); printf_P(PSTR("right_spickle %ld "), rs); printf_P(PSTR("shovel %ld "), sh); @@ -196,7 +196,7 @@ void microb_cs_init(void) /* quadramp */ quadramp_init(&cobboard.shovel.qr); - quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2000, 2000); /* set speed */ + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500); /* set speed */ quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 20); /* set accel */ /* CS */ diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index b7f13ee..21a9bd8 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -135,7 +135,8 @@ void i2c_recvevent(uint8_t * buf, int8_t size) struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; if (size != sizeof(struct i2c_cmd_cobboard_set_mode)) goto error; - state_set_mode(cmd->mode); + if (!state_get_i2c_ignore()) + state_set_mode(cmd->mode); break; } @@ -168,7 +169,8 @@ void i2c_recvevent(uint8_t * buf, int8_t size) goto error; /* mode is in req */ - if (state_get_status() != I2C_COBBOARD_STATUS_OFF) { + if (state_get_status() != I2C_COBBOARD_STATUS_OFF && + !state_get_i2c_ignore()) { state_set_spickle(I2C_LEFT_SIDE, cmd->lspickle); state_set_spickle(I2C_RIGHT_SIDE, cmd->rspickle); } diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c index 6d71e58..abf0c42 100644 --- a/projects/microb2010/cobboard/shovel.c +++ b/projects/microb2010/cobboard/shovel.c @@ -42,6 +42,10 @@ #include "main.h" #include "shovel.h" +#define SHOVEL_DOWN 100 +#define SHOVEL_MID 4900 +#define SHOVEL_UP 10000 + /* init spickle position at beginning */ static void shovel_autopos(void) { @@ -64,6 +68,24 @@ static uint8_t shovel_is_at_pos(int32_t pos) return 0; } +void shovel_down(void) +{ + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 80); + cs_set_consign(&cobboard.shovel.cs, SHOVEL_DOWN); +} + +void shovel_mid(void) +{ + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 80, 80); + cs_set_consign(&cobboard.shovel.cs, SHOVEL_MID); +} + +void shovel_up(void) +{ + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 80, 20); + cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); +} + uint8_t shovel_is_up(void) { return shovel_is_at_pos(SHOVEL_UP); diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h index 241b065..80bceec 100644 --- a/projects/microb2010/cobboard/shovel.h +++ b/projects/microb2010/cobboard/shovel.h @@ -22,26 +22,11 @@ #ifndef _SHOVEL_H_ #define _SHOVEL_H_ -#define SHOVEL_DOWN 100 -#define SHOVEL_MID 4900 -#define SHOVEL_UP 10000 - void shovel_init(void); -static inline void shovel_down(void) -{ - cs_set_consign(&cobboard.shovel.cs, SHOVEL_DOWN); -} - -static inline void shovel_mid(void) -{ - cs_set_consign(&cobboard.shovel.cs, SHOVEL_MID); -} - -static inline void shovel_up(void) -{ - cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); -} +void shovel_down(void); +void shovel_mid(void); +void shovel_up(void); uint8_t shovel_is_up(void); uint8_t shovel_is_down(void); diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 7a1f047..df0e4c6 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -59,12 +59,13 @@ static struct vt100 local_vt100; 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 HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_HARVEST)) -#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; @@ -127,14 +128,16 @@ static void spickle_prepare(uint8_t side) void state_set_spickle(uint8_t side, uint8_t flags) { if (side == I2C_LEFT_SIDE) { - /* preempt current action if not busy */ + /* 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 { - /* preempt current action if not busy */ + /* 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); @@ -146,13 +149,18 @@ void state_set_spickle(uint8_t side, uint8_t flags) 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 @@ -184,15 +192,15 @@ uint8_t state_get_status(void) /* harvest cobs from area */ static void state_do_harvest(uint8_t side) { + /* if there is no cob, return */ + 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; - /* if there is no cob, return */ - if (cob_falling_edge(side) == 0) - return; - STMCH_DEBUG("start"); /* eat the cob */ @@ -306,7 +314,7 @@ void state_machine(void) /* init */ if (INIT(state_mode)) { - state_mode &= (~I2C_COBBOARD_MODE_INIT); + state_mode = I2C_COBBOARD_MODE_HARVEST; state_init(); } @@ -319,16 +327,16 @@ void state_machine(void) /* harvest */ if (cob_count < 5) { if ((lspickle & I2C_COBBOARD_SPK_DEPLOY) && - (lspickle & I2C_COBBOARD_SPK_DEPLOY)) + (lspickle & I2C_COBBOARD_SPK_AUTOHARVEST)) state_do_harvest(I2C_LEFT_SIDE); if ((rspickle & I2C_COBBOARD_SPK_DEPLOY) && - (rspickle & I2C_COBBOARD_SPK_DEPLOY)) + (rspickle & I2C_COBBOARD_SPK_AUTOHARVEST)) 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(); } } diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h index 141b643..f3c90ed 100644 --- a/projects/microb2010/cobboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -30,6 +30,9 @@ void state_set_spickle(uint8_t side, uint8_t flags); uint8_t state_get_mode(void); uint8_t state_get_status(void); +void state_set_i2c_ignore(uint8_t val); +uint8_t state_get_i2c_ignore(void); + uint8_t state_get_cob_count(void); /* launch state machine */ -- 2.39.5