From 38196781643cba8e1d681aafb28cdadc51fe20ec Mon Sep 17 00:00:00 2001
From: zer0 <zer0@carbon.local>
Date: Fri, 30 Apr 2010 23:30:51 +0200
Subject: [PATCH] cobboard no move mode
---
.../microb2010/cobboard/commands_cobboard.c | 13 ++++-
projects/microb2010/cobboard/spickle.c | 27 ++++++++++-
projects/microb2010/cobboard/spickle.h | 7 ++-
projects/microb2010/cobboard/state.c | 39 ++++++++++-----
projects/microb2010/common/avr6.x | 24 +++++-----
projects/microb2010/common/i2c_commands.h | 1 +
projects/microb2010/mainboard/commands.c | 2 +
.../microb2010/mainboard/commands_mainboard.c | 48 ++++++++++++++++++-
projects/microb2010/mainboard/i2c_protocol.c | 14 ++++++
projects/microb2010/mainboard/i2c_protocol.h | 3 ++
projects/microb2010/mainboard/strat.c | 35 +++++++++-----
projects/microb2010/mainboard/strat.h | 2 +
projects/microb2010/mainboard/strat_corn.c | 1 +
13 files changed, 175 insertions(+), 41 deletions(-)
diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index 515f3d5..669c4c8 100644
--- a/projects/microb2010/cobboard/commands_cobboard.c
+++ b/projects/microb2010/cobboard/commands_cobboard.c
@@ -253,11 +253,22 @@ static void cmd_state2_parsed(void *parsed_result,
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);
diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index f8e1613..7296d8e 100644
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -137,7 +137,6 @@ void spickle_set_coefs(uint32_t k1, uint32_t k2)
spickle.k2 = k2;
}
-
void spickle_set_pos(uint8_t side, int32_t pos_packed,
int32_t pos_mid, int32_t pos_deployed)
{
@@ -159,6 +158,32 @@ void spickle_dump_params(void)
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]);
diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h
index 81c74d8..f34894a 100644
--- a/projects/microb2010/cobboard/spickle.h
+++ b/projects/microb2010/cobboard/spickle.h
@@ -1,6 +1,6 @@
-/*
+/*
* 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
@@ -29,6 +29,9 @@ void spickle_set_pos(uint8_t side, int32_t pos_pack,
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);
diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index 86afce9..e315da0 100644
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -96,7 +96,7 @@ static uint8_t state_no_cob_inside(void)
!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;
@@ -104,7 +104,7 @@ static uint8_t is_deployed(uint8_t side)
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;
@@ -112,14 +112,22 @@ static uint8_t is_autoharvest(uint8_t side)
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);
@@ -247,12 +255,15 @@ static void state_do_harvest(uint8_t 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 */
@@ -326,11 +337,13 @@ void state_machine(void)
/* 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);
}
diff --git a/projects/microb2010/common/avr6.x b/projects/microb2010/common/avr6.x
index a758e96..3ad392f 100644
--- a/projects/microb2010/common/avr6.x
+++ b/projects/microb2010/common/avr6.x
@@ -123,6 +123,18 @@ SECTIONS
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)
@@ -151,18 +163,6 @@ SECTIONS
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. */
diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index fa17fca..d64ed8b 100644
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -108,6 +108,7 @@ struct i2c_req_cobboard_status {
#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;
};
diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index 255f26b..e79562b 100644
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -82,6 +82,7 @@ extern parse_pgm_inst_t cmd_beacon_start;
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 */
@@ -173,6 +174,7 @@ parse_pgm_ctx_t main_ctx[] = {
(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 */
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index ee0ffa4..13a327a 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -785,11 +785,19 @@ static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
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);
@@ -1164,6 +1172,44 @@ parse_pgm_inst_t cmd_time_monitor = {
},
};
+
+/**********************************************************/
+/* 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 */
diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 9908f1a..7a2c378 100644
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -438,6 +438,20 @@ int8_t i2c_cobboard_deploy(uint8_t side)
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;
diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h
index b107c44..06cd47e 100644
--- a/projects/microb2010/mainboard/i2c_protocol.h
+++ b/projects/microb2010/mainboard/i2c_protocol.h
@@ -39,6 +39,9 @@ int8_t i2c_cobboard_set_mode(uint8_t mode);
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
diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 423b0f7..94b9b6f 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -67,7 +67,7 @@
#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;
/*************************************************************/
@@ -98,6 +98,16 @@ void strat_conf_dump(const char *caller)
}
+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)
{
@@ -173,11 +183,9 @@ void strat_event(void *dummy)
/* 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
@@ -189,11 +197,9 @@ void strat_event(void *dummy)
/* 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
@@ -216,12 +222,18 @@ static uint8_t strat_harvest(void)
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));
@@ -263,6 +275,7 @@ static uint8_t strat_beginning(void)
TRAJ_FLAGS_STD);
strat_set_acc(ACC_DIST, ACC_ANGLE);
+ strat_set_speed(250, SPEED_ANGLE_SLOW);
#if 1
l1:
diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 9fa720a..daf607c 100644
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -162,5 +162,7 @@ void strat_dump_flags(void);
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
diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index f8a9666..8d3f839 100644
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -218,6 +218,7 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
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); */
--
2.39.5