state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
I2C_COBBOARD_SPK_AUTOHARVEST);
}
+ else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+ state_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+ I2C_COBBOARD_SPK_NO_MOVE);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+ state_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+ I2C_COBBOARD_SPK_AUTOHARVEST |
+ I2C_COBBOARD_SPK_NO_MOVE);
+ }
}
prog_char str_state2_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
-prog_char str_state2_arg1[] = "harvest#deploy#pack";
+prog_char str_state2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
prog_char str_state2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
spickle.k2 = k2;
}
-
void spickle_set_pos(uint8_t side, int32_t pos_packed,
int32_t pos_mid, int32_t pos_deployed)
{
spickle.pos_deployed[I2C_RIGHT_SIDE]);
}
+static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
+{
+ int32_t diff;
+ int32_t enc;
+ if (side == I2C_LEFT_SIDE)
+ enc = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
+ else
+ enc = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
+ diff = pos - enc;
+ if (diff < 0)
+ diff = -diff;
+ if (diff < 500)
+ return 1;
+ return 0;
+}
+
+uint8_t spickle_is_packed(uint8_t side)
+{
+ return spickle_is_at_pos(side, spickle.pos_packed[side]);
+}
+
+uint8_t spickle_is_deployed(uint8_t side)
+{
+ return spickle_is_at_pos(side, spickle.pos_deployed[side]);
+}
+
void spickle_deploy(uint8_t side)
{
cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
-/*
+/*
* Copyright Droids Corporation (2010)
- *
+ *
* 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
void spickle_dump_params(void);
+uint8_t spickle_is_packed(void);
+uint8_t spickle_is_deployed(void);
+
void spickle_deploy(uint8_t side);
void spickle_mid(uint8_t side);
void spickle_pack(uint8_t side);
!sensor_get(S_COB_INSIDE_R);
}
-static uint8_t is_deployed(uint8_t side)
+static uint8_t state_spicklemode_deployed(uint8_t side)
{
if (side == I2C_LEFT_SIDE)
return lspickle & I2C_COBBOARD_SPK_DEPLOY;
return rspickle & I2C_COBBOARD_SPK_DEPLOY;
}
-static uint8_t is_autoharvest(uint8_t side)
+static uint8_t state_spicklemode_autoharvest(uint8_t side)
{
if (side == I2C_LEFT_SIDE)
return lspickle & I2C_COBBOARD_SPK_AUTOHARVEST;
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)
{
- if (cob_count >= 5)
- spickle_pack(side);
- else if (is_deployed(side) && !is_autoharvest(side))
- spickle_deploy(side); /*spickle_mid(side);*/
- else if (is_deployed(side) && is_autoharvest(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);
cobroller_off(side);
cob_count ++;
+ state_debug_wait_key_pressed();
+
/* last cob, nothing to do */
if (cob_count == 5)
return;
- /* redeploy the spickle */
- spickle_deploy(side);
+ /* redeploy the spickle if there is a black cob */
+ if (!state_spicklemode_nomove(side))
+ spickle_deploy(side);
state_debug_wait_key_pressed();
/* let the loaded cobs go */
/* harvest */
if (cob_count < 5) {
- if ((lspickle & I2C_COBBOARD_SPK_DEPLOY) &&
- (lspickle & I2C_COBBOARD_SPK_AUTOHARVEST))
+ if (state_spicklemode_deployed(I2C_LEFT_SIDE) &&
+ state_spicklemode_autoharvest(I2C_LEFT_SIDE) &&
+ !state_spicklemode_nomove(I2C_LEFT_SIDE))
state_do_harvest(I2C_LEFT_SIDE);
- if ((rspickle & I2C_COBBOARD_SPK_DEPLOY) &&
- (rspickle & I2C_COBBOARD_SPK_AUTOHARVEST))
+ if (state_spicklemode_deployed(I2C_RIGHT_SIDE) &&
+ state_spicklemode_autoharvest(I2C_RIGHT_SIDE) &&
+ !state_spicklemode_nomove(I2C_RIGHT_SIDE))
state_do_harvest(I2C_RIGHT_SIDE);
}
KEEP (*(.init9))
*(.text.*) /* trucs de gcc ? */
. = ALIGN(2048);
+ /* some libc stuff */
+ strc*(.text)
+ mem*(.text)
+ printf*(.text)
+ vfprintf*(.text)
+ sprintf*(.text)
+ snprintf*(.text)
+ malloc*(.text)
+ free*(.text)
+ fdevopen*(.text)
+ fputc*(.text)
+ . = ALIGN(2048);
uart*(.text)
pwm*(.text)
parse*(.text)
spi*(.text)
ax12*(.text)
. = ALIGN(2048);
- /* some libc stuff */
- str*(.text)
- mem*(.text)
- printf*(.text)
- vfprintf*(.text)
- sprintf*(.text)
- snprintf*(.text)
- malloc*(.text)
- free*(.text)
- fdevopen*(.text)
- fputc*(.text)
- . = ALIGN(2048);
*(.text)
. = ALIGN(2);
*(.fini9) /* _exit() starts here. */
#define I2C_COBBOARD_SPK_DEPLOY 0x01 /* deploy the spickle */
#define I2C_COBBOARD_SPK_AUTOHARVEST 0x02 /* auto harvest the cobs */
+#define I2C_COBBOARD_SPK_NO_MOVE 0x04 /* if enabled, don't change state */
uint8_t lspickle;
uint8_t rspickle;
};
extern parse_pgm_inst_t cmd_servo_balls;
extern parse_pgm_inst_t cmd_clitoid;
extern parse_pgm_inst_t cmd_time_monitor;
+extern parse_pgm_inst_t cmd_strat_event;
extern parse_pgm_inst_t cmd_test;
/* commands_traj.c */
(parse_pgm_inst_t *)&cmd_servo_balls,
(parse_pgm_inst_t *)&cmd_clitoid,
(parse_pgm_inst_t *)&cmd_time_monitor,
+ (parse_pgm_inst_t *)&cmd_strat_event,
(parse_pgm_inst_t *)&cmd_test,
/* commands_traj.c */
i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
i2c_cobboard_pack(side);
}
+ else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_deploy_nomove(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_autoharvest_nomove(side);
+ }
}
prog_char str_cobboard_setmode2_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
prog_char str_cobboard_setmode2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
},
};
+
+/**********************************************************/
+/* Strat_Event */
+
+/* this structure is filled when cmd_strat_event is parsed successfully */
+struct cmd_strat_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_event is parsed successfully */
+static void cmd_strat_event_parsed(void *parsed_result, void *data)
+{
+ struct cmd_strat_event_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("on")))
+ strat_event_enable();
+ else
+ strat_event_disable();
+}
+
+prog_char str_strat_event_arg0[] = "strat_event";
+parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
+prog_char str_strat_event_arg1[] = "on#off";
+parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
+
+prog_char help_strat_event[] = "Enable/disable strat_event callback";
+parse_pgm_inst_t cmd_strat_event = {
+ .f = cmd_strat_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_event_arg0,
+ (prog_void *)&cmd_strat_event_arg1,
+ NULL,
+ },
+};
+
/**********************************************************/
/* Test */
return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY);
}
+int8_t i2c_cobboard_autoharvest_nomove(uint8_t side)
+{
+ return i2c_cobboard_set_spickle(side,
+ I2C_COBBOARD_SPK_DEPLOY |
+ I2C_COBBOARD_SPK_AUTOHARVEST |
+ I2C_COBBOARD_SPK_NO_MOVE);
+}
+
+int8_t i2c_cobboard_deploy_nomove(uint8_t side)
+{
+ return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+ I2C_COBBOARD_SPK_NO_MOVE);
+}
+
int8_t i2c_ballboard_set_mode(uint8_t mode)
{
struct i2c_cmd_ballboard_set_mode buf;
int8_t i2c_cobboard_pack(uint8_t side);
int8_t i2c_cobboard_autoharvest(uint8_t side);
int8_t i2c_cobboard_deploy(uint8_t side);
+int8_t i2c_cobboard_autoharvest_nomove(uint8_t side);
+int8_t i2c_cobboard_deploy_nomove(uint8_t side);
+
int8_t i2c_ballboard_set_mode(uint8_t mode);
#endif
#define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
#define COL_SCAN_PRE_MARGIN 250
-static uint8_t strat_running = 0;
+static volatile uint8_t strat_running = 0;
struct strat_conf strat_conf;
/*************************************************************/
}
+void strat_event_enable(void)
+{
+ strat_running = 1;
+}
+
+void strat_event_disable(void)
+{
+ strat_running = 0;
+}
+
/* call it just before launching the strat */
void strat_init(void)
{
/* detect cob on left side */
if (corn_is_near(&lidx, I2C_LEFT_SIDE)) {
- if (lcob != I2C_COB_NONE) {
+ if (lcob != I2C_COB_NONE)
corn_set_color(strat_db.corn_table[lidx], lcob);
- DEBUG(E_USER_STRAT, "lcob %s %d",
- lcob == I2C_COB_WHITE ? "white" : "black", lidx);
- }
+
if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
else
/* detect cob on right side */
if (corn_is_near(&ridx, I2C_RIGHT_SIDE)) {
- if (rcob != I2C_COB_NONE) {
+ if (rcob != I2C_COB_NONE)
corn_set_color(strat_db.corn_table[ridx], rcob);
- DEBUG(E_USER_STRAT, "rcob %s %d",
- rcob == I2C_COB_WHITE ? "white" : "black", ridx);
- }
+
if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE)
i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
else
static uint8_t strat_eject(void)
{
+ uint8_t err;
+
+ //XXX return vals
+ strat_set_speed(600, SPEED_ANGLE_SLOW);
+
trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
- err = wait_traj_end(END_INTR|END_TRAJ);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_a_abs(&mainboard.traj, COLOR_A(70));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
strat_hardstop();
- strat_set_speed(600, SPEED_ANGLE_FAST);
/* ball ejection */
trajectory_a_abs(&mainboard.traj, COLOR_A(90));
TRAJ_FLAGS_STD);
strat_set_acc(ACC_DIST, ACC_ANGLE);
+ strat_set_speed(250, SPEED_ANGLE_SLOW);
#if 1
l1:
void strat_goto_near(int16_t x, int16_t y, uint16_t dist);
uint8_t strat_main(void);
void strat_event(void *dummy);
+void strat_event_enable(void);
+void strat_event_disable(void);
#endif
strat_get_speed(&d_speed, &a_speed);
/* XXX 600 -> cste */
+ /* XXX does not work, do better */
/* if (err == 0 && d_speed < 600 && */
/* mainboard.traj.state == RUNNING_CLITOID_LINE) */
/* strat_set_speed(600, SPEED_ANGLE_FAST); */