From 19cd41f7920628a962928580cd1618a1ee5750aa Mon Sep 17 00:00:00 2001 From: zer0 Date: Fri, 23 Apr 2010 23:26:52 +0200 Subject: [PATCH] i2c rework --- .../microb2010/cobboard/commands_cobboard.c | 32 +++------ projects/microb2010/cobboard/i2c_protocol.c | 12 ++-- projects/microb2010/cobboard/state.c | 63 +++++++++++----- projects/microb2010/cobboard/state.h | 1 + projects/microb2010/common/i2c_commands.h | 26 +++---- .../microb2010/mainboard/commands_mainboard.c | 24 ++++--- projects/microb2010/mainboard/i2c_protocol.c | 71 +++++-------------- projects/microb2010/mainboard/i2c_protocol.h | 10 ++- projects/microb2010/mainboard/main.h | 2 + projects/microb2010/mainboard/strat.c | 11 +-- 10 files changed, 120 insertions(+), 132 deletions(-) diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index bea27e9..dbbdd79 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -229,38 +229,26 @@ static void cmd_state2_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state2_result *res = parsed_result; - uint8_t side, mode = state_get_mode(); + uint8_t side; - if (!strcmp_P(res->arg2, PSTR("left"))) { + if (!strcmp_P(res->arg2, PSTR("left"))) side = I2C_LEFT_SIDE; - mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); - } - else { + else side = I2C_RIGHT_SIDE; - mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - } if (!strcmp_P(res->arg1, PSTR("pack"))) { - /* nothing to do */ + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, 0); } else if (!strcmp_P(res->arg1, PSTR("deploy"))) { - if (side == I2C_LEFT_SIDE) - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - else - mode |= I2C_COBBOARD_MODE_R_DEPLOY; + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY); } else if (!strcmp_P(res->arg1, PSTR("harvest"))) { - if (side == I2C_LEFT_SIDE) { - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - mode |= I2C_COBBOARD_MODE_L_HARVEST; - } - else { - mode |= I2C_COBBOARD_MODE_R_DEPLOY; - mode |= I2C_COBBOARD_MODE_R_HARVEST; - } + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY | + I2C_COBBOARD_SPK_AUTOHARVEST); } - - state_set_mode(mode); } prog_char str_state2_arg0[] = "cobboard"; diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index 9b2a9e7..b7f13ee 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -130,16 +130,14 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } -#if 0 case I2C_CMD_COBBOARD_SET_MODE: { struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; if (size != sizeof(struct i2c_cmd_cobboard_set_mode)) goto error; - i2c_set_mode(cmd); + state_set_mode(cmd->mode); break; } -#endif case I2C_CMD_GENERIC_SET_COLOR: { @@ -151,7 +149,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) } #ifdef notyet - case I2C_CMD_EXTENSION_TEST: + case I2C_CMD_EXTENSION_TEST: { struct i2c_cmd_extension_test *cmd = void_cmd; if (size != sizeof (*cmd)) @@ -163,7 +161,6 @@ void i2c_recvevent(uint8_t * buf, int8_t size) /* Add other commands here ...*/ - case I2C_REQ_COBBOARD_STATUS: { struct i2c_req_cobboard_status *cmd = void_cmd; @@ -171,7 +168,10 @@ void i2c_recvevent(uint8_t * buf, int8_t size) goto error; /* mode is in req */ - state_set_mode(cmd->mode); + if (state_get_status() != I2C_COBBOARD_STATUS_OFF) { + state_set_spickle(I2C_LEFT_SIDE, cmd->lspickle); + state_set_spickle(I2C_RIGHT_SIDE, cmd->rspickle); + } i2c_send_status(); break; } diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index d310168..7a1f047 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -57,17 +57,12 @@ #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 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 HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_HARVEST)) #define EJECT(mode) (!!((mode) & I2C_COBBOARD_MODE_EJECT)) #define INIT(mode) (!!((mode) & I2C_COBBOARD_MODE_INIT)) @@ -100,30 +95,58 @@ static uint8_t state_no_cob_inside(void) !sensor_get(S_COB_INSIDE_R); } +static uint8_t is_deployed(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + return lspickle & I2C_COBBOARD_SPK_DEPLOY; + else + return rspickle & I2C_COBBOARD_SPK_DEPLOY; +} + +static uint8_t is_autoharvest(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + return lspickle & I2C_COBBOARD_SPK_AUTOHARVEST; + else + return rspickle & I2C_COBBOARD_SPK_AUTOHARVEST; +} + /* pack/deploy spickles depending on mode */ static void spickle_prepare(uint8_t side) { if (cob_count >= 5) spickle_pack(side); - else if (DEPLOY(side, state_mode) && !HARVEST(side, state_mode)) + else if (is_deployed(side) && !is_autoharvest(side)) spickle_mid(side); - else if (DEPLOY(side, state_mode) && HARVEST(side, state_mode)) + else if (is_deployed(side) && is_autoharvest(side)) spickle_deploy(side); else spickle_pack(side); } +void state_set_spickle(uint8_t side, uint8_t flags) +{ + if (side == I2C_LEFT_SIDE) { + /* preempt current action if 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 */ + 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; - /* preempt current action if not busy */ - if (state_status != I2C_COBBOARD_STATUS_LBUSY) - spickle_prepare(I2C_LEFT_SIDE); - if (state_status != I2C_COBBOARD_STATUS_RBUSY) - spickle_prepare(I2C_RIGHT_SIDE); - /* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */ /* "r_deploy=%d r_harvest=%d eject=%d", */ /* __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */ @@ -295,9 +318,11 @@ void state_machine(void) /* harvest */ if (cob_count < 5) { - if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) + if ((lspickle & I2C_COBBOARD_SPK_DEPLOY) && + (lspickle & I2C_COBBOARD_SPK_DEPLOY)) state_do_harvest(I2C_LEFT_SIDE); - if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) + if ((rspickle & I2C_COBBOARD_SPK_DEPLOY) && + (rspickle & I2C_COBBOARD_SPK_DEPLOY)) state_do_harvest(I2C_RIGHT_SIDE); } @@ -307,7 +332,7 @@ void state_machine(void) state_do_eject(); } } - state_status = I2C_COBBOARD_STATUS_READY; + state_status = I2C_COBBOARD_STATUS_OFF; } void state_init(void) @@ -320,5 +345,5 @@ void state_init(void) spickle_pack(I2C_RIGHT_SIDE); state_mode = 0; cob_count = 0; - state_status = I2C_COBBOARD_STATUS_READY; + state_status = I2C_COBBOARD_STATUS_OFF; } diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h index d9f95fc..141b643 100644 --- a/projects/microb2010/cobboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -24,6 +24,7 @@ /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode); +void state_set_spickle(uint8_t side, uint8_t flags); /* get current state */ uint8_t state_get_mode(void); diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index c0ff8e1..7b04977 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -70,17 +70,13 @@ struct i2c_cmd_generic_color { #define I2C_CMD_COBBOARD_SET_MODE 0x02 -/* XXX disabled, use memory sync instead */ struct i2c_cmd_cobboard_set_mode { struct i2c_cmd_hdr hdr; -#define I2C_COBBOARD_MODE_L_DEPLOY 0x01 /* deploy the spickle */ -#define I2C_COBBOARD_MODE_L_HARVEST 0x02 /* auto harvest withe cobs */ -#define I2C_COBBOARD_MODE_R_DEPLOY 0x04 /* deploy the spickle */ -#define I2C_COBBOARD_MODE_R_HARVEST 0x08 /* auto harvest withe cobs */ -#define I2C_COBBOARD_MODE_EJECT 0x10 /* eject cobs */ -#define I2C_COBBOARD_MODE_INIT 0x20 /* init state machine */ - //uint8_t mode; +#define I2C_COBBOARD_MODE_HARVEST 0x01 /* harvest mode */ +#define I2C_COBBOARD_MODE_EJECT 0x02 /* eject cobs */ +#define I2C_COBBOARD_MODE_INIT 0x03 /* init state machine */ + uint8_t mode; }; #define I2C_CMD_BALLBOARD_SET_MODE 0x10 @@ -108,20 +104,26 @@ struct i2c_cmd_ballboard_set_mode { struct i2c_req_cobboard_status { struct i2c_cmd_hdr hdr; - uint8_t mode; + +#define I2C_COBBOARD_SPK_DEPLOY 0x01 /* deploy the spickle */ +#define I2C_COBBOARD_SPK_AUTOHARVEST 0x02 /* auto harvest the cobs */ + uint8_t lspickle; + uint8_t rspickle; }; #define I2C_ANS_COBBOARD_STATUS 0x81 struct i2c_ans_cobboard_status { struct i2c_cmd_hdr hdr; + /* mode type are defined above: I2C_COBBOARD_MODE_xxx */ uint8_t mode; #define I2C_COBBOARD_STATUS_READY 0x00 -#define I2C_COBBOARD_STATUS_LBUSY 0x01 -#define I2C_COBBOARD_STATUS_RBUSY 0x02 -#define I2C_COBBOARD_STATUS_EJECT 0x03 +#define I2C_COBBOARD_STATUS_OFF 0x01 +#define I2C_COBBOARD_STATUS_LBUSY 0x02 +#define I2C_COBBOARD_STATUS_RBUSY 0x03 +#define I2C_COBBOARD_STATUS_EJECT 0x04 uint8_t status; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index e552583..adec77f 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -730,9 +730,9 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_cobboard_mode_init(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) - i2c_cobboard_mode_eject(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -763,7 +763,7 @@ struct cmd_cobboard_setmode2_result { }; /* function called when cmd_cobboard_setmode2 is parsed successfully */ -static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) +static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data) { struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -773,12 +773,18 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) else if (!strcmp_P(res->arg2, PSTR("right"))) side = I2C_RIGHT_SIDE; - if (!strcmp_P(res->arg1, PSTR("deploy"))) - i2c_cobboard_mode_deploy(side); - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - i2c_cobboard_mode_harvest(side); - else if (!strcmp_P(res->arg1, PSTR("pack"))) - i2c_cobboard_mode_pack(side); + if (!strcmp_P(res->arg1, PSTR("deploy"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_deploy(side); + } + else if (!strcmp_P(res->arg1, PSTR("harvest"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_harvest(side); + } + else if (!strcmp_P(res->arg1, PSTR("pack"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_pack(side); + } } prog_char str_cobboard_setmode2_arg0[] = "cobboard"; diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index aed92c7..6997139 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -272,7 +272,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) goto error; /* status */ - //cobboard.mode = ans->mode; + cobboard.mode = ans->mode; cobboard.status = ans->status; cobboard.left_cobroller_speed = ans->left_cobroller_speed; cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed); @@ -354,7 +354,8 @@ static int8_t i2c_req_cobboard_status(void) int8_t err; buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS; - buf.mode = cobboard.mode; + buf.lspickle = cobboard.lspickle; + buf.rspickle = cobboard.rspickle; err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); @@ -391,78 +392,42 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state) return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf)); } -static int8_t i2c_cobboard_set_mode(uint8_t mode) +int8_t i2c_cobboard_set_mode(uint8_t mode) { #ifdef HOST_VERSION return robotsim_i2c_cobboard_set_mode(mode); #else - cobboard.mode = mode; - return 0; -#endif - -#if 0 /* old */ struct i2c_cmd_cobboard_set_mode buf; buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; - buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; + buf.mode = mode; return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); #endif } -int8_t i2c_cobboard_mode_eject(void) +static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags) { - /* XXXXXXXXX bad bad bad */ - uint8_t mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; - i2c_cobboard_set_mode(mode); - time_wait_ms(500); - mode = cobboard.mode & (~I2C_COBBOARD_MODE_EJECT); - i2c_cobboard_set_mode(mode); + if (side == I2C_LEFT_SIDE) + cobboard.lspickle = flags; + else + cobboard.rspickle = flags; return 0; } -int8_t i2c_cobboard_mode_harvest(uint8_t side) -{ - uint8_t mode = cobboard.mode; - - if (side == I2C_LEFT_SIDE) { - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - mode |= I2C_COBBOARD_MODE_L_HARVEST; - } - else { - mode |= I2C_COBBOARD_MODE_R_DEPLOY; - mode |= I2C_COBBOARD_MODE_R_HARVEST; - } - return i2c_cobboard_set_mode(mode); -} - -int8_t i2c_cobboard_mode_deploy(uint8_t side) +int8_t i2c_cobboard_pack(uint8_t side) { - uint8_t mode = cobboard.mode; - - if (side == I2C_LEFT_SIDE) { - mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - } - else { - mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - mode |= I2C_COBBOARD_MODE_R_DEPLOY; - } - return i2c_cobboard_set_mode(mode); + return i2c_cobboard_set_spickle(side, 0); } -int8_t i2c_cobboard_mode_pack(uint8_t side) +int8_t i2c_cobboard_harvest(uint8_t side) { - uint8_t mode = cobboard.mode; - - if (side == I2C_LEFT_SIDE) - mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); - else - mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - return i2c_cobboard_set_mode(mode); + return i2c_cobboard_set_spickle(side, + I2C_COBBOARD_SPK_DEPLOY | + I2C_COBBOARD_SPK_AUTOHARVEST); } -int8_t i2c_cobboard_mode_init(void) +int8_t i2c_cobboard_deploy(uint8_t side) { - return i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); + return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY); } int8_t i2c_ballboard_set_mode(uint8_t mode) diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h index 7ae7283..20c8ada 100644 --- a/projects/microb2010/mainboard/i2c_protocol.h +++ b/projects/microb2010/mainboard/i2c_protocol.h @@ -35,12 +35,10 @@ void i2c_sendevent(int8_t size); int8_t i2c_set_color(uint8_t addr, uint8_t color); int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state); -int8_t i2c_cobboard_mode_init(void); -int8_t i2c_cobboard_mode_eject(void); -int8_t i2c_cobboard_mode_harvest(uint8_t side); -int8_t i2c_cobboard_mode_deploy(uint8_t side); -int8_t i2c_cobboard_mode_pack(uint8_t side); - +int8_t i2c_cobboard_set_mode(uint8_t mode); +int8_t i2c_cobboard_pack(uint8_t side); +int8_t i2c_cobboard_harvest(uint8_t side); +int8_t i2c_cobboard_deploy(uint8_t side); int8_t i2c_ballboard_set_mode(uint8_t mode); #endif diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index ff86dd2..4fa3dd8 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -196,6 +196,8 @@ struct mainboard { struct cobboard { uint8_t mode; uint8_t status; + uint8_t lspickle; + uint8_t rspickle; int16_t left_cobroller_speed; int16_t right_cobroller_speed; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index a5a952c..df80b79 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -110,8 +110,9 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); - i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); - i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_harvest(I2C_LEFT_SIDE); + i2c_cobboard_harvest(I2C_RIGHT_SIDE); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* used in strat_base for END_TIMER */ @@ -198,15 +199,15 @@ static uint8_t strat_beginning(void) /* half turn */ trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + i2c_cobboard_pack(I2C_LEFT_SIDE); + i2c_cobboard_pack(I2C_RIGHT_SIDE); trajectory_a_rel(&mainboard.traj, COLOR_A(180)); err = wait_traj_end(END_INTR|END_TRAJ); /* cob ejection */ trajectory_d_rel(&mainboard.traj, -100); err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_eject(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); time_wait_ms(2000); trajectory_hardstop(&mainboard.traj); -- 2.39.5