__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";
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:
{
}
#ifdef notyet
- case I2C_CMD_EXTENSION_TEST:
+ case I2C_CMD_EXTENSION_TEST:
{
struct i2c_cmd_extension_test *cmd = void_cmd;
if (size != sizeof (*cmd))
/* Add other commands here ...*/
-
case I2C_REQ_COBBOARD_STATUS:
{
struct i2c_req_cobboard_status *cmd = void_cmd;
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;
}
#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))
!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), */
/* 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);
}
state_do_eject();
}
}
- state_status = I2C_COBBOARD_STATUS_READY;
+ state_status = I2C_COBBOARD_STATUS_OFF;
}
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;
}
/* 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);
#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
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;
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";
};
/* 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;
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";
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);
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);
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)
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
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;
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 */
/* 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);
git.droids-corp.org / aversive.git / commitdiff