From 8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Wed, 17 Mar 2010 23:38:35 +0100 Subject: [PATCH] prepare cobboard and ballboard --- modules/comm/uart/uart_defs.h | 3 + .../{sensorboard => ballboard}/.config | 5 +- .../{sensorboard => ballboard}/Makefile | 5 +- .../{sensorboard => ballboard}/actuator.c | 19 +- .../{sensorboard => ballboard}/actuator.h | 4 - .../{mechboard => ballboard}/adc_config.h | 0 .../{mechboard => ballboard}/ax12_config.h | 0 .../{sensorboard => ballboard}/ax12_user.c | 2 +- .../{sensorboard => ballboard}/ax12_user.h | 0 .../{sensorboard => ballboard}/cmdline.c | 4 +- .../{sensorboard => ballboard}/cmdline.h | 0 .../{sensorboard => ballboard}/commands.c | 21 +- .../commands_ax12.c | 2 +- .../commands_ballboard.c} | 32 +- .../{sensorboard => ballboard}/commands_cs.c | 14 +- .../{sensorboard => ballboard}/commands_gen.c | 2 +- projects/microb2010/ballboard/cs.c | 183 ++ .../{sensorboard => ballboard}/cs.h | 0 .../diagnostic_config.h | 0 .../encoders_spi_config.h | 0 .../{sensorboard => ballboard}/error_config.h | 0 .../{sensorboard => ballboard}/i2c_config.h | 0 projects/microb2010/ballboard/i2c_protocol.c | 147 ++ .../{sensorboard => ballboard}/i2c_protocol.h | 0 .../{sensorboard => ballboard}/main.c | 41 +- .../{sensorboard => ballboard}/main.h | 36 +- .../{mechboard => ballboard}/pid_config.h | 0 .../{mechboard => ballboard}/rdline_config.h | 0 .../scheduler_config.h | 0 .../{sensorboard => ballboard}/sensor.c | 0 .../{sensorboard => ballboard}/sensor.h | 0 .../{mechboard => ballboard}/spi_config.h | 0 .../{sensorboard => ballboard}/time_config.h | 0 .../{sensorboard => ballboard}/timer_config.h | 0 .../{sensorboard => ballboard}/uart_config.h | 0 .../{mechboard => cobboard}/.config | 7 +- .../{mechboard => cobboard}/Makefile | 4 +- .../actuator.h => cobboard/actuator.c} | 58 +- projects/microb2010/cobboard/actuator.h | 27 + .../{sensorboard => cobboard}/adc_config.h | 0 .../{sensorboard => cobboard}/ax12_config.h | 0 .../{mechboard => cobboard}/ax12_user.c | 2 +- .../{mechboard => cobboard}/ax12_user.h | 0 .../{mechboard => cobboard}/cmdline.c | 4 +- .../{mechboard => cobboard}/cmdline.h | 0 .../{mechboard => cobboard}/commands.c | 30 +- .../{mechboard => cobboard}/commands_ax12.c | 2 +- .../microb2010/cobboard/commands_cobboard.c | 477 +++++ .../{mechboard => cobboard}/commands_cs.c | 14 +- .../{mechboard => cobboard}/commands_gen.c | 8 +- projects/microb2010/cobboard/cs.c | 192 ++ .../microb2010/{mechboard => cobboard}/cs.h | 0 .../diagnostic_config.h | 0 .../encoders_spi_config.h | 0 .../{mechboard => cobboard}/error_config.h | 0 .../{mechboard => cobboard}/i2c_config.h | 0 .../{mechboard => cobboard}/i2c_protocol.c | 40 +- .../{mechboard => cobboard}/i2c_protocol.h | 0 .../microb2010/{mechboard => cobboard}/main.c | 19 +- .../microb2010/{mechboard => cobboard}/main.h | 52 +- .../{sensorboard => cobboard}/pid_config.h | 0 .../{sensorboard => cobboard}/rdline_config.h | 0 .../scheduler_config.h | 0 .../{mechboard => cobboard}/sensor.c | 0 .../{mechboard => cobboard}/sensor.h | 0 .../{sensorboard => cobboard}/spi_config.h | 0 projects/microb2010/cobboard/state.c | 163 ++ .../{mechboard => cobboard}/state.h | 6 +- .../{mechboard => cobboard}/time_config.h | 0 .../{mechboard => cobboard}/timer_config.h | 0 .../{mechboard => cobboard}/uart_config.h | 0 projects/microb2010/common/eeprom_mapping.h | 4 +- projects/microb2010/common/i2c_commands.h | 226 +-- projects/microb2010/mechboard/actuator.c | 163 -- projects/microb2010/mechboard/arm_highlevel.c | 644 ------- projects/microb2010/mechboard/arm_highlevel.h | 84 - projects/microb2010/mechboard/arm_xy.c | 809 -------- projects/microb2010/mechboard/arm_xy.h | 123 -- .../microb2010/mechboard/commands_mechboard.c | 1033 ----------- projects/microb2010/mechboard/cs.c | 163 -- projects/microb2010/mechboard/state.c | 1407 -------------- projects/microb2010/sensorboard/beacon.c | 397 ---- projects/microb2010/sensorboard/beacon.h | 58 - .../microb2010/sensorboard/commands_scan.c | 401 ---- projects/microb2010/sensorboard/cs.c | 145 -- .../microb2010/sensorboard/gen_scan_tab.c | 680 ------- .../microb2010/sensorboard/i2c_protocol.c | 256 --- .../microb2010/sensorboard/img_processing.c | 1619 ----------------- .../microb2010/sensorboard/img_processing.h | 111 -- projects/microb2010/sensorboard/scanner.c | 898 --------- projects/microb2010/sensorboard/scanner.h | 158 -- .../microb2010/tests/beacon_tsop/Makefile | 2 +- projects/microb2010/tests/beacon_tsop/main.c | 335 +++- projects/microb2010/tests/beacon_tsop/main.h | 46 - projects/microb2010/tests/beacon_tsop/trigo.c | 392 +++- projects/microb2010/tests/beacon_tsop/trigo.h | 11 + .../tests/static_beacon/static_beacon.c | 17 + 97 files changed, 2054 insertions(+), 9753 deletions(-) rename projects/microb2010/{sensorboard => ballboard}/.config (97%) rename projects/microb2010/{sensorboard => ballboard}/Makefile (91%) rename projects/microb2010/{sensorboard => ballboard}/actuator.c (74%) rename projects/microb2010/{sensorboard => ballboard}/actuator.h (93%) rename projects/microb2010/{mechboard => ballboard}/adc_config.h (100%) rename projects/microb2010/{mechboard => ballboard}/ax12_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/ax12_user.c (99%) rename projects/microb2010/{sensorboard => ballboard}/ax12_user.h (100%) rename projects/microb2010/{sensorboard => ballboard}/cmdline.c (97%) rename projects/microb2010/{sensorboard => ballboard}/cmdline.h (100%) rename projects/microb2010/{sensorboard => ballboard}/commands.c (86%) rename projects/microb2010/{sensorboard => ballboard}/commands_ax12.c (99%) rename projects/microb2010/{sensorboard/commands_sensorboard.c => ballboard/commands_ballboard.c} (89%) rename projects/microb2010/{sensorboard => ballboard}/commands_cs.c (98%) rename projects/microb2010/{sensorboard => ballboard}/commands_gen.c (99%) create mode 100644 projects/microb2010/ballboard/cs.c rename projects/microb2010/{sensorboard => ballboard}/cs.h (100%) rename projects/microb2010/{sensorboard => ballboard}/diagnostic_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/encoders_spi_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/error_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/i2c_config.h (100%) create mode 100644 projects/microb2010/ballboard/i2c_protocol.c rename projects/microb2010/{sensorboard => ballboard}/i2c_protocol.h (100%) rename projects/microb2010/{sensorboard => ballboard}/main.c (87%) rename projects/microb2010/{sensorboard => ballboard}/main.h (84%) rename projects/microb2010/{mechboard => ballboard}/pid_config.h (100%) rename projects/microb2010/{mechboard => ballboard}/rdline_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/scheduler_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/sensor.c (100%) rename projects/microb2010/{sensorboard => ballboard}/sensor.h (100%) rename projects/microb2010/{mechboard => ballboard}/spi_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/time_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/timer_config.h (100%) rename projects/microb2010/{sensorboard => ballboard}/uart_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/.config (96%) rename projects/microb2010/{mechboard => cobboard}/Makefile (91%) rename projects/microb2010/{mechboard/actuator.h => cobboard/actuator.c} (51%) create mode 100644 projects/microb2010/cobboard/actuator.h rename projects/microb2010/{sensorboard => cobboard}/adc_config.h (100%) rename projects/microb2010/{sensorboard => cobboard}/ax12_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/ax12_user.c (99%) rename projects/microb2010/{mechboard => cobboard}/ax12_user.h (100%) rename projects/microb2010/{mechboard => cobboard}/cmdline.c (97%) rename projects/microb2010/{mechboard => cobboard}/cmdline.h (100%) rename projects/microb2010/{mechboard => cobboard}/commands.c (82%) rename projects/microb2010/{mechboard => cobboard}/commands_ax12.c (99%) create mode 100644 projects/microb2010/cobboard/commands_cobboard.c rename projects/microb2010/{mechboard => cobboard}/commands_cs.c (97%) rename projects/microb2010/{mechboard => cobboard}/commands_gen.c (98%) create mode 100644 projects/microb2010/cobboard/cs.c rename projects/microb2010/{mechboard => cobboard}/cs.h (100%) rename projects/microb2010/{mechboard => cobboard}/diagnostic_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/encoders_spi_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/error_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/i2c_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/i2c_protocol.c (74%) rename projects/microb2010/{mechboard => cobboard}/i2c_protocol.h (100%) rename projects/microb2010/{mechboard => cobboard}/main.c (93%) rename projects/microb2010/{mechboard => cobboard}/main.h (78%) rename projects/microb2010/{sensorboard => cobboard}/pid_config.h (100%) rename projects/microb2010/{sensorboard => cobboard}/rdline_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/scheduler_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/sensor.c (100%) rename projects/microb2010/{mechboard => cobboard}/sensor.h (100%) rename projects/microb2010/{sensorboard => cobboard}/spi_config.h (100%) create mode 100644 projects/microb2010/cobboard/state.c rename projects/microb2010/{mechboard => cobboard}/state.h (88%) rename projects/microb2010/{mechboard => cobboard}/time_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/timer_config.h (100%) rename projects/microb2010/{mechboard => cobboard}/uart_config.h (100%) delete mode 100644 projects/microb2010/mechboard/actuator.c delete mode 100644 projects/microb2010/mechboard/arm_highlevel.c delete mode 100644 projects/microb2010/mechboard/arm_highlevel.h delete mode 100644 projects/microb2010/mechboard/arm_xy.c delete mode 100644 projects/microb2010/mechboard/arm_xy.h delete mode 100644 projects/microb2010/mechboard/commands_mechboard.c delete mode 100644 projects/microb2010/mechboard/cs.c delete mode 100644 projects/microb2010/mechboard/state.c delete mode 100755 projects/microb2010/sensorboard/beacon.c delete mode 100755 projects/microb2010/sensorboard/beacon.h delete mode 100644 projects/microb2010/sensorboard/commands_scan.c delete mode 100644 projects/microb2010/sensorboard/cs.c delete mode 100644 projects/microb2010/sensorboard/gen_scan_tab.c delete mode 100644 projects/microb2010/sensorboard/i2c_protocol.c delete mode 100644 projects/microb2010/sensorboard/img_processing.c delete mode 100644 projects/microb2010/sensorboard/img_processing.h delete mode 100644 projects/microb2010/sensorboard/scanner.c delete mode 100644 projects/microb2010/sensorboard/scanner.h diff --git a/modules/comm/uart/uart_defs.h b/modules/comm/uart/uart_defs.h index e37de8c..7d872a1 100644 --- a/modules/comm/uart/uart_defs.h +++ b/modules/comm/uart/uart_defs.h @@ -115,6 +115,9 @@ #ifndef TXC #define TXC TXC0 #endif +#ifndef RXC +#define RXC RXC0 +#endif #ifndef RXB8 #define RXB8 RXB80 #endif diff --git a/projects/microb2010/sensorboard/.config b/projects/microb2010/ballboard/.config similarity index 97% rename from projects/microb2010/sensorboard/.config rename to projects/microb2010/ballboard/.config index bc9e293..0d59101 100644 --- a/projects/microb2010/sensorboard/.config +++ b/projects/microb2010/ballboard/.config @@ -83,7 +83,9 @@ CONFIG_MODULE_CIRBUF=y # CONFIG_MODULE_FIXED_POINT is not set # CONFIG_MODULE_VECT2 is not set CONFIG_MODULE_GEOMETRY=y +# CONFIG_MODULE_HOSTSIM is not set CONFIG_MODULE_SCHEDULER=y +# CONFIG_MODULE_SCHEDULER_STATS is not set # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set # CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set # CONFIG_MODULE_SCHEDULER_TIMER0 is not set @@ -168,9 +170,10 @@ CONFIG_MODULE_ENCODERS_SPI=y CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y # -# Robot specific modules +# Robot specific modules (fixed point lib may be needed) # # CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set # CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set # CONFIG_MODULE_POSITION_MANAGER is not set # CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set diff --git a/projects/microb2010/sensorboard/Makefile b/projects/microb2010/ballboard/Makefile similarity index 91% rename from projects/microb2010/sensorboard/Makefile rename to projects/microb2010/ballboard/Makefile index 3013479..afa396b 100644 --- a/projects/microb2010/sensorboard/Makefile +++ b/projects/microb2010/ballboard/Makefile @@ -9,11 +9,8 @@ LDFLAGS = -T ../common/avr6.x # List C source files here. (C dependencies are automatically generated.) SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c -SRC += commands_cs.c commands_sensorboard.c commands.c commands_scan.c +SRC += commands_cs.c commands_ballboard.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c -SRC += beacon.c -SRC += img_processing.c -SRC += scanner.c # List Assembler source files here. diff --git a/projects/microb2010/sensorboard/actuator.c b/projects/microb2010/ballboard/actuator.c similarity index 74% rename from projects/microb2010/sensorboard/actuator.c rename to projects/microb2010/ballboard/actuator.c index 2895c5a..367e241 100644 --- a/projects/microb2010/sensorboard/actuator.c +++ b/projects/microb2010/ballboard/actuator.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -42,20 +42,3 @@ #include "main.h" -#define PICKUP_WHEEL_L_ON 2000 -#define PICKUP_WHEEL_R_ON -2000 -#define PICKUP_WHEEL_L_OFF 0 -#define PICKUP_WHEEL_R_OFF 0 - -void pickup_wheels_on(void) -{ - pwm_ng_set(PICKUP_WHEEL_L_PWM, PICKUP_WHEEL_L_ON); - pwm_ng_set(PICKUP_WHEEL_R_PWM, PICKUP_WHEEL_R_ON); -} - -void pickup_wheels_off(void) -{ - pwm_ng_set(PICKUP_WHEEL_L_PWM, PICKUP_WHEEL_L_OFF); - pwm_ng_set(PICKUP_WHEEL_R_PWM, PICKUP_WHEEL_R_OFF); -} - diff --git a/projects/microb2010/sensorboard/actuator.h b/projects/microb2010/ballboard/actuator.h similarity index 93% rename from projects/microb2010/sensorboard/actuator.h rename to projects/microb2010/ballboard/actuator.h index a1a535d..a6320be 100644 --- a/projects/microb2010/sensorboard/actuator.h +++ b/projects/microb2010/ballboard/actuator.h @@ -19,7 +19,3 @@ * */ -void pickup_wheels_on(void); -void pickup_wheels_off(void); - - diff --git a/projects/microb2010/mechboard/adc_config.h b/projects/microb2010/ballboard/adc_config.h similarity index 100% rename from projects/microb2010/mechboard/adc_config.h rename to projects/microb2010/ballboard/adc_config.h diff --git a/projects/microb2010/mechboard/ax12_config.h b/projects/microb2010/ballboard/ax12_config.h similarity index 100% rename from projects/microb2010/mechboard/ax12_config.h rename to projects/microb2010/ballboard/ax12_config.h diff --git a/projects/microb2010/sensorboard/ax12_user.c b/projects/microb2010/ballboard/ax12_user.c similarity index 99% rename from projects/microb2010/sensorboard/ax12_user.c rename to projects/microb2010/ballboard/ax12_user.c index 231495e..e3a5f9f 100644 --- a/projects/microb2010/sensorboard/ax12_user.c +++ b/projects/microb2010/ballboard/ax12_user.c @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/sensorboard/ax12_user.h b/projects/microb2010/ballboard/ax12_user.h similarity index 100% rename from projects/microb2010/sensorboard/ax12_user.h rename to projects/microb2010/ballboard/ax12_user.h diff --git a/projects/microb2010/sensorboard/cmdline.c b/projects/microb2010/ballboard/cmdline.c similarity index 97% rename from projects/microb2010/sensorboard/cmdline.c rename to projects/microb2010/ballboard/cmdline.c index ceb3efe..6f8ff46 100644 --- a/projects/microb2010/sensorboard/cmdline.c +++ b/projects/microb2010/ballboard/cmdline.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -122,7 +122,7 @@ int cmdline_interact(void) int16_t c; rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); - snprintf(gen.prompt, sizeof(gen.prompt), "sensorboard > "); + snprintf(gen.prompt, sizeof(gen.prompt), "ballboard > "); rdline_newline(&gen.rdl, gen.prompt); while (1) { diff --git a/projects/microb2010/sensorboard/cmdline.h b/projects/microb2010/ballboard/cmdline.h similarity index 100% rename from projects/microb2010/sensorboard/cmdline.h rename to projects/microb2010/ballboard/cmdline.h diff --git a/projects/microb2010/sensorboard/commands.c b/projects/microb2010/ballboard/commands.c similarity index 86% rename from projects/microb2010/sensorboard/commands.c rename to projects/microb2010/ballboard/commands.c index 539c96d..9ab33ad 100644 --- a/projects/microb2010/sensorboard/commands.c +++ b/projects/microb2010/ballboard/commands.c @@ -60,19 +60,10 @@ extern parse_pgm_inst_t cmd_cs_status; extern parse_pgm_inst_t cmd_blocking_i; extern parse_pgm_inst_t cmd_blocking_i_show; -/* commands_sensorboard.c */ +/* commands_ballboard.c */ extern parse_pgm_inst_t cmd_event; extern parse_pgm_inst_t cmd_test; -/* commands_scan.c */ -extern parse_pgm_inst_t cmd_sample; -extern parse_pgm_inst_t cmd_sadc; -extern parse_pgm_inst_t cmd_scan_params; -extern parse_pgm_inst_t cmd_scan_calibre; -extern parse_pgm_inst_t cmd_scan_do; -extern parse_pgm_inst_t cmd_scan_img; - - /* in progmem */ parse_pgm_ctx_t main_ctx[] = { @@ -112,17 +103,9 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_blocking_i, (parse_pgm_inst_t *)&cmd_blocking_i_show, - /* commands_sensorboard.c */ + /* commands_ballboard.c */ (parse_pgm_inst_t *)&cmd_event, (parse_pgm_inst_t *)&cmd_test, - /* commands_scan.c */ - (parse_pgm_inst_t *)&cmd_sample, - (parse_pgm_inst_t *)&cmd_sadc, - (parse_pgm_inst_t *)&cmd_scan_params, - (parse_pgm_inst_t *)&cmd_scan_calibre, - (parse_pgm_inst_t *)&cmd_scan_do, - (parse_pgm_inst_t *)&cmd_scan_img, - NULL, }; diff --git a/projects/microb2010/sensorboard/commands_ax12.c b/projects/microb2010/ballboard/commands_ax12.c similarity index 99% rename from projects/microb2010/sensorboard/commands_ax12.c rename to projects/microb2010/ballboard/commands_ax12.c index 771fb30..2fda225 100644 --- a/projects/microb2010/sensorboard/commands_ax12.c +++ b/projects/microb2010/ballboard/commands_ax12.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/sensorboard/commands_sensorboard.c b/projects/microb2010/ballboard/commands_ballboard.c similarity index 89% rename from projects/microb2010/sensorboard/commands_sensorboard.c rename to projects/microb2010/ballboard/commands_ballboard.c index 9b90bd1..03dc830 100644 --- a/projects/microb2010/sensorboard/commands_sensorboard.c +++ b/projects/microb2010/ballboard/commands_ballboard.c @@ -15,7 +15,7 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: commands_sensorboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $ + * Revision : $Id: commands_ballboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $ * * Olivier MATZ */ @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -66,18 +66,18 @@ static void cmd_event_parsed(void *parsed_result, void *data) if (!strcmp_P(res->arg1, PSTR("all"))) { bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; if (!strcmp_P(res->arg2, PSTR("on"))) - sensorboard.flags |= bit; + ballboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) - sensorboard.flags &= bit; + ballboard.flags &= bit; else { /* show */ printf_P(PSTR("encoders is %s\r\n"), - (DO_ENCODERS & sensorboard.flags) ? "on":"off"); + (DO_ENCODERS & ballboard.flags) ? "on":"off"); printf_P(PSTR("cs is %s\r\n"), - (DO_CS & sensorboard.flags) ? "on":"off"); + (DO_CS & ballboard.flags) ? "on":"off"); printf_P(PSTR("bd is %s\r\n"), - (DO_BD & sensorboard.flags) ? "on":"off"); + (DO_BD & ballboard.flags) ? "on":"off"); printf_P(PSTR("power is %s\r\n"), - (DO_POWER & sensorboard.flags) ? "on":"off"); + (DO_POWER & ballboard.flags) ? "on":"off"); } return; } @@ -85,7 +85,6 @@ static void cmd_event_parsed(void *parsed_result, void *data) if (!strcmp_P(res->arg1, PSTR("encoders"))) bit = DO_ENCODERS; else if (!strcmp_P(res->arg1, PSTR("cs"))) { - //strat_hardstop(); bit = DO_CS; } else if (!strcmp_P(res->arg1, PSTR("bd"))) @@ -95,16 +94,17 @@ static void cmd_event_parsed(void *parsed_result, void *data) if (!strcmp_P(res->arg2, PSTR("on"))) - sensorboard.flags |= bit; + ballboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) { if (!strcmp_P(res->arg1, PSTR("cs"))) { - pwm_ng_set(BEACON_PWM, 0); - pwm_ng_set(SCANNER_PWM, 0); + pwm_ng_set(ROLLER_PWM, 0); + pwm_ng_set(FORKTRANS_PWM, 0); + pwm_ng_set(FORKROT_PWM, 0); } - sensorboard.flags &= (~bit); + ballboard.flags &= (~bit); } printf_P(PSTR("%s is %s\r\n"), res->arg1, - (bit & sensorboard.flags) ? "on":"off"); + (bit & ballboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; @@ -141,10 +141,10 @@ static void cmd_color_parsed(void *parsed_result, void *data) { struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; if (!strcmp_P(res->color, PSTR("red"))) { - sensorboard.our_color = I2C_COLOR_RED; + ballboard.our_color = I2C_COLOR_RED; } else if (!strcmp_P(res->color, PSTR("green"))) { - sensorboard.our_color = I2C_COLOR_GREEN; + ballboard.our_color = I2C_COLOR_GREEN; } printf_P(PSTR("Done\r\n")); } diff --git a/projects/microb2010/sensorboard/commands_cs.c b/projects/microb2010/ballboard/commands_cs.c similarity index 98% rename from projects/microb2010/sensorboard/commands_cs.c rename to projects/microb2010/ballboard/commands_cs.c index 690922a..098ffb3 100644 --- a/projects/microb2010/sensorboard/commands_cs.c +++ b/projects/microb2010/ballboard/commands_cs.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -51,11 +51,13 @@ struct csb_list { struct cs_block *csb; }; -prog_char csb_beacon_str[] = "beacon"; -prog_char csb_scanner_str[] = "scanner"; +prog_char csb_roller_str[] = "roller"; +prog_char csb_forktrans_str[] = "forktrans"; +prog_char csb_forkrot_str[] = "forkrot"; struct csb_list csb_list[] = { - { .name = csb_beacon_str, .csb = &sensorboard.beacon }, - { .name = csb_scanner_str, .csb = &sensorboard.scanner }, + { .name = csb_roller_str, .csb = &ballboard.roller }, + { .name = csb_forktrans_str, .csb = &ballboard.forktrans }, + { .name = csb_forkrot_str, .csb = &ballboard.forkrot }, }; struct cmd_cs_result { @@ -64,7 +66,7 @@ struct cmd_cs_result { }; /* token to be used for all cs-related commands */ -prog_char str_csb_name[] = "beacon#scanner"; +prog_char str_csb_name[] = "roller#forktrans#forkrot"; parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name); struct cs_block *cs_from_name(const char *name) diff --git a/projects/microb2010/sensorboard/commands_gen.c b/projects/microb2010/ballboard/commands_gen.c similarity index 99% rename from projects/microb2010/sensorboard/commands_gen.c rename to projects/microb2010/ballboard/commands_gen.c index 9f7cdd6..b2a1e5d 100644 --- a/projects/microb2010/sensorboard/commands_gen.c +++ b/projects/microb2010/ballboard/commands_gen.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c new file mode 100644 index 0000000..7e87fca --- /dev/null +++ b/projects/microb2010/ballboard/cs.c @@ -0,0 +1,183 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: cs.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "actuator.h" + +int32_t encoders_spi_update_roller_speed(void *number) +{ + static volatile int32_t roller_pos; + int32_t tmp, speed; + tmp = encoders_spi_get_value(number); + speed = tmp - roller_pos; + roller_pos = tmp; + return speed; +} + +/* called every 5 ms */ +static void do_cs(void *dummy) +{ + /* read encoders */ + if (ballboard.flags & DO_ENCODERS) { + encoders_spi_manage(NULL); + } + /* control system */ + if (ballboard.flags & DO_CS) { + if (ballboard.roller.on) + cs_manage(&ballboard.roller.cs); + if (ballboard.forktrans.on) + cs_manage(&ballboard.forktrans.cs); + if (ballboard.forkrot.on) + cs_manage(&ballboard.forkrot.cs); + } + if (ballboard.flags & DO_BD) { + bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs); + bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs); + bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs); + } + if (ballboard.flags & DO_POWER) + BRAKE_OFF(); + else + BRAKE_ON(); +} + +void dump_cs(const char *name, struct cs *cs) +{ + printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " + "in=% .5ld out=% .5ld\r\n"), + name, cs_get_consign(cs), cs_get_filtered_consign(cs), + cs_get_error(cs), cs_get_filtered_feedback(cs), + cs_get_out(cs)); +} + +void dump_pid(const char *name, struct pid_filter *pid) +{ + printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + name, + pid_get_value_in(pid) * pid_get_gain_P(pid), + pid_get_value_I(pid) * pid_get_gain_I(pid), + pid_get_value_D(pid) * pid_get_gain_D(pid), + pid_get_value_out(pid)); +} + +void microb_cs_init(void) +{ + /* ---- CS roller */ + /* PID */ + pid_init(&ballboard.roller.pid); + pid_set_gains(&ballboard.roller.pid, 80, 80, 250); + pid_set_maximums(&ballboard.roller.pid, 0, 10000, 2000); + pid_set_out_shift(&ballboard.roller.pid, 6); + pid_set_derivate_filter(&ballboard.roller.pid, 6); + + /* CS */ + cs_init(&ballboard.roller.cs); + cs_set_correct_filter(&ballboard.roller.cs, pid_do_filter, &ballboard.roller.pid); + cs_set_process_in(&ballboard.roller.cs, pwm_ng_set, ROLLER_PWM); + cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER); + cs_set_consign(&ballboard.roller.cs, 0); + + /* ---- CS forktrans */ + /* PID */ + pid_init(&ballboard.forktrans.pid); + pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250); + pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047); + pid_set_out_shift(&ballboard.forktrans.pid, 6); + pid_set_derivate_filter(&ballboard.forktrans.pid, 6); + + /* QUADRAMP */ + quadramp_init(&ballboard.forktrans.qr); + quadramp_set_1st_order_vars(&ballboard.forktrans.qr, 200, 200); /* set speed */ + quadramp_set_2nd_order_vars(&ballboard.forktrans.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&ballboard.forktrans.cs); + cs_set_consign_filter(&ballboard.forktrans.cs, quadramp_do_filter, &ballboard.forktrans.qr); + cs_set_correct_filter(&ballboard.forktrans.cs, pid_do_filter, &ballboard.forktrans.pid); + cs_set_process_in(&ballboard.forktrans.cs, pwm_ng_set, FORKTRANS_PWM); + cs_set_process_out(&ballboard.forktrans.cs, encoders_spi_get_value, FORKTRANS_ENCODER); + cs_set_consign(&ballboard.forktrans.cs, 0); + + /* Blocking detection */ + bd_init(&ballboard.forktrans.bd); + bd_set_speed_threshold(&ballboard.forktrans.bd, 150); + bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40); + + /* ---- CS forkrot */ + /* PID */ + pid_init(&ballboard.forkrot.pid); + pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250); + pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047); + pid_set_out_shift(&ballboard.forkrot.pid, 6); + pid_set_derivate_filter(&ballboard.forkrot.pid, 6); + + /* QUADRAMP */ + quadramp_init(&ballboard.forkrot.qr); + quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */ + quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&ballboard.forkrot.cs); + cs_set_consign_filter(&ballboard.forkrot.cs, quadramp_do_filter, &ballboard.forkrot.qr); + cs_set_correct_filter(&ballboard.forkrot.cs, pid_do_filter, &ballboard.forkrot.pid); + cs_set_process_in(&ballboard.forkrot.cs, pwm_ng_set, FORKROT_PWM); + cs_set_process_out(&ballboard.forkrot.cs, encoders_spi_get_value, FORKROT_ENCODER); + cs_set_consign(&ballboard.forkrot.cs, 0); + + /* Blocking detection */ + bd_init(&ballboard.forkrot.bd); + bd_set_speed_threshold(&ballboard.forkrot.bd, 150); + bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40); + + /* set them on !! */ + ballboard.roller.on = 0; + ballboard.forktrans.on = 1; + ballboard.forkrot.on = 1; + + + scheduler_add_periodical_event_priority(do_cs, NULL, + 5000L / SCHEDULER_UNIT, + CS_PRIO); +} diff --git a/projects/microb2010/sensorboard/cs.h b/projects/microb2010/ballboard/cs.h similarity index 100% rename from projects/microb2010/sensorboard/cs.h rename to projects/microb2010/ballboard/cs.h diff --git a/projects/microb2010/sensorboard/diagnostic_config.h b/projects/microb2010/ballboard/diagnostic_config.h similarity index 100% rename from projects/microb2010/sensorboard/diagnostic_config.h rename to projects/microb2010/ballboard/diagnostic_config.h diff --git a/projects/microb2010/sensorboard/encoders_spi_config.h b/projects/microb2010/ballboard/encoders_spi_config.h similarity index 100% rename from projects/microb2010/sensorboard/encoders_spi_config.h rename to projects/microb2010/ballboard/encoders_spi_config.h diff --git a/projects/microb2010/sensorboard/error_config.h b/projects/microb2010/ballboard/error_config.h similarity index 100% rename from projects/microb2010/sensorboard/error_config.h rename to projects/microb2010/ballboard/error_config.h diff --git a/projects/microb2010/sensorboard/i2c_config.h b/projects/microb2010/ballboard/i2c_config.h similarity index 100% rename from projects/microb2010/sensorboard/i2c_config.h rename to projects/microb2010/ballboard/i2c_config.h diff --git a/projects/microb2010/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c new file mode 100644 index 0000000..8995e55 --- /dev/null +++ b/projects/microb2010/ballboard/i2c_protocol.c @@ -0,0 +1,147 @@ +/* + * Copyright Droids Corporation (2007) + * + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: i2c_protocol.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "actuator.h" + +void i2c_protocol_init(void) +{ +} + +/*** LED CONTROL ***/ +void i2c_led_control(uint8_t l, uint8_t state) +{ + switch(l) { + case 1: + state? LED1_ON():LED1_OFF(); + break; + case 2: + state? LED2_ON():LED2_OFF(); + break; + default: + break; + } +} + +void i2c_send_status(void) +{ + struct i2c_ans_ballboard_status ans; + i2c_flush(); + ans.hdr.cmd = I2C_ANS_BALLBOARD_STATUS; + ans.status = 0x55; /* XXX */ + + i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, + sizeof(ans), I2C_CTRL_GENERIC); +} + +void i2c_recvevent(uint8_t * buf, int8_t size) +{ + void *void_cmd = buf; + static uint8_t a=0; + a=!a; + if (a) + LED2_ON(); + else + LED2_OFF(); + + if (size <= 0) { + goto error; + } + + switch (buf[0]) { + + /* Commands (no answer needed) */ + case I2C_CMD_GENERIC_LED_CONTROL: + { + struct i2c_cmd_led_control *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + i2c_led_control(cmd->led_num, cmd->state); + break; + } + + case I2C_CMD_GENERIC_SET_COLOR: + { + struct i2c_cmd_generic_color *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + ballboard.our_color = cmd->color; + break; + } + + + /* Add other commands here ...*/ + + + case I2C_REQ_BALLBOARD_STATUS: + { + struct i2c_req_ballboard_status *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + + i2c_send_status(); + break; + } + + default: + goto error; + } + + error: + /* log error on a led ? */ + return; +} + +void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c) +{ +} + +void i2c_sendevent(int8_t size) +{ +} + + diff --git a/projects/microb2010/sensorboard/i2c_protocol.h b/projects/microb2010/ballboard/i2c_protocol.h similarity index 100% rename from projects/microb2010/sensorboard/i2c_protocol.h rename to projects/microb2010/ballboard/i2c_protocol.h diff --git a/projects/microb2010/sensorboard/main.c b/projects/microb2010/ballboard/main.c similarity index 87% rename from projects/microb2010/sensorboard/main.c rename to projects/microb2010/ballboard/main.c index 82fa2a5..46a3b52 100755 --- a/projects/microb2010/sensorboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include @@ -74,7 +74,7 @@ */ struct genboard gen; -struct sensorboard sensorboard; +struct ballboard ballboard; /***********************/ @@ -150,8 +150,8 @@ int main(void) DDRL = 0xc0; LED1_OFF(); memset(&gen, 0, sizeof(gen)); - memset(&sensorboard, 0, sizeof(sensorboard)); - sensorboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD + memset(&ballboard, 0, sizeof(ballboard)); + ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD /* UART */ uart_init(); @@ -165,10 +165,10 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_SENSORBOARD); + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != - EEPROM_MAGIC_SENSORBOARD) { + EEPROM_MAGIC_BALLBOARD) { sei(); printf_P(PSTR("Bad eeprom value\r\n")); while(1); @@ -186,7 +186,7 @@ int main(void) /* I2C */ i2c_protocol_init(); - i2c_init(I2C_MODE_SLAVE, I2C_SENSORBOARD_ADDR); + i2c_init(I2C_MODE_SLAVE, I2C_BALLBOARD_ADDR); i2c_register_recv_event(i2c_recvevent); /* TIMER */ @@ -242,38 +242,11 @@ int main(void) /* ax12 */ ax12_user_init(); - /* beacon */ - beacon_init(); - scheduler_add_periodical_event_priority(beacon_calc, NULL, - 20000L / SCHEDULER_UNIT, - BEACON_PRIO); - - - - /* scan */ - - - scanner_init(); - scheduler_add_periodical_event_priority(do_scan, NULL, - (1024L*1L) / SCHEDULER_UNIT, - CS_PRIO-1); - sei(); - scan_params.speed = SCAN_DEFAULT_SPEED; - scan_params.debug = 0; - - scanner_calibre_mirror(); - scanner_calibre_laser(); - - beacon_calibre_pos(); - - printf_P(PSTR("\r\n")); printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n")); cmdline_interact(); - - return 0; } diff --git a/projects/microb2010/sensorboard/main.h b/projects/microb2010/ballboard/main.h similarity index 84% rename from projects/microb2010/sensorboard/main.h rename to projects/microb2010/ballboard/main.h index 3a4ff83..36915bd 100755 --- a/projects/microb2010/sensorboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -46,24 +46,16 @@ #define BRAKE_ON() do { PORTJ |= 0xF0; } while(0) #define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0) -#define BEACON_ENCODER ((void *)0) -#define SCANNER_ENCODER ((void *)1) +#define ROLLER_ENCODER ((void *)0) +#define FORKTRANS_ENCODER ((void *)1) +#define FORKROT_ENCODER ((void *)2) -#define BEACON_PWM ((void *)&gen.pwm1_4A) -#define SCANNER_PWM ((void *)&gen.pwm2_4B) -#define PICKUP_WHEEL_L_PWM ((void *)&gen.pwm3_1A) -#define PICKUP_WHEEL_R_PWM ((void *)&gen.pwm4_1B) - -#define BEACON_POS_SENSOR 2 -#define SCANNER_POS_SENSOR 7 -#define SCANNER_MAXCOLUMN_SENSOR 1 - - - -#define SCANNER_POS_OUT 179 -#define SCANNER_POS_CALIBRE 370 -#define SCANNER_POS_IN 400 +#define ROLLER_PWM ((void *)&gen.pwm1_4A) +#define FORKTRANS_PWM ((void *)&gen.pwm2_4B) +#define FORKROT_PWM ((void *)&gen.pwm3_1A) +#define XXX_PWM ((void *)&gen.pwm4_1B) +#define BALL_PRESENT_SENSOR 2 /* XXX dummy example */ /** ERROR NUMS */ #define E_USER_I2C_PROTO 195 @@ -76,7 +68,6 @@ #define TIME_PRIO 160 #define ADC_PRIO 120 #define CS_PRIO 100 -#define BEACON_PRIO 80 #define I2C_POLL_PRIO 20 #define CS_PERIOD 5000L @@ -118,8 +109,8 @@ struct cs_block { struct blocking_detection bd; }; -/* sensorboard specific */ -struct sensorboard { +/* ballboard specific */ +struct ballboard { #define DO_ENCODERS 1 #define DO_CS 2 #define DO_BD 4 @@ -127,15 +118,16 @@ struct sensorboard { uint8_t flags; /* misc flags */ /* control systems */ - struct cs_block beacon; - struct cs_block scanner; + struct cs_block roller; + struct cs_block forktrans; + struct cs_block forkrot; /* robot status */ uint8_t our_color; }; extern struct genboard gen; -extern struct sensorboard sensorboard; +extern struct ballboard ballboard; /* start the bootloader */ void bootloader(void); diff --git a/projects/microb2010/mechboard/pid_config.h b/projects/microb2010/ballboard/pid_config.h similarity index 100% rename from projects/microb2010/mechboard/pid_config.h rename to projects/microb2010/ballboard/pid_config.h diff --git a/projects/microb2010/mechboard/rdline_config.h b/projects/microb2010/ballboard/rdline_config.h similarity index 100% rename from projects/microb2010/mechboard/rdline_config.h rename to projects/microb2010/ballboard/rdline_config.h diff --git a/projects/microb2010/sensorboard/scheduler_config.h b/projects/microb2010/ballboard/scheduler_config.h similarity index 100% rename from projects/microb2010/sensorboard/scheduler_config.h rename to projects/microb2010/ballboard/scheduler_config.h diff --git a/projects/microb2010/sensorboard/sensor.c b/projects/microb2010/ballboard/sensor.c similarity index 100% rename from projects/microb2010/sensorboard/sensor.c rename to projects/microb2010/ballboard/sensor.c diff --git a/projects/microb2010/sensorboard/sensor.h b/projects/microb2010/ballboard/sensor.h similarity index 100% rename from projects/microb2010/sensorboard/sensor.h rename to projects/microb2010/ballboard/sensor.h diff --git a/projects/microb2010/mechboard/spi_config.h b/projects/microb2010/ballboard/spi_config.h similarity index 100% rename from projects/microb2010/mechboard/spi_config.h rename to projects/microb2010/ballboard/spi_config.h diff --git a/projects/microb2010/sensorboard/time_config.h b/projects/microb2010/ballboard/time_config.h similarity index 100% rename from projects/microb2010/sensorboard/time_config.h rename to projects/microb2010/ballboard/time_config.h diff --git a/projects/microb2010/sensorboard/timer_config.h b/projects/microb2010/ballboard/timer_config.h similarity index 100% rename from projects/microb2010/sensorboard/timer_config.h rename to projects/microb2010/ballboard/timer_config.h diff --git a/projects/microb2010/sensorboard/uart_config.h b/projects/microb2010/ballboard/uart_config.h similarity index 100% rename from projects/microb2010/sensorboard/uart_config.h rename to projects/microb2010/ballboard/uart_config.h diff --git a/projects/microb2010/mechboard/.config b/projects/microb2010/cobboard/.config similarity index 96% rename from projects/microb2010/mechboard/.config rename to projects/microb2010/cobboard/.config index fd14974..706895c 100644 --- a/projects/microb2010/mechboard/.config +++ b/projects/microb2010/cobboard/.config @@ -83,8 +83,9 @@ CONFIG_MODULE_CIRBUF=y # CONFIG_MODULE_FIXED_POINT is not set # CONFIG_MODULE_VECT2 is not set # CONFIG_MODULE_GEOMETRY is not set -# CONFIG_MODULE_GEOMETRY_CREATE_CONFIG is not set +# CONFIG_MODULE_HOSTSIM is not set CONFIG_MODULE_SCHEDULER=y +# CONFIG_MODULE_SCHEDULER_STATS is not set # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set # CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set # CONFIG_MODULE_SCHEDULER_TIMER0 is not set @@ -169,11 +170,13 @@ CONFIG_MODULE_ENCODERS_SPI=y CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y # -# Robot specific modules +# Robot specific modules (fixed point lib may be needed) # # CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set # CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set # CONFIG_MODULE_POSITION_MANAGER is not set +# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set # CONFIG_MODULE_TRAJECTORY_MANAGER is not set CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y # CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set diff --git a/projects/microb2010/mechboard/Makefile b/projects/microb2010/cobboard/Makefile similarity index 91% rename from projects/microb2010/mechboard/Makefile rename to projects/microb2010/cobboard/Makefile index 34efb9f..0c93238 100644 --- a/projects/microb2010/mechboard/Makefile +++ b/projects/microb2010/cobboard/Makefile @@ -9,9 +9,9 @@ LDFLAGS = -T ../common/avr6.x # List C source files here. (C dependencies are automatically generated.) SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c -SRC += commands_cs.c commands_mechboard.c commands.c +SRC += commands_cs.c commands_cobboard.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c -SRC += arm_xy.c state.c ax12_user.c arm_highlevel.c +SRC += state.c ax12_user.c # List Assembler source files here. # Make them always end in a capital .S. Files ending in a lowercase .s diff --git a/projects/microb2010/mechboard/actuator.h b/projects/microb2010/cobboard/actuator.c similarity index 51% rename from projects/microb2010/mechboard/actuator.h rename to projects/microb2010/cobboard/actuator.c index c55f564..67598ea 100644 --- a/projects/microb2010/mechboard/actuator.h +++ b/projects/microb2010/cobboard/actuator.c @@ -15,25 +15,53 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: actuator.h,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $ * */ -#define FINGER_RIGHT 666 -#define FINGER_RIGHT_RELAX 621 -#define FINGER_CENTER_RIGHT 491 -#define FINGER_CENTER 490 -#define FINGER_CENTER_LEFT 489 -#define FINGER_LEFT 340 -#define FINGER_LEFT_RELAX 385 +#include +#include +#include +#include -void finger_goto(uint16_t position); -uint16_t finger_get_goal_pos(void); -uint8_t finger_get_side(void); +#include +#include +#include +#include +#include +#include +#include +#include -void servo_lintel_out(void); -void servo_lintel_1lin(void); -void servo_lintel_2lin(void); +#include +#include +#include +#include -void actuator_init(void); +#include +#include "../common/i2c_commands.h" +#include "actuator.h" +#include "ax12_user.h" +#include "main.h" + +void servo_carry_open(void) +{ +} + +void servo_carry_close(void) +{ +} + +void servo_door_open(void) +{ +} + +void servo_door_close(void) +{ +} + +void actuator_init(void) +{ + +} diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h new file mode 100644 index 0000000..cd2e28b --- /dev/null +++ b/projects/microb2010/cobboard/actuator.h @@ -0,0 +1,27 @@ +/* + * Copyright Droids Corporation (2009) + * + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: actuator.h,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +void actuator_init(void); +void servo_carry_open(void); +void servo_carry_close(void); +void servo_door_open(void); +void servo_door_close(void); + diff --git a/projects/microb2010/sensorboard/adc_config.h b/projects/microb2010/cobboard/adc_config.h similarity index 100% rename from projects/microb2010/sensorboard/adc_config.h rename to projects/microb2010/cobboard/adc_config.h diff --git a/projects/microb2010/sensorboard/ax12_config.h b/projects/microb2010/cobboard/ax12_config.h similarity index 100% rename from projects/microb2010/sensorboard/ax12_config.h rename to projects/microb2010/cobboard/ax12_config.h diff --git a/projects/microb2010/mechboard/ax12_user.c b/projects/microb2010/cobboard/ax12_user.c similarity index 99% rename from projects/microb2010/mechboard/ax12_user.c rename to projects/microb2010/cobboard/ax12_user.c index 9000bbd..b022ef7 100644 --- a/projects/microb2010/mechboard/ax12_user.c +++ b/projects/microb2010/cobboard/ax12_user.c @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/mechboard/ax12_user.h b/projects/microb2010/cobboard/ax12_user.h similarity index 100% rename from projects/microb2010/mechboard/ax12_user.h rename to projects/microb2010/cobboard/ax12_user.h diff --git a/projects/microb2010/mechboard/cmdline.c b/projects/microb2010/cobboard/cmdline.c similarity index 97% rename from projects/microb2010/mechboard/cmdline.c rename to projects/microb2010/cobboard/cmdline.c index f784b35..6de7569 100644 --- a/projects/microb2010/mechboard/cmdline.c +++ b/projects/microb2010/cobboard/cmdline.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -122,7 +122,7 @@ int cmdline_interact(void) int16_t c; rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); - snprintf(gen.prompt, sizeof(gen.prompt), "mechboard > "); + snprintf(gen.prompt, sizeof(gen.prompt), "cobboard > "); rdline_newline(&gen.rdl, gen.prompt); while (1) { diff --git a/projects/microb2010/mechboard/cmdline.h b/projects/microb2010/cobboard/cmdline.h similarity index 100% rename from projects/microb2010/mechboard/cmdline.h rename to projects/microb2010/cobboard/cmdline.h diff --git a/projects/microb2010/mechboard/commands.c b/projects/microb2010/cobboard/commands.c similarity index 82% rename from projects/microb2010/mechboard/commands.c rename to projects/microb2010/cobboard/commands.c index 1c2186d..1723f2a 100644 --- a/projects/microb2010/mechboard/commands.c +++ b/projects/microb2010/cobboard/commands.c @@ -62,25 +62,16 @@ extern parse_pgm_inst_t cmd_cs_status; extern parse_pgm_inst_t cmd_blocking_i; extern parse_pgm_inst_t cmd_blocking_i_show; -/* commands_mechboard.c */ +/* commands_cobboard.c */ extern parse_pgm_inst_t cmd_event; extern parse_pgm_inst_t cmd_color; -extern parse_pgm_inst_t cmd_arm_show; -extern parse_pgm_inst_t cmd_arm_goto; -extern parse_pgm_inst_t cmd_arm_goto_fixed; -extern parse_pgm_inst_t cmd_arm_simulate; -extern parse_pgm_inst_t cmd_finger; -extern parse_pgm_inst_t cmd_pump; extern parse_pgm_inst_t cmd_state1; extern parse_pgm_inst_t cmd_state2; extern parse_pgm_inst_t cmd_state3; -extern parse_pgm_inst_t cmd_state4; -extern parse_pgm_inst_t cmd_state5; extern parse_pgm_inst_t cmd_state_debug; extern parse_pgm_inst_t cmd_state_machine; -extern parse_pgm_inst_t cmd_servo_lintel; -extern parse_pgm_inst_t cmd_pump_current; -extern parse_pgm_inst_t cmd_manivelle; +extern parse_pgm_inst_t cmd_servo_door; +extern parse_pgm_inst_t cmd_servo_carry; extern parse_pgm_inst_t cmd_test; @@ -125,25 +116,16 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_blocking_i, (parse_pgm_inst_t *)&cmd_blocking_i_show, - /* commands_mechboard.c */ + /* commands_cobboard.c */ (parse_pgm_inst_t *)&cmd_event, (parse_pgm_inst_t *)&cmd_color, - (parse_pgm_inst_t *)&cmd_arm_show, - (parse_pgm_inst_t *)&cmd_arm_goto, - (parse_pgm_inst_t *)&cmd_arm_goto_fixed, - (parse_pgm_inst_t *)&cmd_arm_simulate, - (parse_pgm_inst_t *)&cmd_finger, - (parse_pgm_inst_t *)&cmd_pump, (parse_pgm_inst_t *)&cmd_state1, (parse_pgm_inst_t *)&cmd_state2, (parse_pgm_inst_t *)&cmd_state3, - (parse_pgm_inst_t *)&cmd_state4, - (parse_pgm_inst_t *)&cmd_state5, (parse_pgm_inst_t *)&cmd_state_debug, (parse_pgm_inst_t *)&cmd_state_machine, - (parse_pgm_inst_t *)&cmd_servo_lintel, - (parse_pgm_inst_t *)&cmd_pump_current, - (parse_pgm_inst_t *)&cmd_manivelle, + (parse_pgm_inst_t *)&cmd_servo_door, + (parse_pgm_inst_t *)&cmd_servo_carry, (parse_pgm_inst_t *)&cmd_test, NULL, diff --git a/projects/microb2010/mechboard/commands_ax12.c b/projects/microb2010/cobboard/commands_ax12.c similarity index 99% rename from projects/microb2010/mechboard/commands_ax12.c rename to projects/microb2010/cobboard/commands_ax12.c index b052944..ebe3834 100644 --- a/projects/microb2010/mechboard/commands_ax12.c +++ b/projects/microb2010/cobboard/commands_ax12.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c new file mode 100644 index 0000000..b822e4a --- /dev/null +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -0,0 +1,477 @@ +/* + * Copyright Droids Corporation (2009) + * + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: commands_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "sensor.h" +#include "cmdline.h" +#include "state.h" +#include "i2c_protocol.h" +#include "actuator.h" +#include "actuator.h" + +extern uint16_t state_debug; + +struct cmd_event_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + + +/* function called when cmd_event is parsed successfully */ +static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + u08 bit=0; + + struct cmd_event_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("all"))) { + bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + if (!strcmp_P(res->arg2, PSTR("on"))) + cobboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) + cobboard.flags &= bit; + else { /* show */ + printf_P(PSTR("encoders is %s\r\n"), + (DO_ENCODERS & cobboard.flags) ? "on":"off"); + printf_P(PSTR("cs is %s\r\n"), + (DO_CS & cobboard.flags) ? "on":"off"); + printf_P(PSTR("bd is %s\r\n"), + (DO_BD & cobboard.flags) ? "on":"off"); + printf_P(PSTR("power is %s\r\n"), + (DO_POWER & cobboard.flags) ? "on":"off"); + } + return; + } + + if (!strcmp_P(res->arg1, PSTR("encoders"))) + bit = DO_ENCODERS; + else if (!strcmp_P(res->arg1, PSTR("cs"))) { + bit = DO_CS; + } + else if (!strcmp_P(res->arg1, PSTR("bd"))) + bit = DO_BD; + else if (!strcmp_P(res->arg1, PSTR("power"))) + bit = DO_POWER; + + + if (!strcmp_P(res->arg2, PSTR("on"))) + cobboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) { + if (!strcmp_P(res->arg1, PSTR("cs"))) { + pwm_ng_set(LEFT_SPICKLE_PWM, 0); + pwm_ng_set(RIGHT_SPICKLE_PWM, 0); + pwm_ng_set(SHOVEL_PWM, 0); + } + cobboard.flags &= (~bit); + } + printf_P(PSTR("%s is %s\r\n"), res->arg1, + (bit & cobboard.flags) ? "on":"off"); +} + +prog_char str_event_arg0[] = "event"; +parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); +prog_char str_event_arg1[] = "all#encoders#cs#bd#power"; +parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); +prog_char str_event_arg2[] = "on#off#show"; +parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); + +prog_char help_event[] = "Enable/disable events"; +parse_pgm_inst_t cmd_event = { + .f = cmd_event_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_event, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Color */ + +/* this structure is filled when cmd_color is parsed successfully */ +struct cmd_color_result { + fixed_string_t arg0; + fixed_string_t color; +}; + +/* function called when cmd_color is parsed successfully */ +static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; + if (!strcmp_P(res->color, PSTR("red"))) { + cobboard.our_color = I2C_COLOR_RED; + } + else if (!strcmp_P(res->color, PSTR("green"))) { + cobboard.our_color = I2C_COLOR_GREEN; + } + printf_P(PSTR("Done\r\n")); +} + +prog_char str_color_arg0[] = "color"; +parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0); +prog_char str_color_color[] = "green#red"; +parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color); + +prog_char help_color[] = "Set our color"; +parse_pgm_inst_t cmd_color = { + .f = cmd_color_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_color, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, + NULL, + }, +}; + +/**********************************************************/ +/* State1 */ + +/* this structure is filled when cmd_state1 is parsed successfully */ +struct cmd_state1_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_state1 is parsed successfully */ +static void cmd_state1_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state1_result *res = parsed_result; + struct i2c_cmd_cobboard_set_mode command; + + if (!strcmp_P(res->arg1, PSTR("init"))) { + state_init(); + return; + } + + if (!strcmp_P(res->arg1, PSTR("manual"))) + command.mode = I2C_COBBOARD_MODE_MANUAL; + else if (!strcmp_P(res->arg1, PSTR("harvest"))) + command.mode = I2C_COBBOARD_MODE_HARVEST; + state_set_mode(&command); +} + +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#manual"; +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"; +parse_pgm_inst_t cmd_state1 = { + .f = cmd_state1_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state1_arg0, + (prog_void *)&cmd_state1_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* State2 */ + +/* this structure is filled when cmd_state2 is parsed successfully */ +struct cmd_state2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_state2 is parsed successfully */ +static void cmd_state2_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state2_result *res = parsed_result; + struct i2c_cmd_cobboard_set_mode command; + uint8_t side; + + if (!strcmp_P(res->arg2, PSTR("left"))) + side = I2C_LEFT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("right"))) + side = I2C_RIGHT_SIDE; + + if (!strcmp_P(res->arg1, PSTR("yyy"))) { + } + else if (!strcmp_P(res->arg1, PSTR("xxx"))) { + } + + + state_set_mode(&command); +} + +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[] = "xxx"; +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); + +prog_char help_state2[] = "set cobboard mode"; +parse_pgm_inst_t cmd_state2 = { + .f = cmd_state2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state2_arg0, + (prog_void *)&cmd_state2_arg1, + (prog_void *)&cmd_state2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* State3 */ + +/* this structure is filled when cmd_state3 is parsed successfully */ +struct cmd_state3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t arg2; +}; + +/* function called when cmd_state3 is parsed successfully */ +static void cmd_state3_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state3_result *res = parsed_result; + struct i2c_cmd_cobboard_set_mode command; + + if (!strcmp_P(res->arg1, PSTR("xxx"))) { + /* xxx = res->arg2 */ + } + else if (!strcmp_P(res->arg1, PSTR("yyy"))) { + } + state_set_mode(&command); +} + +prog_char str_state3_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0); +prog_char str_state3_arg1[] = "xxx"; +parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1); +parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8); + +prog_char help_state3[] = "set cobboard mode"; +parse_pgm_inst_t cmd_state3 = { + .f = cmd_state3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state3_arg0, + (prog_void *)&cmd_state3_arg1, + (prog_void *)&cmd_state3_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* State_Machine */ + +/* this structure is filled when cmd_state_machine is parsed successfully */ +struct cmd_state_machine_result { + fixed_string_t arg0; +}; + +/* function called when cmd_state_machine is parsed successfully */ +static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + state_machine(); +} + +prog_char str_state_machine_arg0[] = "state_machine"; +parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0); + +prog_char help_state_machine[] = "launch state machine"; +parse_pgm_inst_t cmd_state_machine = { + .f = cmd_state_machine_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state_machine, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state_machine_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* State_Debug */ + +/* this structure is filled when cmd_state_debug is parsed successfully */ +struct cmd_state_debug_result { + fixed_string_t arg0; + uint8_t on; +}; + +/* function called when cmd_state_debug is parsed successfully */ +static void cmd_state_debug_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state_debug_result *res = parsed_result; + state_debug = res->on; +} + +prog_char str_state_debug_arg0[] = "state_debug"; +parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0); +parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8); + +prog_char help_state_debug[] = "Set debug timer for state machine"; +parse_pgm_inst_t cmd_state_debug = { + .f = cmd_state_debug_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state_debug, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state_debug_arg0, + (prog_void *)&cmd_state_debug_on, + NULL, + }, +}; + +/**********************************************************/ +/* Servo_Door */ + +/* this structure is filled when cmd_servo_door is parsed successfully */ +struct cmd_servo_door_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_servo_door is parsed successfully */ +static void cmd_servo_door_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_servo_door_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("open"))) + servo_door_open(); + else if (!strcmp_P(res->arg1, PSTR("closed"))) + servo_door_close(); +} + +prog_char str_servo_door_arg0[] = "door"; +parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0); +prog_char str_servo_door_arg1[] = "open#closed"; +parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1); + +prog_char help_servo_door[] = "Servo door function"; +parse_pgm_inst_t cmd_servo_door = { + .f = cmd_servo_door_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo_door, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_servo_door_arg0, + (prog_void *)&cmd_servo_door_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Servo_Carry */ + +/* this structure is filled when cmd_servo_carry is parsed successfully */ +struct cmd_servo_carry_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_servo_carry is parsed successfully */ +static void cmd_servo_carry_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_servo_carry_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("open"))) + servo_carry_open(); + else if (!strcmp_P(res->arg1, PSTR("closed"))) + servo_carry_close(); +} + +prog_char str_servo_carry_arg0[] = "carry"; +parse_pgm_token_string_t cmd_servo_carry_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg0, str_servo_carry_arg0); +prog_char str_servo_carry_arg1[] = "open#closed"; +parse_pgm_token_string_t cmd_servo_carry_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg1, str_servo_carry_arg1); + +prog_char help_servo_carry[] = "Servo carry function"; +parse_pgm_inst_t cmd_servo_carry = { + .f = cmd_servo_carry_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo_carry, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_servo_carry_arg0, + (prog_void *)&cmd_servo_carry_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Test */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_test is parsed successfully */ +static void cmd_test_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ +} + +prog_char str_test_arg0[] = "test"; +parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); + +prog_char help_test[] = "Test function"; +parse_pgm_inst_t cmd_test = { + .f = cmd_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_test_arg0, + NULL, + }, +}; diff --git a/projects/microb2010/mechboard/commands_cs.c b/projects/microb2010/cobboard/commands_cs.c similarity index 97% rename from projects/microb2010/mechboard/commands_cs.c rename to projects/microb2010/cobboard/commands_cs.c index c2937f5..27be783 100644 --- a/projects/microb2010/mechboard/commands_cs.c +++ b/projects/microb2010/cobboard/commands_cs.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -51,11 +51,13 @@ struct csb_list { struct cs_block *csb; }; -prog_char csb_left_arm_str[] = "left_arm"; -prog_char csb_right_arm_str[] = "right_arm"; +prog_char csb_left_spickle_str[] = "left_spickle"; +prog_char csb_right_spickle_str[] = "right_spickle"; +prog_char csb_shovel_str[] = "shovel"; struct csb_list csb_list[] = { - { .name = csb_left_arm_str, .csb = &mechboard.left_arm }, - { .name = csb_right_arm_str, .csb = &mechboard.right_arm }, + { .name = csb_left_spickle_str, .csb = &cobboard.left_spickle }, + { .name = csb_right_spickle_str, .csb = &cobboard.right_spickle }, + { .name = csb_shovel_str, .csb = &cobboard.shovel }, }; struct cmd_cs_result { @@ -64,7 +66,7 @@ struct cmd_cs_result { }; /* token to be used for all cs-related commands */ -prog_char str_csb_name[] = "left_arm#right_arm"; +prog_char str_csb_name[] = "left_spickle#right_spickle#shovel"; parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name); struct cs_block *cs_from_name(const char *name) diff --git a/projects/microb2010/mechboard/commands_gen.c b/projects/microb2010/cobboard/commands_gen.c similarity index 98% rename from projects/microb2010/mechboard/commands_gen.c rename to projects/microb2010/cobboard/commands_gen.c index 6517ea0..393f874 100644 --- a/projects/microb2010/mechboard/commands_gen.c +++ b/projects/microb2010/cobboard/commands_gen.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -360,8 +360,6 @@ static const prog_char i2c_log[] = "i2c"; static const prog_char i2cproto_log[] = "i2cproto"; static const prog_char sensor_log[] = "sensor"; static const prog_char block_log[] = "bd"; -static const prog_char arm_log[] = "arm"; -static const prog_char finger_log[] = "finger"; static const prog_char state_log[] = "state"; static const prog_char ax12_log[] = "ax12"; @@ -376,8 +374,6 @@ static const struct log_name_and_num log_name_and_num[] = { { i2cproto_log, E_USER_I2C_PROTO }, { sensor_log, E_USER_SENSOR }, { block_log, E_BLOCKING_DETECTION_MANAGER }, - { arm_log, E_USER_ARM }, - { finger_log, E_USER_FINGER }, { state_log, E_USER_ST_MACH }, { ax12_log, E_USER_AX12 }, }; @@ -528,7 +524,7 @@ static void cmd_log_type_parsed(void * parsed_result, __attribute__((unused)) vo prog_char str_log_arg1_type[] = "type"; parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type); /* keep it sync with log_name_and_num above */ -prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#arm#finger#state#ax12"; +prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd##state#ax12"; parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type); prog_char str_log_arg3[] = "on#off"; parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3); diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c new file mode 100644 index 0000000..4d8ccb0 --- /dev/null +++ b/projects/microb2010/cobboard/cs.c @@ -0,0 +1,192 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: cs.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "actuator.h" + +/* called every 5 ms */ +static void do_cs(__attribute__((unused)) void *dummy) +{ + /* read encoders */ + if (cobboard.flags & DO_ENCODERS) { + encoders_spi_manage(NULL); + } + /* control system */ + if (cobboard.flags & DO_CS) { + if (cobboard.left_spickle.on) + cs_manage(&cobboard.left_spickle.cs); + if (cobboard.right_spickle.on) + cs_manage(&cobboard.right_spickle.cs); + if (cobboard.shovel.on) + cs_manage(&cobboard.shovel.cs); + } + if (cobboard.flags & DO_BD) { + bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs); + bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs); + bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs); + } + if (cobboard.flags & DO_POWER) + BRAKE_OFF(); + else + BRAKE_ON(); +} + +void dump_cs_debug(const char *name, struct cs *cs) +{ + DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld " + "in=% .5ld out=% .5ld", + name, cs_get_consign(cs), cs_get_filtered_consign(cs), + cs_get_error(cs), cs_get_filtered_feedback(cs), + cs_get_out(cs)); +} + +void dump_cs(const char *name, struct cs *cs) +{ + printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " + "in=% .5ld out=% .5ld\r\n"), + name, cs_get_consign(cs), cs_get_filtered_consign(cs), + cs_get_error(cs), cs_get_filtered_feedback(cs), + cs_get_out(cs)); +} + +void dump_pid(const char *name, struct pid_filter *pid) +{ + printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + name, + pid_get_value_in(pid) * pid_get_gain_P(pid), + pid_get_value_I(pid) * pid_get_gain_I(pid), + pid_get_value_D(pid) * pid_get_gain_D(pid), + pid_get_value_out(pid)); +} + +void microb_cs_init(void) +{ + /* ---- CS left_spickle */ + /* PID */ + pid_init(&cobboard.left_spickle.pid); + pid_set_gains(&cobboard.left_spickle.pid, 500, 40, 5000); + pid_set_maximums(&cobboard.left_spickle.pid, 0, 5000, 2400); /* max is 12 V */ + pid_set_out_shift(&cobboard.left_spickle.pid, 10); + pid_set_derivate_filter(&cobboard.left_spickle.pid, 4); + + /* QUADRAMP */ + quadramp_init(&cobboard.left_spickle.qr); + quadramp_set_1st_order_vars(&cobboard.left_spickle.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&cobboard.left_spickle.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&cobboard.left_spickle.cs); + cs_set_consign_filter(&cobboard.left_spickle.cs, quadramp_do_filter, &cobboard.left_spickle.qr); + cs_set_correct_filter(&cobboard.left_spickle.cs, pid_do_filter, &cobboard.left_spickle.pid); + cs_set_process_in(&cobboard.left_spickle.cs, pwm_ng_set, LEFT_SPICKLE_PWM); + cs_set_process_out(&cobboard.left_spickle.cs, encoders_spi_get_value, LEFT_SPICKLE_ENCODER); + cs_set_consign(&cobboard.left_spickle.cs, 0); + + /* Blocking detection */ + bd_init(&cobboard.left_spickle.bd); + bd_set_speed_threshold(&cobboard.left_spickle.bd, 150); + bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40); + + /* ---- CS right_spickle */ + /* PID */ + pid_init(&cobboard.right_spickle.pid); + pid_set_gains(&cobboard.right_spickle.pid, 500, 40, 5000); + pid_set_maximums(&cobboard.right_spickle.pid, 0, 5000, 2400); /* max is 12 V */ + pid_set_out_shift(&cobboard.right_spickle.pid, 10); + pid_set_derivate_filter(&cobboard.right_spickle.pid, 6); + + /* QUADRAMP */ + quadramp_init(&cobboard.right_spickle.qr); + quadramp_set_1st_order_vars(&cobboard.right_spickle.qr, 1000, 1000); /* set speed */ + quadramp_set_2nd_order_vars(&cobboard.right_spickle.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&cobboard.right_spickle.cs); + cs_set_consign_filter(&cobboard.right_spickle.cs, quadramp_do_filter, &cobboard.right_spickle.qr); + cs_set_correct_filter(&cobboard.right_spickle.cs, pid_do_filter, &cobboard.right_spickle.pid); + cs_set_process_in(&cobboard.right_spickle.cs, pwm_ng_set, RIGHT_SPICKLE_PWM); + cs_set_process_out(&cobboard.right_spickle.cs, encoders_spi_get_value, RIGHT_SPICKLE_ENCODER); + cs_set_consign(&cobboard.right_spickle.cs, 0); + + /* Blocking detection */ + bd_init(&cobboard.right_spickle.bd); + bd_set_speed_threshold(&cobboard.right_spickle.bd, 150); + bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40); + + /* ---- CS shovel */ + /* PID */ + pid_init(&cobboard.shovel.pid); + pid_set_gains(&cobboard.shovel.pid, 500, 40, 5000); + pid_set_maximums(&cobboard.shovel.pid, 0, 5000, 2400); /* max is 12 V */ + pid_set_out_shift(&cobboard.shovel.pid, 10); + pid_set_derivate_filter(&cobboard.shovel.pid, 6); + + /* QUADRAMP */ + quadramp_init(&cobboard.shovel.qr); + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 1000); /* set speed */ + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&cobboard.shovel.cs); + cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr); + cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid); + cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM); + cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER); + cs_set_consign(&cobboard.shovel.cs, 0); + + /* Blocking detection */ + bd_init(&cobboard.shovel.bd); + bd_set_speed_threshold(&cobboard.shovel.bd, 150); + bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40); + + /* set them on !! */ + cobboard.left_spickle.on = 1; + cobboard.right_spickle.on = 1; + cobboard.shovel.on = 1; + + scheduler_add_periodical_event_priority(do_cs, NULL, + CS_PERIOD / SCHEDULER_UNIT, + CS_PRIO); +} diff --git a/projects/microb2010/mechboard/cs.h b/projects/microb2010/cobboard/cs.h similarity index 100% rename from projects/microb2010/mechboard/cs.h rename to projects/microb2010/cobboard/cs.h diff --git a/projects/microb2010/mechboard/diagnostic_config.h b/projects/microb2010/cobboard/diagnostic_config.h similarity index 100% rename from projects/microb2010/mechboard/diagnostic_config.h rename to projects/microb2010/cobboard/diagnostic_config.h diff --git a/projects/microb2010/mechboard/encoders_spi_config.h b/projects/microb2010/cobboard/encoders_spi_config.h similarity index 100% rename from projects/microb2010/mechboard/encoders_spi_config.h rename to projects/microb2010/cobboard/encoders_spi_config.h diff --git a/projects/microb2010/mechboard/error_config.h b/projects/microb2010/cobboard/error_config.h similarity index 100% rename from projects/microb2010/mechboard/error_config.h rename to projects/microb2010/cobboard/error_config.h diff --git a/projects/microb2010/mechboard/i2c_config.h b/projects/microb2010/cobboard/i2c_config.h similarity index 100% rename from projects/microb2010/mechboard/i2c_config.h rename to projects/microb2010/cobboard/i2c_config.h diff --git a/projects/microb2010/mechboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c similarity index 74% rename from projects/microb2010/mechboard/i2c_protocol.c rename to projects/microb2010/cobboard/i2c_protocol.c index 902207b..b790128 100644 --- a/projects/microb2010/mechboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -87,33 +87,19 @@ static void i2c_test(uint16_t val) static void i2c_send_status(void) { - struct i2c_ans_mechboard_status ans; + struct i2c_ans_cobboard_status ans; i2c_flush(); - ans.hdr.cmd = I2C_ANS_MECHBOARD_STATUS; + ans.hdr.cmd = I2C_ANS_COBBOARD_STATUS; /* status */ ans.mode = state_get_mode(); - ans.status = mechboard.status; - - ans.lintel_count = mechboard.lintel_count; - ans.column_flags = mechboard.column_flags; - /* pumps pwm */ - ans.pump_left1 = mechboard.pump_left1; - ans.pump_right1 = mechboard.pump_right1; - ans.pump_left2 = mechboard.pump_left2; - ans.pump_right2 = mechboard.pump_right2; - /* pumps current */ - ans.pump_right1_current = sensor_get_adc(ADC_CSENSE3); - ans.pump_right2_current = sensor_get_adc(ADC_CSENSE4); - /* servos */ - ans.servo_lintel_left = mechboard.servo_lintel_left; - ans.servo_lintel_right = mechboard.servo_lintel_right; + ans.status = 0x55; i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, sizeof(ans), I2C_CTRL_GENERIC); } -static int8_t i2c_set_mode(struct i2c_cmd_mechboard_set_mode *cmd) +static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) { state_set_mode(cmd); return 0; @@ -145,10 +131,10 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } - case I2C_CMD_MECHBOARD_SET_MODE: + case I2C_CMD_COBBOARD_SET_MODE: { - struct i2c_cmd_mechboard_set_mode *cmd = void_cmd; - if (size != sizeof(struct i2c_cmd_mechboard_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); break; @@ -159,7 +145,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) struct i2c_cmd_generic_color *cmd = void_cmd; if (size != sizeof (*cmd)) goto error; - mechboard.our_color = cmd->color; + cobboard.our_color = cmd->color; break; } @@ -177,14 +163,12 @@ void i2c_recvevent(uint8_t * buf, int8_t size) /* Add other commands here ...*/ - case I2C_REQ_MECHBOARD_STATUS: + case I2C_REQ_COBBOARD_STATUS: { - struct i2c_req_mechboard_status *cmd = void_cmd; - if (size != sizeof (struct i2c_req_mechboard_status)) + //struct i2c_req_cobboard_status *cmd = void_cmd; + if (size != sizeof (struct i2c_req_cobboard_status)) goto error; - mechboard.pump_left1_current = cmd->pump_left1_current; - mechboard.pump_left2_current = cmd->pump_left2_current; i2c_send_status(); break; } diff --git a/projects/microb2010/mechboard/i2c_protocol.h b/projects/microb2010/cobboard/i2c_protocol.h similarity index 100% rename from projects/microb2010/mechboard/i2c_protocol.h rename to projects/microb2010/cobboard/i2c_protocol.h diff --git a/projects/microb2010/mechboard/main.c b/projects/microb2010/cobboard/main.c similarity index 93% rename from projects/microb2010/mechboard/main.c rename to projects/microb2010/cobboard/main.c index 27c0138..418f5e1 100755 --- a/projects/microb2010/mechboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include @@ -74,7 +74,7 @@ */ struct genboard gen; -struct mechboard mechboard; +struct cobboard cobboard; /***********************/ @@ -150,9 +150,9 @@ int main(void) DDRL = 0xc0; LED1_OFF(); memset(&gen, 0, sizeof(gen)); - memset(&mechboard, 0, sizeof(mechboard)); + memset(&cobboard, 0, sizeof(cobboard)); /* cs is enabled after arm_calibrate() */ - mechboard.flags = DO_ENCODERS | DO_POWER; // DO_BD + cobboard.flags = DO_ENCODERS | DO_POWER; // DO_BD /* UART */ uart_init(); @@ -166,10 +166,10 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MECHBOARD); + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != - EEPROM_MAGIC_MECHBOARD) { + EEPROM_MAGIC_COBBOARD) { sei(); printf_P(PSTR("Bad eeprom value\r\n")); while(1); @@ -187,7 +187,7 @@ int main(void) /* I2C */ i2c_protocol_init(); - i2c_init(I2C_MODE_SLAVE, I2C_MECHBOARD_ADDR); + i2c_init(I2C_MODE_SLAVE, I2C_COBBOARD_ADDR); i2c_register_recv_event(i2c_recvevent); /* TIMER */ @@ -257,10 +257,7 @@ int main(void) /* arm management */ gen.logs[0] = E_USER_ST_MACH; gen.log_level = 5; - arm_init(); - mechboard.flags |= DO_CS; - arm_do_xy(&left_arm, 60, -80, 0); - arm_do_xy(&right_arm, 60, -80, 0); + cobboard.flags |= DO_CS; state_machine(); cmdline_interact(); diff --git a/projects/microb2010/mechboard/main.h b/projects/microb2010/cobboard/main.h similarity index 78% rename from projects/microb2010/mechboard/main.h rename to projects/microb2010/cobboard/main.h index 5422f6c..33fb833 100755 --- a/projects/microb2010/mechboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -46,26 +46,18 @@ #define BRAKE_ON() do { PORTJ |= 0xF0; } while(0) #define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0) -#define LEFT_ARM_ENCODER ((void *)0) -#define RIGHT_ARM_ENCODER ((void *)1) +#define LEFT_SPICKLE_ENCODER ((void *)0) +#define RIGHT_SPICKLE_ENCODER ((void *)1) +#define SHOVEL_ENCODER ((void *)1) -#define LEFT_ARM_PWM ((void *)&gen.pwm1_4A) -#define RIGHT_ARM_PWM ((void *)&gen.pwm2_4B) - -#define RIGHT_PUMP1_PWM ((void *)&gen.pwm3_1A) -#define RIGHT_PUMP2_PWM ((void *)&gen.pwm4_1B) - -#define R_ELBOW_AX12 1 -#define R_WRIST_AX12 2 -#define L_ELBOW_AX12 4 -#define L_WRIST_AX12 3 -#define FINGER_AX12 5 +#define LEFT_SPICKLE_PWM ((void *)&gen.pwm1_4A) +#define RIGHT_SPICKLE_PWM ((void *)&gen.pwm2_4B) +#define SHOVEL_PWM ((void *)&gen.pwm2_4B) /** ERROR NUMS */ #define E_USER_I2C_PROTO 195 #define E_USER_SENSOR 196 -#define E_USER_ARM 197 -#define E_USER_FINGER 198 +#define E_USER_SPICKLE 197 #define E_USER_ST_MACH 199 #define E_USER_CS 200 #define E_USER_AX12 201 @@ -74,7 +66,6 @@ #define TIME_PRIO 160 #define ADC_PRIO 120 #define CS_PRIO 100 -#define ARM_PRIO 50 #define I2C_POLL_PRIO 20 #define CS_PERIOD 5000L @@ -116,8 +107,8 @@ struct cs_block { struct blocking_detection bd; }; -/* mechboard specific */ -struct mechboard { +/* cobboard specific */ +struct cobboard { #define DO_ENCODERS 1 #define DO_CS 2 #define DO_BD 4 @@ -125,33 +116,18 @@ struct mechboard { uint8_t flags; /* misc flags */ /* control systems */ - struct cs_block left_arm; - struct cs_block right_arm; + struct cs_block left_spickle; + struct cs_block right_spickle; + struct cs_block shovel; /* robot status */ uint8_t our_color; - volatile uint8_t lintel_count; - volatile uint8_t column_flags; + volatile uint8_t cob_count; volatile uint8_t status; - - /* local pumps */ - int16_t pump_right1; - int16_t pump_right2; - /* remote pump (on mainboard) */ - int16_t pump_left1; - int16_t pump_left2; - - /* remote pump current */ - int16_t pump_left1_current; - int16_t pump_left2_current; - - uint16_t servo_lintel_left; - uint16_t servo_lintel_right; - }; extern struct genboard gen; -extern struct mechboard mechboard; +extern struct cobboard cobboard; /* start the bootloader */ void bootloader(void); diff --git a/projects/microb2010/sensorboard/pid_config.h b/projects/microb2010/cobboard/pid_config.h similarity index 100% rename from projects/microb2010/sensorboard/pid_config.h rename to projects/microb2010/cobboard/pid_config.h diff --git a/projects/microb2010/sensorboard/rdline_config.h b/projects/microb2010/cobboard/rdline_config.h similarity index 100% rename from projects/microb2010/sensorboard/rdline_config.h rename to projects/microb2010/cobboard/rdline_config.h diff --git a/projects/microb2010/mechboard/scheduler_config.h b/projects/microb2010/cobboard/scheduler_config.h similarity index 100% rename from projects/microb2010/mechboard/scheduler_config.h rename to projects/microb2010/cobboard/scheduler_config.h diff --git a/projects/microb2010/mechboard/sensor.c b/projects/microb2010/cobboard/sensor.c similarity index 100% rename from projects/microb2010/mechboard/sensor.c rename to projects/microb2010/cobboard/sensor.c diff --git a/projects/microb2010/mechboard/sensor.h b/projects/microb2010/cobboard/sensor.h similarity index 100% rename from projects/microb2010/mechboard/sensor.h rename to projects/microb2010/cobboard/sensor.h diff --git a/projects/microb2010/sensorboard/spi_config.h b/projects/microb2010/cobboard/spi_config.h similarity index 100% rename from projects/microb2010/sensorboard/spi_config.h rename to projects/microb2010/cobboard/spi_config.h diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c new file mode 100644 index 0000000..f9aefa6 --- /dev/null +++ b/projects/microb2010/cobboard/state.c @@ -0,0 +1,163 @@ +/* + * Copyright Droids Corporation (2009) + * + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "cmdline.h" +#include "sensor.h" +#include "actuator.h" +#include "state.h" + +#define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args) +#define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args) +#define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args) + +/* shorter aliases for this file */ +#define INIT I2C_COBBOARD_MODE_INIT +#define MANUAL I2C_COBBOARD_MODE_MANUAL +#define HARVEST I2C_COBBOARD_MODE_HARVEST +#define EXIT I2C_COBBOARD_MODE_EXIT + +static struct i2c_cmd_cobboard_set_mode mainboard_command; +static struct vt100 local_vt100; +static volatile uint8_t prev_state; +static volatile uint8_t changed = 0; + +uint8_t state_debug = 0; + +void state_dump_sensors(void) +{ + STMCH_DEBUG("TODO\n"); +} + +void state_debug_wait_key_pressed(void) +{ + if (!state_debug) + return; + printf_P(PSTR("press a key\r\n")); + while(!cmdline_keypressed()); +} + +/* set a new state, return 0 on success */ +int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +{ + changed = 1; + prev_state = mainboard_command.mode; + memcpy(&mainboard_command, cmd, sizeof(mainboard_command)); + STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode); + return 0; +} + +/* check that state is the one in parameter and that state did not + * changed */ +uint8_t state_check(uint8_t mode) +{ + int16_t c; + if (mode != mainboard_command.mode) + return 0; + + if (changed) + return 0; + + /* force quit when CTRL-C is typed */ + c = cmdline_getchar(); + if (c == -1) + return 1; + if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) { + mainboard_command.mode = EXIT; + return 0; + } + return 1; +} + +uint8_t state_get_mode(void) +{ + return mainboard_command.mode; +} + +/* manual mode, arm position is sent from mainboard */ +static void state_do_manual(void) +{ + if (!state_check(MANUAL)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(MANUAL)); +} + +/* init mode */ +static void state_do_init(void) +{ + if (!state_check(INIT)) + return; + state_init(); + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(INIT)); +} + +/* harvest columns elts from area */ +static void state_do_harvest(void) +{ + if (!state_check(HARVEST)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(HARVEST)); +} +/* main state machine */ +void state_machine(void) +{ + while (state_get_mode() != EXIT) { + changed = 0; + state_do_init(); + state_do_manual(); + state_do_harvest(); + } +} + +void state_init(void) +{ + vt100_init(&local_vt100); + mainboard_command.mode = HARVEST; + cobboard.cob_count = 0; +} diff --git a/projects/microb2010/mechboard/state.h b/projects/microb2010/cobboard/state.h similarity index 88% rename from projects/microb2010/mechboard/state.h rename to projects/microb2010/cobboard/state.h index 0b49890..889970b 100644 --- a/projects/microb2010/mechboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -22,12 +22,8 @@ #ifndef _STATE_H_ #define _STATE_H_ -extern volatile uint8_t lintel_count; - -void state_manivelle(int16_t step_deg); - /* set a new state, return 0 on success */ -int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd); +int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd); /* get current state */ uint8_t state_get_mode(void); diff --git a/projects/microb2010/mechboard/time_config.h b/projects/microb2010/cobboard/time_config.h similarity index 100% rename from projects/microb2010/mechboard/time_config.h rename to projects/microb2010/cobboard/time_config.h diff --git a/projects/microb2010/mechboard/timer_config.h b/projects/microb2010/cobboard/timer_config.h similarity index 100% rename from projects/microb2010/mechboard/timer_config.h rename to projects/microb2010/cobboard/timer_config.h diff --git a/projects/microb2010/mechboard/uart_config.h b/projects/microb2010/cobboard/uart_config.h similarity index 100% rename from projects/microb2010/mechboard/uart_config.h rename to projects/microb2010/cobboard/uart_config.h diff --git a/projects/microb2010/common/eeprom_mapping.h b/projects/microb2010/common/eeprom_mapping.h index 01bd6e3..5ea6125 100644 --- a/projects/microb2010/common/eeprom_mapping.h +++ b/projects/microb2010/common/eeprom_mapping.h @@ -23,8 +23,8 @@ #define _EEPROM_MAPPING_H_ #define EEPROM_MAGIC_MAINBOARD 1 -#define EEPROM_MAGIC_MECHBOARD 2 -#define EEPROM_MAGIC_SENSORBOARD 3 +#define EEPROM_MAGIC_COBBOARD 2 +#define EEPROM_MAGIC_BALLBOARD 3 #define EEPROM_MAGIC_ADDRESS ((uint8_t *)0x100) diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index fdb5d49..35605b8 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -23,19 +23,17 @@ #define _I2C_COMMANDS_H_ #define I2C_MAINBOARD_ADDR 1 -#define I2C_MECHBOARD_ADDR 2 -#define I2C_SENSORBOARD_ADDR 3 +#define I2C_COBBOARD_ADDR 2 +#define I2C_BALLBOARD_ADDR 3 #define I2C_LEFT_SIDE 0 #define I2C_RIGHT_SIDE 1 -#define I2C_AUTO_SIDE 2 /* for prepare_pickup */ -#define I2C_CENTER_SIDE 3 /* for prepare_pickup */ +#define I2C_AUTO_SIDE 2 +#define I2C_CENTER_SIDE 3 #define I2C_COLOR_RED 0 #define I2C_COLOR_GREEN 1 -#define I2C_AUTOBUILD_DEFAULT_DIST 210 - struct i2c_cmd_hdr { uint8_t cmd; }; @@ -63,146 +61,21 @@ struct i2c_cmd_generic_color { /****/ -#define I2C_CMD_MECHBOARD_SET_MODE 0x02 +#define I2C_CMD_COBBOARD_SET_MODE 0x02 -struct i2c_cmd_mechboard_set_mode { +struct i2c_cmd_cobboard_set_mode { struct i2c_cmd_hdr hdr; -#define I2C_MECHBOARD_MODE_MANUAL 0x00 -#define I2C_MECHBOARD_MODE_HARVEST 0x01 -#define I2C_MECHBOARD_MODE_PREPARE_PICKUP 0x02 -#define I2C_MECHBOARD_MODE_PICKUP 0x03 -#define I2C_MECHBOARD_MODE_PREPARE_BUILD 0x04 -#define I2C_MECHBOARD_MODE_AUTOBUILD 0x05 -#define I2C_MECHBOARD_MODE_WAIT 0x06 -#define I2C_MECHBOARD_MODE_INIT 0x07 -#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08 -#define I2C_MECHBOARD_MODE_GET_LINTEL 0x09 -#define I2C_MECHBOARD_MODE_PUT_LINTEL 0x0A -#define I2C_MECHBOARD_MODE_PREPARE_EJECT 0x0B -#define I2C_MECHBOARD_MODE_EJECT 0x0C -#define I2C_MECHBOARD_MODE_CLEAR 0x0D -#define I2C_MECHBOARD_MODE_LAZY_HARVEST 0x0E -#define I2C_MECHBOARD_MODE_LOADED 0x0F -#define I2C_MECHBOARD_MODE_PREPARE_INSIDE 0x10 -#define I2C_MECHBOARD_MODE_STORE 0x11 -#define I2C_MECHBOARD_MODE_LAZY_PICKUP 0x12 -#define I2C_MECHBOARD_MODE_MANIVELLE 0x13 -#define I2C_MECHBOARD_MODE_PUSH_TEMPLE 0x14 -#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC 0x15 -#define I2C_MECHBOARD_MODE_EXIT 0xFF +#define I2C_COBBOARD_MODE_INIT 0x00 +#define I2C_COBBOARD_MODE_MANUAL 0x01 +#define I2C_COBBOARD_MODE_HARVEST 0x02 +#define I2C_COBBOARD_MODE_EXIT 0xFF uint8_t mode; union { struct { - } manual; struct { - uint8_t side; - uint8_t next_mode; - } prep_pickup; - - struct { - uint8_t level_l; - uint8_t level_r; - } prep_build; - - struct { - uint8_t side; - } push_temple_disc; - - struct { - uint8_t level; - } push_temple; - - struct { - uint8_t level_left; - uint8_t level_right; - uint8_t count_left; - uint8_t count_right; - uint8_t distance_left; - uint8_t distance_right; - uint8_t do_lintel; - } autobuild; - - struct { - uint8_t level_l; - uint8_t level_r; - } prep_inside; - }; -}; - -/****/ - -/* only valid in manual mode */ -#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03 - -struct i2c_cmd_mechboard_arm_goto { - struct i2c_cmd_hdr hdr; -#define I2C_MECHBOARD_ARM_LEFT 0 -#define I2C_MECHBOARD_ARM_RIGHT 1 -#define I2C_MECHBOARD_ARM_BOTH 2 - uint8_t which; - - uint8_t height; /* in cm */ - uint8_t distance; /* in cm */ -}; - -/****/ - -#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04 - -struct i2c_cmd_sensorboard_start_beacon { - struct i2c_cmd_hdr hdr; - uint8_t enable; -}; - - -/****/ - -#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05 - -struct i2c_cmd_sensorboard_scanner { - struct i2c_cmd_hdr hdr; - -#define I2C_SENSORBOARD_SCANNER_STOP 0x00 -#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01 -#define I2C_SENSORBOARD_SCANNER_START 0x02 - uint8_t mode; -}; - -/*****/ - -#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06 -struct i2c_cmd_sensorboard_calib_scanner { - struct i2c_cmd_hdr hdr; -}; - -/*****/ - -#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07 -struct i2c_cmd_sensorboard_scanner_algo { - struct i2c_cmd_hdr hdr; - -#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1 -#define I2C_SCANNER_ALGO_CHECK_TEMPLE 2 -#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3 - uint8_t algo; - - union { - struct { -#define I2C_SCANNER_ZONE_0 0 -#define I2C_SCANNER_ZONE_1 1 -#define I2C_SCANNER_ZONE_DISC 2 - uint8_t working_zone; - int16_t center_x; - int16_t center_y; - } drop_zone; - - struct { - uint8_t level; - int16_t temple_x; - int16_t temple_y; - } check_temple; + } harvest; }; }; @@ -211,85 +84,46 @@ struct i2c_cmd_sensorboard_scanner_algo { /****/ -#define I2C_REQ_MECHBOARD_STATUS 0x80 +#define I2C_REQ_COBBOARD_STATUS 0x80 -struct i2c_req_mechboard_status { +struct i2c_req_cobboard_status { struct i2c_cmd_hdr hdr; - - int16_t pump_left1_current; - int16_t pump_left2_current; + int16_t sickle_left1_current; }; -#define I2C_ANS_MECHBOARD_STATUS 0x81 +#define I2C_ANS_COBBOARD_STATUS 0x81 -struct i2c_ans_mechboard_status { +struct i2c_ans_cobboard_status { struct i2c_cmd_hdr hdr; - /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */ + /* mode type are defined above: I2C_COBBOARD_MODE_xxx */ uint8_t mode; -#define I2C_MECHBOARD_STATUS_F_READY 0x00 -#define I2C_MECHBOARD_STATUS_F_BUSY 0x01 -#define I2C_MECHBOARD_STATUS_F_EXCPT 0x02 +#define I2C_COBBOARD_STATUS_F_READY 0x00 +#define I2C_COBBOARD_STATUS_F_BUSY 0x01 +#define I2C_COBBOARD_STATUS_F_EXCPT 0x02 uint8_t status; - uint8_t lintel_count; - - /* flag is there if column was taken by this pump. Note that - * we should also check ADC (current) to see if the column is - * still there. */ -#define I2C_MECHBOARD_COLUMN_L1 0x01 -#define I2C_MECHBOARD_COLUMN_L2 0x02 -#define I2C_MECHBOARD_COLUMN_R1 0x04 -#define I2C_MECHBOARD_COLUMN_R2 0x08 - uint8_t column_flags; - - int16_t pump_left1; - int16_t pump_right1; - int16_t pump_left2; - int16_t pump_right2; - -#define I2C_MECHBOARD_CURRENT_COLUMN 85 - int16_t pump_right1_current; - int16_t pump_right2_current; - - uint16_t servo_lintel_left; - uint16_t servo_lintel_right; + uint8_t cob_count; }; -#define I2C_REQ_SENSORBOARD_STATUS 0x82 +#define I2C_REQ_BALLBOARD_STATUS 0x82 -struct i2c_req_sensorboard_status { +struct i2c_req_ballboard_status { struct i2c_cmd_hdr hdr; - - /* position sent by mainboard */ - int16_t x; - int16_t y; - int16_t a; - - /* PWM for pickup */ - uint8_t enable_pickup_wheels; }; -#define I2C_ANS_SENSORBOARD_STATUS 0x83 +#define I2C_ANS_BALLBOARD_STATUS 0x83 -struct i2c_ans_sensorboard_status { +struct i2c_ans_ballboard_status { struct i2c_cmd_hdr hdr; - uint8_t status; -#define I2C_OPPONENT_NOT_THERE -1000 - int16_t opponent_x; - int16_t opponent_y; - int16_t opponent_a; - int16_t opponent_d; -#define I2C_SCAN_DONE 1 -#define I2C_SCAN_MAX_COLUMN 2 - uint8_t scan_status; +#define I2C_BALLBOARD_STATUS_F_READY 0x00 +#define I2C_BALLBOARD_STATUS_F_BUSY 0x01 +#define I2C_BALLBOARD_STATUS_F_EXCPT 0x02 + uint8_t status; -#define I2C_COLUMN_NO_DROPZONE -1 - int8_t dropzone_h; - int16_t dropzone_x; - int16_t dropzone_y; + uint8_t ball_count; }; #endif /* _I2C_PROTOCOL_H_ */ diff --git a/projects/microb2010/mechboard/actuator.c b/projects/microb2010/mechboard/actuator.c deleted file mode 100644 index a36a70c..0000000 --- a/projects/microb2010/mechboard/actuator.c +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $ - * - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "../common/i2c_commands.h" -#include "actuator.h" -#include "ax12_user.h" -#include "main.h" - -#define FINGER_DEBUG(args...) DEBUG(E_USER_FINGER, args) -#define FINGER_NOTICE(args...) NOTICE(E_USER_FINGER, args) -#define FINGER_ERROR(args...) ERROR(E_USER_FINGER, args) - -struct finger { - int8_t event; - uint16_t destination; -}; - -struct finger finger; - -static void finger_goto_cb(void *); - -/* schedule a single event for the finger */ -static void finger_schedule_event(struct finger *finger) -{ - uint8_t flags; - int8_t ret; - - IRQ_LOCK(flags); - ret = scheduler_add_event(SCHEDULER_SINGLE, - (void *)finger_goto_cb, - finger, 1, ARM_PRIO); - if (ret == -1) { - IRQ_UNLOCK(flags); - FINGER_ERROR("Cannot load finger event"); - return; - } - finger->event = ret; - IRQ_UNLOCK(flags); -} - -static void finger_goto_cb(void *data) -{ - uint8_t flags; - struct finger *finger = data; - uint16_t position; - - IRQ_LOCK(flags); - finger->event = -1; - position = finger->destination; - IRQ_UNLOCK(flags); - FINGER_DEBUG("goto_cb %d", position); - ax12_user_write_int(&gen.ax12, FINGER_AX12, - AA_GOAL_POSITION_L, position); -} - -/* load an event that will move the ax12 for us */ -void finger_goto(uint16_t position) -{ - uint8_t flags; - FINGER_NOTICE("goto %d", position); - - IRQ_LOCK(flags); - finger.destination = position; - if (finger.event != -1) { - IRQ_UNLOCK(flags); - return; /* nothing to do, event already scheduled */ - } - IRQ_UNLOCK(flags); - finger_schedule_event(&finger); -} - -static void finger_init(void) -{ - finger.event = -1; - finger.destination = 0; - /* XXX set pos ? */ -} - -uint16_t finger_get_goal_pos(void) -{ - return finger.destination; -} - -uint8_t finger_get_side(void) -{ - if (finger.destination <= FINGER_CENTER) - return I2C_LEFT_SIDE; - return I2C_RIGHT_SIDE; -} - -/**********/ - -#define SERVO_LEFT_OUT 400 -#define SERVO_LEFT_1LIN 520 -#define SERVO_LEFT_2LIN 485 -#define SERVO_RIGHT_OUT 290 -#define SERVO_RIGHT_1LIN 155 -#define SERVO_RIGHT_2LIN 180 - -void servo_lintel_out(void) -{ - mechboard.servo_lintel_left = SERVO_LEFT_OUT; - mechboard.servo_lintel_right = SERVO_RIGHT_OUT; -} - -void servo_lintel_1lin(void) -{ - mechboard.servo_lintel_left = SERVO_LEFT_1LIN; - mechboard.servo_lintel_right = SERVO_RIGHT_1LIN; -} - -void servo_lintel_2lin(void) -{ - mechboard.servo_lintel_left = SERVO_LEFT_2LIN; - mechboard.servo_lintel_right = SERVO_RIGHT_2LIN; -} - -/**********/ - -void actuator_init(void) -{ - finger_init(); - servo_lintel_out(); -} diff --git a/projects/microb2010/mechboard/arm_highlevel.c b/projects/microb2010/mechboard/arm_highlevel.c deleted file mode 100644 index abda4d4..0000000 --- a/projects/microb2010/mechboard/arm_highlevel.c +++ /dev/null @@ -1,644 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: arm_highlevel.c,v 1.4 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "arm_xy.h" -#include "arm_highlevel.h" - -#define WRIST_ANGLE_PUMP1 -2 /* in degree */ -#define WRIST_ANGLE_PUMP2 103 /* in degree */ - -struct arm *arm_num2ptr(uint8_t arm_num) -{ - switch (arm_num) { - case ARM_LEFT_NUM: - return &left_arm; - case ARM_RIGHT_NUM: - return &right_arm; - default: - return NULL; - } -} - -#define ARM_MAX_H 110 -#define ARM_STRAIGHT_D 254 -#define ARM_STRAIGHT_H 0 - -void arm_goto_straight(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_STRAIGHT_D, ARM_STRAIGHT_H, angle); -} - -/* position to get a column */ -#define ARM_GET_D 60 -#define ARM_GET_H -140 - -void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_GET_D, ARM_GET_H, angle); -} - -/* position to get a column */ -#define ARM_PREPARE_D 62 -#define ARM_PREPARE_H -133 - -void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_PREPARE_D, ARM_PREPARE_H, angle); -} - -#define ARM_INTERMEDIATE_D (65) -#define ARM_INTERMEDIATE_H (-115) - -void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_INTERMEDIATE_D, ARM_INTERMEDIATE_H, angle); -} - -/* used in prepare pickup */ -#define ARM_INTERMEDIATE_FRONT_D (90) -#define ARM_INTERMEDIATE_FRONT_H (-105) - -void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_INTERMEDIATE_FRONT_D, - ARM_INTERMEDIATE_FRONT_H, angle); -} - -/* ****** */ - -#define ARM_PREPARE_EJECT_D (70) -#define ARM_PREPARE_EJECT_H (-50) - -void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_PREPARE_EJECT_D, ARM_PREPARE_EJECT_H, angle); -} - -#define ARM_EJECT_D (200) -#define ARM_EJECT_H (30) - -void arm_goto_eject(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_EJECT_D, ARM_EJECT_H, angle); -} - -/* ****** */ - -#define ARM_PREPARE_GET_LINTEL_INSIDE1_D 90 -#define ARM_PREPARE_GET_LINTEL_INSIDE1_H -75 -#define ARM_PREPARE_GET_LINTEL_INSIDE1_A -30 -void arm_goto_prepare_get_lintel_inside1(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D, - ARM_PREPARE_GET_LINTEL_INSIDE1_H, - ARM_PREPARE_GET_LINTEL_INSIDE1_A); - arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D, - ARM_PREPARE_GET_LINTEL_INSIDE1_H, - ARM_PREPARE_GET_LINTEL_INSIDE1_A); -} - -#define ARM_PREPARE_GET_LINTEL_INSIDE2_D 30 -#define ARM_PREPARE_GET_LINTEL_INSIDE2_H -75 -#define ARM_PREPARE_GET_LINTEL_INSIDE2_A -30 -void arm_goto_prepare_get_lintel_inside2(uint8_t lintel_count) -{ - uint16_t d; - d = ARM_PREPARE_GET_LINTEL_INSIDE2_D; - if (lintel_count == 2) - d += 34; - arm_do_xy(&left_arm, d, - ARM_PREPARE_GET_LINTEL_INSIDE2_H, - ARM_PREPARE_GET_LINTEL_INSIDE2_A); - arm_do_xy(&right_arm, d, - ARM_PREPARE_GET_LINTEL_INSIDE2_H, - ARM_PREPARE_GET_LINTEL_INSIDE2_A); -} - -#define ARM_GET_LINTEL_INSIDE_D 10 -#define ARM_GET_LINTEL_INSIDE_H -75 -#define ARM_GET_LINTEL_INSIDE_A -30 -void arm_goto_get_lintel_inside(uint8_t lintel_count) -{ - uint16_t d; - d = ARM_GET_LINTEL_INSIDE_D; - if (lintel_count == 2) - d += 34; - arm_do_xy(&left_arm, d, - ARM_GET_LINTEL_INSIDE_H, - ARM_GET_LINTEL_INSIDE_A); - arm_do_xy(&right_arm, d, - ARM_GET_LINTEL_INSIDE_H, - ARM_GET_LINTEL_INSIDE_A); -} - -#define ARM_PREPARE_BUILD_LINTEL1_D 30 -#define ARM_PREPARE_BUILD_LINTEL1_H -50 -#define ARM_PREPARE_BUILD_LINTEL1_A -30 -void arm_goto_prepare_build_lintel1(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL1_D, - ARM_PREPARE_BUILD_LINTEL1_H, - ARM_PREPARE_BUILD_LINTEL1_A); - arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL1_D, - ARM_PREPARE_BUILD_LINTEL1_H, - ARM_PREPARE_BUILD_LINTEL1_A); -} - -#define ARM_PREPARE_BUILD_LINTEL2_D 80 -#define ARM_PREPARE_BUILD_LINTEL2_H -110 -#define ARM_PREPARE_BUILD_LINTEL2_A 60 -void arm_goto_prepare_build_lintel2(uint8_t level) -{ - int16_t h; - if (level < 3) - level = 3; - h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL2_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL2_D, - h, ARM_PREPARE_BUILD_LINTEL2_A); - arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL2_D, - h, ARM_PREPARE_BUILD_LINTEL2_A); -} - -#define ARM_PREPARE_BUILD_LINTEL3_D 205 -#define ARM_PREPARE_BUILD_LINTEL3_H -100 -#define ARM_PREPARE_BUILD_LINTEL3_A 50 -void arm_goto_prepare_build_lintel3(uint8_t level) -{ - int16_t h; - if (level < 2) - level = 2; - h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL3_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL3_D, - h, ARM_PREPARE_BUILD_LINTEL3_A); - arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL3_D, - h, ARM_PREPARE_BUILD_LINTEL3_A); -} - -#define ARM_BUILD_LINTEL_D 205 -#define ARM_BUILD_LINTEL_H -128 -#define ARM_BUILD_LINTEL_A 50 -void arm_goto_build_lintel(uint8_t level) -{ - int16_t h; - h = (int16_t)level * 30 + ARM_BUILD_LINTEL_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(&left_arm, ARM_BUILD_LINTEL_D, - h, ARM_BUILD_LINTEL_A); - arm_do_xy(&right_arm, ARM_BUILD_LINTEL_D, - h, ARM_BUILD_LINTEL_A); -} - -/* ****** */ - -#define ARM_PREPARE_GET_LINTEL_DISP_D 190 -#define ARM_PREPARE_GET_LINTEL_DISP_H -40 -#define ARM_PREPARE_GET_LINTEL_DISP_A 50 -void arm_goto_prepare_get_lintel_disp(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_DISP_D, - ARM_PREPARE_GET_LINTEL_DISP_H, - ARM_PREPARE_GET_LINTEL_DISP_A); - arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_DISP_D, - ARM_PREPARE_GET_LINTEL_DISP_H, - ARM_PREPARE_GET_LINTEL_DISP_A); -} - -#define ARM_GET_LINTEL_DISP_D 190 -#define ARM_GET_LINTEL_DISP_H -70 -#define ARM_GET_LINTEL_DISP_A 40 -void arm_goto_get_lintel_disp(void) -{ - arm_do_xy(&left_arm, ARM_GET_LINTEL_DISP_D, - ARM_GET_LINTEL_DISP_H, - ARM_GET_LINTEL_DISP_A); - arm_do_xy(&right_arm, ARM_GET_LINTEL_DISP_D, - ARM_GET_LINTEL_DISP_H, - ARM_GET_LINTEL_DISP_A); -} - -#define ARM_PREPARE_PUT_LINTEL_DISP_D 130 -#define ARM_PREPARE_PUT_LINTEL_DISP_H 0 -#define ARM_PREPARE_PUT_LINTEL_DISP_A 0 -void arm_goto_prepare_put_lintel(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_PUT_LINTEL_DISP_D, - ARM_PREPARE_PUT_LINTEL_DISP_H, - ARM_PREPARE_PUT_LINTEL_DISP_A); - arm_do_xy(&right_arm, ARM_PREPARE_PUT_LINTEL_DISP_D, - ARM_PREPARE_PUT_LINTEL_DISP_H, - ARM_PREPARE_PUT_LINTEL_DISP_A); -} - -#define ARM_PUT_LINTEL_DISP_D 30 -#define ARM_PUT_LINTEL_DISP_H -60 -#define ARM_PUT_LINTEL_DISP_A -30 -void arm_goto_put_lintel(uint8_t lintel_count) -{ - arm_do_xy(&left_arm, - ARM_PUT_LINTEL_DISP_D + lintel_count * 30, - ARM_PUT_LINTEL_DISP_H, - ARM_PUT_LINTEL_DISP_A); - arm_do_xy(&right_arm, - ARM_PUT_LINTEL_DISP_D + lintel_count * 30, - ARM_PUT_LINTEL_DISP_H, - ARM_PUT_LINTEL_DISP_A); -} - -/* ****** */ - - -#define ARM_LOADED_D 100 -#define ARM_LOADED_H 0 - -void arm_goto_loaded(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - arm_do_xy(arm, ARM_LOADED_D, ARM_LOADED_H, WRIST_ANGLE_PUMP2); -} - - -/* for columns */ -#define ARM_PREPARE_BUILD_INSIDE_D 90 -#define ARM_PREPARE_BUILD_INSIDE_H -45 - -void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (level < 2) - level = 2; - h = (int16_t)level * 30 + ARM_PREPARE_BUILD_INSIDE_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, ARM_PREPARE_BUILD_INSIDE_D, h, angle); -} - -#define ARM_PREPARE_AUTOBUILD_INSIDE_D 90 -#define ARM_PREPARE_AUTOBUILD_INSIDE_H -70 - -void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (level < 2) - level = 2; - h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_INSIDE_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, ARM_PREPARE_AUTOBUILD_INSIDE_D, h, angle); -} - -#define ARM_PREPARE_AUTOBUILD_OUTSIDE_D 210 /* not used, see dist below */ -#define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1 (-110) -#define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2 (-90) - -void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) - h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1; - else - h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, dist, h, angle); -} - -#define ARM_AUTOBUILD_D_P1 208 /* not used, see dist below */ -#define ARM_AUTOBUILD_D_P2 210 /* not used, see dist below */ -#define ARM_AUTOBUILD_D_P1_OFFSET (-2) -#define ARM_AUTOBUILD_D_P2_OFFSET (0) -#define ARM_AUTOBUILD_H_P1 (-133) -#define ARM_AUTOBUILD_H_P2 (-130) - -void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) { - h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P1; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P1_OFFSET, h, angle); - } - else { - h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P2; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P2_OFFSET, h, angle); - } -} - -#define ARM_PUSH_TEMPLE_D 170 -#define ARM_PUSH_TEMPLE_H -165 - -void arm_goto_push_temple(uint8_t arm_num, uint8_t level) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t h = ARM_PUSH_TEMPLE_H; - - /* level can be 0 or 1 */ - if (level) - h += 30; - arm_do_xy(arm, ARM_PUSH_TEMPLE_D, h, WRIST_ANGLE_PUMP1); -} - -#define ARM_PREPARE_PUSH_TEMPLE_D 120 -#define ARM_PREPARE_PUSH_TEMPLE_H -60 - -void arm_goto_prepare_push_temple(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_D, - ARM_PREPARE_PUSH_TEMPLE_H, WRIST_ANGLE_PUMP1); -} - -#define ARM_PUSH_TEMPLE_DISC_D1 215 -#define ARM_PUSH_TEMPLE_DISC_H1 -100 -#define ARM_PUSH_TEMPLE_DISC_D2 190 -#define ARM_PUSH_TEMPLE_DISC_H2 -65 - -void arm_goto_push_temple_disc(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int8_t pump_num; - - pump_num = arm_get_busy_pump(arm_num); - if (pump_num == -1) - arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D1, - ARM_PUSH_TEMPLE_DISC_H1, WRIST_ANGLE_PUMP1); - else - arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D2, - ARM_PUSH_TEMPLE_DISC_H2, WRIST_ANGLE_PUMP1); -} - -#define ARM_PREPARE_PUSH_TEMPLE_DISC_D 100 -#define ARM_PREPARE_PUSH_TEMPLE_DISC_H -80 - -void arm_goto_prepare_push_temple_disc(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_DISC_D, - ARM_PREPARE_PUSH_TEMPLE_DISC_H, WRIST_ANGLE_PUMP1); -} - -void arm_prepare_free_pumps(void) -{ - int8_t pump_num; - - pump_num = arm_get_free_pump(ARM_LEFT_NUM); - if (pump_num != -1) - arm_goto_prepare_get(ARM_LEFT_NUM, pump_num); - pump_num = arm_get_free_pump(ARM_RIGHT_NUM); - if (pump_num != -1) - arm_goto_prepare_get(ARM_RIGHT_NUM, pump_num); -} - - -/* return the id of a free pump on this arm */ -int8_t arm_get_free_pump(uint8_t arm_num) -{ - switch (arm_num) { - case ARM_LEFT_NUM: - if (pump_is_free(PUMP_LEFT1_NUM) && - pump_is_free(PUMP_LEFT2_NUM)) - return PUMP_LEFT1_NUM; - else if (pump_is_free(PUMP_LEFT2_NUM)) - return PUMP_LEFT2_NUM; - return -1; - case ARM_RIGHT_NUM: - if (pump_is_free(PUMP_RIGHT1_NUM) && - pump_is_free(PUMP_RIGHT2_NUM)) - return PUMP_RIGHT1_NUM; - else if (pump_is_free(PUMP_RIGHT2_NUM)) - return PUMP_RIGHT2_NUM; - return -1; - default: - return -1; - } -} - -/* return the id of a busy pump on this arm */ -int8_t arm_get_busy_pump(uint8_t arm_num) -{ - switch (arm_num) { - case ARM_LEFT_NUM: - if (pump_is_busy(PUMP_LEFT2_NUM)) - return PUMP_LEFT2_NUM; - else if (pump_is_busy(PUMP_LEFT1_NUM)) - return PUMP_LEFT1_NUM; - return -1; - case ARM_RIGHT_NUM: - if (pump_is_busy(PUMP_RIGHT2_NUM)) - return PUMP_RIGHT2_NUM; - else if (pump_is_busy(PUMP_RIGHT1_NUM)) - return PUMP_RIGHT1_NUM; - return -1; - default: - return -1; - } -} - -uint8_t arm_wait_both(uint8_t mask) -{ - uint8_t ret; - ret = arm_wait_traj_end(&left_arm, mask); - if (ret != ARM_TRAJ_END && ret != ARM_TRAJ_NEAR) - return ret; - return arm_wait_traj_end(&right_arm, mask); -} - -uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask) -{ - if (left && right) - return arm_wait_both(mask); - if (left) - return arm_wait_traj_end(&left_arm, mask); - if (right) - return arm_wait_traj_end(&right_arm, mask); - return ARM_TRAJ_END; -} - -/*********************/ - -int16_t *pump_num2ptr(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - return &mechboard.pump_left1; - case PUMP_RIGHT1_NUM: - return &mechboard.pump_right1; - case PUMP_LEFT2_NUM: - return &mechboard.pump_left2; - case PUMP_RIGHT2_NUM: - return &mechboard.pump_right2; - default: - return NULL; - } -} - -void pump_set(uint8_t pump_num, int16_t val) -{ - int16_t *pump_ptr = pump_num2ptr(pump_num); - - *pump_ptr = val; - - switch (pump_num) { - case PUMP_RIGHT1_NUM: - pwm_ng_set(RIGHT_PUMP1_PWM, val); - break; - case PUMP_RIGHT2_NUM: - pwm_ng_set(RIGHT_PUMP2_PWM, val); - break; - - /* no pwm, it's remote */ - case PUMP_LEFT1_NUM: - case PUMP_LEFT2_NUM: - default: - break; - } -} - -int16_t pump_num2angle(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - case PUMP_RIGHT1_NUM: - return WRIST_ANGLE_PUMP1; - case PUMP_LEFT2_NUM: - case PUMP_RIGHT2_NUM: - return WRIST_ANGLE_PUMP2; - default: - return 0; - } -} - -void pump_mark_busy(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L1; - break; - case PUMP_RIGHT1_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R1; - break; - case PUMP_LEFT2_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L2; - break; - case PUMP_RIGHT2_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R2; - break; - default: - break; - } - -} - -void pump_mark_free(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L1); - break; - case PUMP_RIGHT1_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R1); - break; - case PUMP_LEFT2_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L2); - break; - case PUMP_RIGHT2_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R2); - break; - default: - break; - } - -} - -uint8_t pump_is_free(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1); - case PUMP_RIGHT1_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1); - case PUMP_LEFT2_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2); - case PUMP_RIGHT2_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2); - default: - return 0; - } -} - -uint8_t pump_is_busy(uint8_t pump_num) -{ - return !pump_is_free(pump_num); -} diff --git a/projects/microb2010/mechboard/arm_highlevel.h b/projects/microb2010/mechboard/arm_highlevel.h deleted file mode 100644 index eaf3ae2..0000000 --- a/projects/microb2010/mechboard/arm_highlevel.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: arm_highlevel.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#define ARM_LEFT_NUM 0 -#define ARM_RIGHT_NUM 1 - -#define PUMP_LEFT1_NUM 0 -#define PUMP_RIGHT1_NUM 1 -#define PUMP_LEFT2_NUM 2 -#define PUMP_RIGHT2_NUM 3 - -struct arm *arm_num2ptr(uint8_t arm_num); - -void arm_goto_straight(uint8_t arm_num, uint8_t pump_num); -void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num); -void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num); -void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num); -void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num); -void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num); -void arm_goto_eject(uint8_t arm_num, uint8_t pump_num); -void arm_goto_loaded(uint8_t arm_num); -void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, - uint8_t level); -void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, - uint8_t level); -void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist); -void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist); - -void arm_goto_prepare_get_lintel_inside1(void); -void arm_goto_prepare_get_lintel_inside2(uint8_t lintel_count); -void arm_goto_get_lintel_inside(uint8_t lintel_count); -void arm_goto_prepare_build_lintel1(void); -void arm_goto_prepare_build_lintel2(uint8_t level); -void arm_goto_prepare_build_lintel3(uint8_t level); -void arm_goto_build_lintel(uint8_t level); - -void arm_goto_prepare_get_lintel_disp(void); -void arm_goto_get_lintel_disp(void); - -void arm_goto_prepare_put_lintel(void); -void arm_goto_put_lintel(uint8_t lintel_count); -void arm_goto_prepare_push_temple(uint8_t arm_num); -void arm_goto_push_temple(uint8_t arm_num, uint8_t level); -void arm_goto_prepare_push_temple_disc(uint8_t arm_num); -void arm_goto_push_temple_disc(uint8_t arm_num); - -void arm_prepare_free_pumps(void); -uint8_t arm_wait_both(uint8_t mask); -uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask); - -/* return the id of the free pump for the arm, or return -1 */ -int8_t arm_get_free_pump(uint8_t arm_num); -int8_t arm_get_busy_pump(uint8_t arm_num); - -#define PUMP_ON 3400 -#define PUMP_OFF 0 -#define PUMP_REVERSE -3400 - -void pump_set(uint8_t pump_num, int16_t val); -int16_t pump_num2angle(uint8_t pump_num); -void pump_mark_busy(uint8_t pump_num); -void pump_mark_free(uint8_t pump_num); -uint8_t pump_is_free(uint8_t pump_num); -uint8_t pump_is_busy(uint8_t pump_num); diff --git a/projects/microb2010/mechboard/arm_xy.c b/projects/microb2010/mechboard/arm_xy.c deleted file mode 100644 index 2690844..0000000 --- a/projects/microb2010/mechboard/arm_xy.c +++ /dev/null @@ -1,809 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: arm_xy.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ - * - * Fabrice DESCLAUX - * Olivier MATZ - */ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "main.h" -#include "cmdline.h" -#include "arm_xy.h" -#include "ax12_user.h" - -#define ARM_DEBUG(args...) DEBUG(E_USER_ARM, args) -#define ARM_NOTICE(args...) NOTICE(E_USER_ARM, args) -#define ARM_ERROR(args...) ERROR(E_USER_ARM, args) - -#define DEG(x) (((double)(x)) * (180.0 / M_PI)) -#define RAD(x) (((double)(x)) * (M_PI / 180.0)) -#define M_2PI (2*M_PI) - -/* physical location/dimensions of arm */ -#define ARM_S_LEN 124. -#define ARM_E_LEN 130. - -/* timeout after 1 second if position is not reached */ -#define ARM_GLOBAL_TIMEOUT 1000000L - -/* timeout 100ms after position is reached if not in window */ -#define ARM_WINDOW_TIMEOUT 200000L - -/* default (template) period, but real one is variable */ -#define ARM_PERIOD 50000L -#define ARM_MAX_DIST 40L - -/* we pos reached, check arm in window every period */ -#define ARM_SURVEY_PERIOD 25000UL /* in us */ - -/* number of steps/s */ -#define ARM_AX12_MAX_SPEED (800L) - -/* Maximum number of steps in one ARM_PERIOD */ -#define ARM_MAX_E (((ARM_AX12_MAX_SPEED*ARM_PERIOD)/1000000L)) -/* 4000 steps/CS => 800step/ms */ -#define ARM_MAX_S ((800L*ARM_PERIOD)/1000L) - - -/* window limits in ax12/cs unit */ -#define ARM_SHOULDER_WINDOW_POS 250 -#define ARM_ELBOW_WINDOW_POS 8 -#define ARM_ELBOW_WINDOW_SPEED 100 -#define ARM_WRIST_WINDOW_POS 8 -#define ARM_WRIST_WINDOW_SPEED 100 - -/* default and max speeds */ -#define SHOULDER_DEFAULT_SPEED 800 -#define ELBOW_DEFAULT_SPEED 0x3ff -#define SHOULDER_MAX_SPEED 10000 -#define ELBOW_MAX_SPEED 0x3ff - -/* window status flags */ -#define SHOULDER_NOT_IN_WIN 1 -#define ELBOW_NOT_IN_WIN 2 -#define WRIST_NOT_IN_WIN 4 - -static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out); -static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot); -static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad); -static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out); -static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot); -static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad); - -static void arm_schedule_event(struct arm *arm, uint32_t time); - -struct arm left_arm = { - .config = { - .wrist_angle_deg2robot = wrist_angle_deg2robot_l, - .angle_rad2robot = angle_rad2robot_l, - .angle_robot2rad = angle_robot2rad_l, - .elbow_ax12 = L_ELBOW_AX12, - .wrist_ax12 = L_WRIST_AX12, - }, -}; - -struct arm right_arm = { - .config = { - .wrist_angle_deg2robot = wrist_angle_deg2robot_r, - .angle_rad2robot = angle_rad2robot_r, - .angle_robot2rad = angle_robot2rad_r, - .elbow_ax12 = R_ELBOW_AX12, - .wrist_ax12 = R_WRIST_AX12, - }, -}; - -/* process shoulder + elbow angles from height and distance */ -int8_t cart2angle(int32_t h_int, int32_t d_int, double *alpha, double *beta) -{ - double h, d, l; - double elbow, shoulder; - - d = d_int; - h = h_int; - l = sqrt(d*d + h*h); - if (l > (ARM_S_LEN + ARM_E_LEN)) - return -1; - - elbow = -acos((d*d + h*h - ARM_E_LEN*ARM_E_LEN - - ARM_S_LEN*ARM_S_LEN) / (2*ARM_S_LEN*ARM_E_LEN)); - shoulder = atan2(h,d) - atan2(ARM_E_LEN*sin(elbow), - ARM_S_LEN+ARM_E_LEN*cos(elbow)); - - *alpha = shoulder; - *beta = elbow; - - return 0; -} - - -/* process height and distance from shoulder + elbow angles */ -void angle2cart(double alpha, double beta, int32_t *h, int32_t *d) -{ - double tmp_a; - int32_t tmp_h, tmp_d; - - tmp_h = ARM_S_LEN * sin(alpha); - tmp_d = ARM_S_LEN * cos(alpha); - - tmp_a = alpha+beta; - *h = tmp_h + ARM_E_LEN * sin(tmp_a); - *d = tmp_d + ARM_E_LEN * cos(tmp_a); -} - -/*** left arm */ - -#define ARM_LEFT_S_OFFSET -1150. -#define ARM_LEFT_E_OFFSET 476. -#define ARM_LEFT_W_OFFSET 90. - -static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out) -{ - *wrist_out = -wrist_deg * 3.41 + ARM_LEFT_W_OFFSET; -} - -/* convert an angle in radian into a robot-specific unit - * for shoulder and elbow for LEFT ARM*/ -static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot) -{ - *shoulder_robot = shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_LEFT_S_OFFSET; - *elbow_robot = -elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_LEFT_E_OFFSET; -} - -/* convert a robot-specific unit into an angle in radian - * for shoulder and elbow for LEFT ARM */ -static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad) -{ - *shoulder_rad = ((shoulder_robot - ARM_LEFT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); - *elbow_rad = -((elbow_robot - ARM_LEFT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); -} - -/*** right arm */ - -#define ARM_RIGHT_S_OFFSET 1150. -#define ARM_RIGHT_E_OFFSET 673. -#define ARM_RIGHT_W_OFFSET 935. - -static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out) -{ - *wrist_out = wrist_deg * 3.41 + ARM_RIGHT_W_OFFSET; -} - -/* convert an angle in radian into a robot-specific unit - * for shoulder and elbow */ -static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot) -{ - *shoulder_robot = -shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_RIGHT_S_OFFSET; - *elbow_robot = elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_RIGHT_E_OFFSET; -} - -/* convert a robot-specific unit into an angle in radian - * for shoulder and elbow */ -static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad) -{ - *shoulder_rad = -((shoulder_robot - ARM_RIGHT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); - *elbow_rad = ((elbow_robot - ARM_RIGHT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); -} - - -/* - * Fill the arm_status structure according to request. - * - * return: - * 0 => success - * < 0 => error - */ -static int8_t arm_do_step(struct arm *arm) -{ - const struct arm_config *conf = &arm->config; - const struct arm_request *req = &arm->req; - struct arm_status *status = &arm->status; - - int8_t ret; - int32_t diff_h, diff_d; /* position delta in steps */ - int32_t next_h, next_d; /* next position in steps */ - int32_t l; /* distance between cur pos and next pos */ - - double as_cur_rad, ae_cur_rad; /* current angle in rad */ - double as_next_rad, ae_next_rad; /* next angle in rad */ - double as_cur, ae_cur; /* current angle in angle_steps */ - double as_next, ae_next; /* next angle in angle_steps */ - - int32_t as_diff, ae_diff; /* angle delta in angle_steps */ - int32_t s_speed, e_speed; /* elbow/shoulder speed in angle_steps */ - - double as_coef, ae_coef; - - /* process diff between final request and current pos */ - diff_h = req->h_mm - status->h_mm; - diff_d = req->d_mm - status->d_mm; - ARM_NOTICE("goal:d=%ld,h=%ld cur:d=%ld,h=%ld diff:d=%ld,h=%ld", - req->d_mm, req->h_mm, status->d_mm, status->h_mm, - diff_d, diff_h); - - /* if distance to next point is too large, saturate it */ - l = sqrt(diff_h*diff_h + diff_d*diff_d); - if (l > ARM_MAX_DIST) { - diff_h = diff_h * ARM_MAX_DIST / l; - diff_d = diff_d * ARM_MAX_DIST / l; - } - ARM_NOTICE("l=%ld ; after max dist: diff:d=%ld,h=%ld", l, diff_d, diff_h); - - /* process next position */ - next_h = status->h_mm + diff_h; - next_d = status->d_mm + diff_d; - ARM_DEBUG("next:d=%ld,h=%ld", next_d, next_h); - - /* calculate the current angle of arm in radian */ - ret = cart2angle(status->h_mm, status->d_mm, &as_cur_rad, &ae_cur_rad); - if (ret) - return ret; - ARM_DEBUG("as_cur_rad=%f ae_cur_rad=%f", as_cur_rad, ae_cur_rad); - - /* calculate the next angle of arm in radian */ - ret = cart2angle(next_h, next_d, &as_next_rad, &ae_next_rad); - if (ret) - return ret; - ARM_DEBUG("as_next_rad=%f ae_next_rad=%f", as_next_rad, ae_next_rad); - - /* convert radian in angle_steps */ - conf->angle_rad2robot(as_cur_rad, ae_cur_rad, - &as_cur, &ae_cur); - ARM_DEBUG("as_cur=%f ae_cur=%f", as_cur, ae_cur); - conf->angle_rad2robot(as_next_rad, ae_next_rad, - &as_next, &ae_next); - ARM_DEBUG("as_next=%f ae_next=%f", as_next, ae_next); - - /* process angle delta in angle_steps */ - as_diff = as_next - as_cur; - ae_diff = ae_next - ae_cur; - ARM_DEBUG("as_diff=%ld ae_diff=%ld", as_diff, ae_diff); - - /* update position status */ - status->h_mm = next_h; - status->d_mm = next_d; - status->shoulder_angle_steps = as_next; - status->elbow_angle_steps = ae_next; - status->shoulder_angle_rad = as_next_rad; - status->elbow_angle_rad = ae_next_rad; - - /* we reached destination, nothing to do */ - if (as_diff == 0 && ae_diff == 0) { - status->shoulder_speed = SHOULDER_DEFAULT_SPEED; - status->elbow_speed = ELBOW_DEFAULT_SPEED; - status->next_update_time = 0; - ARM_NOTICE("reaching end"); - return 0; - } - - /* test if one actuator is already in position */ - if (as_diff == 0) { - ARM_DEBUG("shoulder reached destination"); - ae_coef = (double)ARM_MAX_E / (double)ae_diff; - status->next_update_time = ARM_PERIOD * ABS(ae_coef); - e_speed = ABS(ae_coef) * ABS(ae_diff); - s_speed = ARM_MAX_S; - } - else if (ae_diff == 0) { - ARM_DEBUG("elbow reached destination"); - as_coef = (double)ARM_MAX_S / (double)as_diff; - status->next_update_time = ARM_PERIOD / ABS(as_coef); - e_speed = ARM_MAX_E; - s_speed = ABS(as_coef) * ABS(as_diff); - } - - else { - as_coef = (double)ARM_MAX_S / (double)as_diff; - ae_coef = (double)ARM_MAX_E / (double)ae_diff; - - ARM_DEBUG("as_coef=%f ae_coef=%f", as_coef, ae_coef); - - /* if elbow is limitating */ - if (ABS(as_coef) >= ABS(ae_coef)) { - ARM_DEBUG("elbow limit"); - status->next_update_time = ARM_PERIOD / ABS(ae_coef); - s_speed = ABS(ae_coef) * ABS(as_diff); - e_speed = ABS(ae_coef) * ABS(ae_diff); - } - /* else, shoulder is limitating */ - else { - ARM_DEBUG("shoulder limit"); - status->next_update_time = ARM_PERIOD / ABS(as_coef); - s_speed = ABS(as_coef) * ABS(as_diff); - e_speed = ABS(as_coef) * ABS(ae_diff); - } - } - - ARM_NOTICE("next update: %ld", status->next_update_time); - - /* convert speed in specific unit */ - status->shoulder_speed = (s_speed * CS_PERIOD) / ARM_PERIOD; - status->elbow_speed = (e_speed * 0x3ff) / ARM_MAX_E; - - ARM_DEBUG("speeds: s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); - - /* avoid limits */ - if (status->shoulder_speed == 0) - status->shoulder_speed = 1; - if (status->elbow_speed == 0) - status->elbow_speed = 1; - if (status->shoulder_speed >= SHOULDER_MAX_SPEED) - status->shoulder_speed = SHOULDER_MAX_SPEED; - if (status->elbow_speed >= ELBOW_MAX_SPEED) - status->elbow_speed = ELBOW_MAX_SPEED; - - ARM_DEBUG("speeds (sat): s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); - - return 0; -} - -static void arm_delete_event(struct arm *arm) -{ - if (arm->status.event == -1) - return; - ARM_DEBUG("Delete arm event"); - scheduler_del_event(arm->status.event); - arm->status.event = -1; -} - -/* write values to ax12 + cs */ -static void arm_apply(struct arm *arm) -{ - struct cs_block *csb = arm->config.csb; - const struct arm_status *st = &arm->status; - - ARM_DEBUG("arm_apply"); - - if (arm->config.simulate) - return; - - /* set speed and pos of shoulder */ - quadramp_set_1st_order_vars(&csb->qr, - st->shoulder_speed, - st->shoulder_speed); - cs_set_consign(&csb->cs, st->shoulder_angle_steps); - - /* set speed and position of elbow */ - ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, - AA_MOVING_SPEED_L, st->elbow_speed); - ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, - AA_GOAL_POSITION_L, st->elbow_angle_steps); -} - -/* return true if one of the mask condition is true */ -uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask) -{ - if ((mask & ARM_TRAJ_END) && (arm->status.state & ARM_FLAG_IN_WINDOW)) - return ARM_TRAJ_END; - - if ((mask & ARM_TRAJ_NEAR) && (arm->status.state & ARM_FLAG_LAST_STEP)) - return ARM_TRAJ_NEAR; - - if ((mask & ARM_TRAJ_TIMEOUT) && (arm->status.state & ARM_FLAG_TIMEOUT)) - return ARM_TRAJ_TIMEOUT; - - if ((mask & ARM_TRAJ_ERROR) && (arm->status.state & ARM_FLAG_ERROR)) - return ARM_TRAJ_ERROR; - - return 0; -} - -uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask) -{ - uint8_t ret; - while(1) { - ret = arm_test_traj_end(arm, mask); - if (ret) - return ret; - } -} - -/* return true if one of the mask condition is true */ -uint8_t arm_in_window(struct arm *arm, uint8_t *status) -{ - int8_t err; -/* uint16_t spd; */ - int16_t pos; - int32_t cs_err; - - *status = 0; - - if (arm->config.simulate) - return 1; - - /* shoulder, just check position */ - cs_err = cs_get_error(&arm->config.csb->cs); - if (ABS(cs_err) > ARM_SHOULDER_WINDOW_POS) - *status |= SHOULDER_NOT_IN_WIN; - -#if 0 - /* check elbow speed */ - err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, - AA_PRESENT_SPEED_L, &spd); - if (err) - goto fail; - if (spd > ARM_ELBOW_WINDOW_SPEED) - return 0; - - /* check wrist speed */ - err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, - AA_PRESENT_SPEED_L, &spd); - if (err) - goto fail; - if (spd > ARM_WRIST_WINDOW_SPEED) - return 0; -#endif - /* check elbow pos */ - err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, - AA_PRESENT_POSITION_L, (uint16_t *)&pos); - if (err) - goto fail; - if (ABS(arm->status.elbow_angle_steps - pos) > ARM_ELBOW_WINDOW_POS) - *status |= ELBOW_NOT_IN_WIN; - - /* check wrist pos */ - err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, - AA_PRESENT_POSITION_L, (uint16_t *)&pos); - if (err) - goto fail; - if (ABS(arm->status.wrist_angle_steps - pos) > ARM_WRIST_WINDOW_POS) - *status |= WRIST_NOT_IN_WIN; - - if (*status) - return 0; - - ARM_NOTICE("arm is in window (%ld us after reach pos)", - time_get_us2() - arm->status.pos_reached_time); - return 1; /* ok, we are in window */ - - fail: - return 0; -} - -/* process wrist pos and apply it. it's done only once. */ -static int8_t arm_set_wrist(struct arm *arm) -{ - int8_t err; - int32_t as_deg, ae_deg, aw_deg; - uint16_t wrist_out_u16; - double wrist_out, as_rad, ae_rad; - int16_t pos; - uint32_t diff_time; - - /* calculate the destination angle of arm in radian */ - err = cart2angle(arm->req.h_mm, arm->req.d_mm, - &as_rad, &ae_rad); - if (err) - return -1; - - /* calc angle destination */ - as_deg = DEG(as_rad); - ae_deg = DEG(ae_rad); - ARM_DEBUG("as_dest_deg=%d ae_dest_deg=%d", as_deg, ae_deg); - aw_deg = as_deg + ae_deg - arm->req.w_deg; - arm->config.wrist_angle_deg2robot(aw_deg, &wrist_out); - wrist_out_u16 = wrist_out; - - ARM_DEBUG("set wrist to %ld degrees (%d steps)", aw_deg, - wrist_out_u16); - - /* process the theorical reach time for the wrist */ - if (arm->config.simulate) { - pos = arm->status.wrist_angle_steps; - } - else { - err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, - AA_PRESENT_POSITION_L, (uint16_t *)&pos); - if (err) - pos = arm->status.wrist_angle_steps; - } - /* 600 is the number of steps/s */ - diff_time = (ABS((int16_t)wrist_out_u16 - pos) * 1000000L) / 600; - arm->status.wrist_reach_time = arm->status.start_time + diff_time; - ARM_DEBUG("wrist reach time is %ld (diff=%ld)", - arm->status.wrist_reach_time, diff_time); - - /* update current position to destination */ - arm->status.wrist_angle_steps = wrist_out_u16; - - if (arm->config.simulate) - return 0; - - /* send it to ax12 */ - ax12_user_write_int(&gen.ax12, arm->config.wrist_ax12, - AA_GOAL_POSITION_L, wrist_out_u16); - return 0; -} - -/* event callback */ -static void arm_do_xy_cb(struct arm *arm) -{ - uint8_t win_status; - - arm->status.event = -1; - - /* if consign haven't reach destination */ - if ((arm->status.state & ARM_FLAG_LAST_STEP) == 0) { - if (arm_do_step(arm)) - arm->status.state |= ARM_FLAG_ERROR; - - /* it's the first call for the traj */ - if (arm->status.state == ARM_STATE_INIT) { - arm->status.state |= ARM_FLAG_MOVING; - if (arm_set_wrist(arm)) - arm->status.state |= ARM_FLAG_ERROR; - } - - /* we have more steps to do */ - if (arm->status.next_update_time == 0) { - arm->status.state &= ~ARM_FLAG_MOVING; - arm->status.state |= ARM_FLAG_LAST_STEP; - arm->status.pos_reached_time = time_get_us2(); - } - arm_apply(arm); - } - /* last step is reached, we can check that arm is in window */ - else if ((arm->status.state & ARM_FLAG_IN_WINDOW) == 0) { - if (arm_in_window(arm, &win_status)) - arm->status.state |= ARM_FLAG_IN_WINDOW; - - /* check for window arm timeout */ - else { - microseconds t; - int32_t diff1, diff2; - t = time_get_us2(); - diff1 = t - arm->status.pos_reached_time; - diff2 = t - arm->status.wrist_reach_time; - if (diff1 > ARM_WINDOW_TIMEOUT && - diff2 > ARM_WINDOW_TIMEOUT) { - ARM_NOTICE("win timeout at %ld win_status=%x", - t, win_status); - arm->status.state |= ARM_FLAG_TIMEOUT; - } - } - } - - /* check for global arm timeout */ - if ((time_get_us2() - arm->status.start_time) > ARM_GLOBAL_TIMEOUT) { - ARM_NOTICE("global timeout at %ld", time_get_us2()); - arm->status.state |= ARM_FLAG_TIMEOUT; - } - - /* reload event if needed */ - if ((arm->status.state & ARM_FLAG_FINISHED) == ARM_FLAG_FINISHED) { - ARM_NOTICE("arm traj finished"); - return; /* no more event, position reached */ - } - if (arm->status.state & (ARM_FLAG_ERROR|ARM_FLAG_TIMEOUT)) { - ARM_NOTICE("error or timeout"); - return; /* no more event */ - } - else if (arm->status.state & ARM_FLAG_LAST_STEP) { - /* theorical position is reached, but reload an event - * for position survey (window), every 25ms */ - arm_schedule_event(arm, ARM_SURVEY_PERIOD); - } - else { - /* reload event for next position step */ - arm_schedule_event(arm, arm->status.next_update_time); - } -} - -/* schedule a single event for this arm */ -static void arm_schedule_event(struct arm *arm, uint32_t time) -{ - uint8_t flags; - int8_t ret; - - arm_delete_event(arm); - if (time < SCHEDULER_UNIT) - time = SCHEDULER_UNIT; - IRQ_LOCK(flags); - ret = scheduler_add_event(SCHEDULER_SINGLE, - (void *)arm_do_xy_cb, - arm, time/SCHEDULER_UNIT, ARM_PRIO); - if (ret == -1) { - IRQ_UNLOCK(flags); - ARM_ERROR("Cannot load arm event"); - return; - } - arm->status.event = ret; - IRQ_UNLOCK(flags); -} - -int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg) -{ - ARM_NOTICE("arm_do_xy: d_mm=%d h_mm=%d w_deg=%d", d_mm, h_mm, w_deg); - - /* remove previous event if any */ - arm_delete_event(arm); - - /* init mandatory params */ - arm->req.d_mm = d_mm; - arm->req.h_mm = h_mm; - arm->req.w_deg = w_deg; - arm->status.start_time = time_get_us2(); - arm->status.state = ARM_STATE_INIT; - - /* all the job will be done asynchronously now */ - arm_schedule_event(arm, 0); - return 0; -} - -void arm_dump(struct arm *arm) -{ - printf_P(PSTR("config: simulate=%d\r\n"), - arm->config.simulate); - printf_P(PSTR("req: d_mm=%ld h_mm=%ld w_deg=%ld\r\n"), - arm->req.d_mm, arm->req.h_mm, arm->req.w_deg); - printf_P(PSTR("status: ")); - if (arm->status.state == ARM_STATE_INIT) - printf_P(PSTR("ARM_STATE_INIT ")); - if (arm->status.state & ARM_FLAG_MOVING) - printf_P(PSTR("ARM_FLAG_MOVING ")); - if (arm->status.state & ARM_FLAG_LAST_STEP) - printf_P(PSTR("ARM_FLAG_LAST_STEP ")); - if (arm->status.state & ARM_FLAG_IN_WINDOW) - printf_P(PSTR("ARM_FLAG_IN_WINDOW ")); - if (arm->status.state & ARM_FLAG_ERROR) - printf_P(PSTR("ARM_FLAG_ERROR ")); - if (arm->status.state & ARM_FLAG_TIMEOUT) - printf_P(PSTR("ARM_FLAG_TIMEOUT ")); - printf_P(PSTR("\r\n")); - - printf_P(PSTR(" d_mm=%ld h_mm=%ld goal_w_steps=%d\r\n"), - arm->status.d_mm, arm->status.h_mm, arm->status.wrist_angle_steps); - printf_P(PSTR(" cur_shl_steps=%ld cur_elb_steps=%ld\r\n"), - arm->status.shoulder_angle_steps, arm->status.elbow_angle_steps); - printf_P(PSTR(" cur_shl_rad=%f cur_elb_rad=%f\r\n"), - arm->status.shoulder_angle_rad, arm->status.elbow_angle_rad); - printf_P(PSTR(" cur_shl_deg=%f cur_elb_deg=%f\r\n"), - DEG(arm->status.shoulder_angle_rad), DEG(arm->status.elbow_angle_rad)); - printf_P(PSTR(" event=%d next_update_time=%ld\r\n"), - arm->status.event, arm->status.next_update_time); - printf_P(PSTR(" start_time=%ld pos_reached_time=%ld wrist_reach_time=%ld\r\n"), - arm->status.start_time, arm->status.pos_reached_time, - arm->status.wrist_reach_time); -} - -#define CALIB_ANGLE (RAD(-93.)) - -void arm_calibrate(void) -{ - double shoulder, elbow; - - pwm_ng_set(LEFT_ARM_PWM, 500); - pwm_ng_set(RIGHT_ARM_PWM, -500); - wait_ms(200); - - pwm_ng_set(LEFT_ARM_PWM, 300); - pwm_ng_set(RIGHT_ARM_PWM, -300); - wait_ms(700); - - printf_P(PSTR("Init arm, please wait...")); - ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_TORQUE_ENABLE, 0x1); - ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_ALARM_SHUTDOWN, 0x04); - - angle_rad2robot_r(0, CALIB_ANGLE, &shoulder, &elbow); - ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); - ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_GOAL_POSITION_L, 628); - - angle_rad2robot_l(0, CALIB_ANGLE, &shoulder, &elbow); - ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); - ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_GOAL_POSITION_L, 394); - pwm_ng_set(LEFT_ARM_PWM, -100); - pwm_ng_set(RIGHT_ARM_PWM, 100); - - wait_ms(2000); - - cs_set_consign(&mechboard.left_arm.cs, 0); - cs_set_consign(&mechboard.right_arm.cs, 0); - encoders_spi_set_value(LEFT_ARM_ENCODER, 0); - encoders_spi_set_value(RIGHT_ARM_ENCODER, 0); - - printf_P(PSTR("ok\r\n")); -} - -/* init arm config */ -void arm_init(void) -{ - uint32_t shoulder_robot; - uint16_t elbow_robot, wrist_robot; - double shoulder_rad, elbow_rad; - int32_t h, d; - uint8_t err = 0; - - memset(&left_arm.status, 0, sizeof(left_arm.status)); - memset(&right_arm.status, 0, sizeof(right_arm.status)); - left_arm.status.event = -1; - right_arm.status.event = -1; - - arm_calibrate(); - - /* set des slopes XXX */ - - /* set maximum moving speeds */ - err |= ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); - err |= ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); - err |= ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); - err |= ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); - - /* left arm init */ - shoulder_robot = encoders_spi_get_value(LEFT_ARM_ENCODER); - err |= ax12_user_read_int(&gen.ax12, L_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); - err |= ax12_user_read_int(&gen.ax12, L_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); - - angle_robot2rad_l(shoulder_robot, elbow_robot, - &shoulder_rad, &elbow_rad); - angle2cart(shoulder_rad, elbow_rad, &h, &d); - printf_P(PSTR("left arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); - left_arm.status.h_mm = h; - left_arm.status.d_mm = d; - left_arm.status.wrist_angle_steps = wrist_robot; - left_arm.status.state = ARM_FLAG_FINISHED; - left_arm.config.csb = &mechboard.left_arm; - - /* left arm init */ - shoulder_robot = encoders_spi_get_value(RIGHT_ARM_ENCODER); - err |= ax12_user_read_int(&gen.ax12, R_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); - err |= ax12_user_read_int(&gen.ax12, R_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); - - angle_robot2rad_r(shoulder_robot, elbow_robot, - &shoulder_rad, &elbow_rad); - angle2cart(shoulder_rad, elbow_rad, &h, &d); - printf_P(PSTR("right arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); - right_arm.status.h_mm = h; - right_arm.status.d_mm = d; - right_arm.status.wrist_angle_steps = wrist_robot; - right_arm.status.state = ARM_FLAG_FINISHED; - right_arm.config.csb = &mechboard.right_arm; - - if (err) - ARM_ERROR("ARM INIT ERROR"); -} diff --git a/projects/microb2010/mechboard/arm_xy.h b/projects/microb2010/mechboard/arm_xy.h deleted file mode 100644 index 3b1407f..0000000 --- a/projects/microb2010/mechboard/arm_xy.h +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: arm_xy.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $ - * - * Fabrice DESCLAUX - * Olivier MATZ - */ - -/* all static configuration for an arm */ -struct arm_config { - void (*wrist_angle_deg2robot)(double wrist_edg, - double *wrist_out); - - void (*angle_rad2robot)(double shoulder_deg, double elbow_deg, - double *shoulder_out, double *elbow_out); - - void (*angle_robot2rad)(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad); - - /* ax12 identifiers */ - int8_t elbow_ax12; - int8_t wrist_ax12; - - /* related control system */ - struct cs_block *csb; - - /* if true, don't apply to ax12 / cs */ - uint8_t simulate; -}; - -/* request for a final position, in mm */ -struct arm_request { - int32_t h_mm; - int32_t d_mm; - int32_t w_deg; -}; - -/* returned by arm_test_traj_end() */ -#define ARM_TRAJ_END 0x01 -#define ARM_TRAJ_NEAR 0x02 -#define ARM_TRAJ_TIMEOUT 0x04 -#define ARM_TRAJ_ERROR 0x08 - -#define ARM_TRAJ_ALL (ARM_TRAJ_END|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR) -#define ARM_TRAJ_ALL_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR) -#define ARM_TRAJ_END_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR) - -#define ARM_STATE_INIT 0 -#define ARM_FLAG_MOVING 0x01 /* arm is currently moving */ -#define ARM_FLAG_LAST_STEP 0x02 /* no more step is needed */ -#define ARM_FLAG_IN_WINDOW 0x04 /* arm speed and pos are ok */ -#define ARM_FLAG_TIMEOUT 0x08 /* too much time too reach pos */ -#define ARM_FLAG_ERROR 0x10 /* error */ -#define ARM_FLAG_FINISHED (ARM_FLAG_LAST_STEP | ARM_FLAG_IN_WINDOW) - -/* Describes the current position of the arm. Mainly filled by - * arm_do_step(), arm_set_wrist(), arm_do_xy_cb(), ... */ -struct arm_status { - /* current position */ - int32_t h_mm; - int32_t d_mm; - - /* wrist goal position (set once at init) */ - int16_t wrist_angle_steps; - - /* current angles in steps */ - int32_t elbow_angle_steps; - int32_t shoulder_angle_steps; - - /* current angles in radian */ - double elbow_angle_rad; - double shoulder_angle_rad; - - /* time before next update */ - uint32_t next_update_time; - - /* what speed to be applied, in specific speed unit */ - uint32_t shoulder_speed; - uint32_t elbow_speed; - - volatile int8_t state; /* see list of flags above */ - int8_t event; /* scheduler event, -1 if not running */ - - microseconds start_time; /* when we started that command */ - microseconds wrist_reach_time; /* when the wrist should reach dest */ - microseconds pos_reached_time; /* when last step is sent */ -}; - -struct arm { - struct arm_config config; - struct arm_status status; - struct arm_request req; -}; - -extern struct arm left_arm; -extern struct arm right_arm; - -uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask); -uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask); - -/* do a specific move to distance, height. This function _must_ be - * called from a context with a prio < ARM_PRIO to avoid any race - * condition. */ -int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg); - -void arm_dump(struct arm *arm); -void arm_calibrate(void); -void arm_init(void); diff --git a/projects/microb2010/mechboard/commands_mechboard.c b/projects/microb2010/mechboard/commands_mechboard.c deleted file mode 100644 index 2790618..0000000 --- a/projects/microb2010/mechboard/commands_mechboard.c +++ /dev/null @@ -1,1033 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: commands_mechboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "sensor.h" -#include "cmdline.h" -#include "state.h" -#include "i2c_protocol.h" -#include "actuator.h" -#include "arm_xy.h" -#include "arm_highlevel.h" -#include "actuator.h" - -extern uint16_t state_debug; - -struct cmd_event_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - - -/* function called when cmd_event is parsed successfully */ -static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - u08 bit=0; - - struct cmd_event_result * res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; - if (!strcmp_P(res->arg2, PSTR("on"))) - mechboard.flags |= bit; - else if (!strcmp_P(res->arg2, PSTR("off"))) - mechboard.flags &= bit; - else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), - (DO_ENCODERS & mechboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), - (DO_CS & mechboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), - (DO_BD & mechboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), - (DO_POWER & mechboard.flags) ? "on":"off"); - } - return; - } - - if (!strcmp_P(res->arg1, PSTR("encoders"))) - bit = DO_ENCODERS; - else if (!strcmp_P(res->arg1, PSTR("cs"))) { - if (!strcmp_P(res->arg2, PSTR("on"))) - arm_calibrate(); - bit = DO_CS; - } - else if (!strcmp_P(res->arg1, PSTR("bd"))) - bit = DO_BD; - else if (!strcmp_P(res->arg1, PSTR("power"))) - bit = DO_POWER; - - - if (!strcmp_P(res->arg2, PSTR("on"))) - mechboard.flags |= bit; - else if (!strcmp_P(res->arg2, PSTR("off"))) { - if (!strcmp_P(res->arg1, PSTR("cs"))) { - pwm_ng_set(LEFT_ARM_PWM, 0); - pwm_ng_set(RIGHT_ARM_PWM, 0); - } - mechboard.flags &= (~bit); - } - printf_P(PSTR("%s is %s\r\n"), res->arg1, - (bit & mechboard.flags) ? "on":"off"); -} - -prog_char str_event_arg0[] = "event"; -parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#bd#power"; -parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); -prog_char str_event_arg2[] = "on#off#show"; -parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); - -prog_char help_event[] = "Enable/disable events"; -parse_pgm_inst_t cmd_event = { - .f = cmd_event_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_event, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* Color */ - -/* this structure is filled when cmd_color is parsed successfully */ -struct cmd_color_result { - fixed_string_t arg0; - fixed_string_t color; -}; - -/* function called when cmd_color is parsed successfully */ -static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; - if (!strcmp_P(res->color, PSTR("red"))) { - mechboard.our_color = I2C_COLOR_RED; - } - else if (!strcmp_P(res->color, PSTR("green"))) { - mechboard.our_color = I2C_COLOR_GREEN; - } - printf_P(PSTR("Done\r\n")); -} - -prog_char str_color_arg0[] = "color"; -parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0); -prog_char str_color_color[] = "green#red"; -parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color); - -prog_char help_color[] = "Set our color"; -parse_pgm_inst_t cmd_color = { - .f = cmd_color_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_color, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, - NULL, - }, -}; - -/**********************************************************/ -/* arm_show */ - -/* this structure is filled when cmd_arm_show is parsed successfully */ -struct cmd_arm_show_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - -/* function called when cmd_arm_show is parsed successfully */ -static void cmd_arm_show_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_show_result *res = parsed_result; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) - arm_dump(&left_arm); - else if (strcmp_P(res->arg1, PSTR("right")) == 0) - arm_dump(&right_arm); - else { - arm_dump(&left_arm); - arm_dump(&right_arm); - } -} - -prog_char str_arm_show_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg0, str_arm_show_arg0); -prog_char str_arm_show_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg1, str_arm_show_arg1); -prog_char str_arm_show_arg2[] = "show"; -parse_pgm_token_string_t cmd_arm_show_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg2, str_arm_show_arg2); - -prog_char help_arm_show[] = "Show arm status"; -parse_pgm_inst_t cmd_arm_show = { - .f = cmd_arm_show_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_show, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_show_arg0, - (prog_void *)&cmd_arm_show_arg1, - (prog_void *)&cmd_arm_show_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* arm_goto */ - -/* this structure is filled when cmd_arm_goto is parsed successfully */ -struct cmd_arm_goto_result { - fixed_string_t arg0; - fixed_string_t arg1; - int16_t arg2; - int16_t arg3; - int16_t arg4; -}; - -/* function called when cmd_arm_goto is parsed successfully */ -static void cmd_arm_goto_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_goto_result *res = parsed_result; - uint8_t err; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) { - arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4); - err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("err %x\r\n"), err); - } - else if (strcmp_P(res->arg1, PSTR("right")) == 0) { - arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4); - err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("err %x\r\n"), err); - } - else { - arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4); - arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4); - err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("left err %x\r\n"), err); - err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("right err %x\r\n"), err); - } -} - -prog_char str_arm_goto_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg0, str_arm_goto_arg0); -prog_char str_arm_goto_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_goto_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg1, str_arm_goto_arg1); -parse_pgm_token_num_t cmd_arm_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg2, INT16); -parse_pgm_token_num_t cmd_arm_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg3, INT16); -parse_pgm_token_num_t cmd_arm_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg4, INT16); - -prog_char help_arm_goto[] = "Arm goto d_mm,h_mm,w_deg"; -parse_pgm_inst_t cmd_arm_goto = { - .f = cmd_arm_goto_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_goto, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_goto_arg0, - (prog_void *)&cmd_arm_goto_arg1, - (prog_void *)&cmd_arm_goto_arg2, - (prog_void *)&cmd_arm_goto_arg3, - (prog_void *)&cmd_arm_goto_arg4, - NULL, - }, -}; - -/**********************************************************/ -/* arm_goto_fixed */ - -/* this structure is filled when cmd_arm_goto_fixed is parsed successfully */ -struct cmd_arm_goto_fixed_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; - fixed_string_t arg3; -}; - -/* function called when cmd_arm_goto_fixed is parsed successfully */ -static void cmd_arm_goto_fixed_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_goto_fixed_result *res = parsed_result; - void (*f)(uint8_t, uint8_t) = NULL; - uint8_t err, pump_num = 0; - - if (strcmp_P(res->arg2, PSTR("prepare")) == 0) - f = arm_goto_prepare_get; - else if (strcmp_P(res->arg2, PSTR("get")) == 0) - f = arm_goto_get_column; - else if (strcmp_P(res->arg2, PSTR("inter")) == 0) - f = arm_goto_intermediate_get; - else if (strcmp_P(res->arg2, PSTR("inter")) == 0) - f = arm_goto_straight; - - if (f == NULL) - return; - - /* no matter if it's left or right here */ - if (strcmp_P(res->arg3, PSTR("p1")) == 0) - pump_num = PUMP_LEFT1_NUM; - if (strcmp_P(res->arg3, PSTR("p2")) == 0) - pump_num = PUMP_LEFT2_NUM; - - /* /!\ strcmp() inverted logic do handle "both" case */ - if (strcmp_P(res->arg1, PSTR("right"))) - f(ARM_LEFT_NUM, pump_num); - if (strcmp_P(res->arg1, PSTR("left"))) - f(ARM_RIGHT_NUM, pump_num); - - if (strcmp_P(res->arg1, PSTR("right"))) { - err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("left err %x\r\n"), err); - } - if (strcmp_P(res->arg1, PSTR("left"))) { - err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("right err %x\r\n"), err); - } -} - -prog_char str_arm_goto_fixed_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg0, str_arm_goto_fixed_arg0); -prog_char str_arm_goto_fixed_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg1, str_arm_goto_fixed_arg1); -prog_char str_arm_goto_fixed_arg2[] = "prepare#get#inter#straight"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg2, str_arm_goto_fixed_arg2); -prog_char str_arm_goto_fixed_arg3[] = "p1#p2"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg3, str_arm_goto_fixed_arg3); - -prog_char help_arm_goto_fixed[] = "Goto fixed positions"; -parse_pgm_inst_t cmd_arm_goto_fixed = { - .f = cmd_arm_goto_fixed_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_goto_fixed, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_goto_fixed_arg0, - (prog_void *)&cmd_arm_goto_fixed_arg1, - (prog_void *)&cmd_arm_goto_fixed_arg2, - (prog_void *)&cmd_arm_goto_fixed_arg3, - NULL, - }, -}; - -/**********************************************************/ -/* arm_simulate */ - -/* this structure is filled when cmd_arm_simulate is parsed successfully */ -struct cmd_arm_simulate_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - -/* function called when cmd_arm_simulate is parsed successfully */ -static void cmd_arm_simulate_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_simulate_result *res = parsed_result; - uint8_t val; - - if (strcmp_P(res->arg2, PSTR("simulate")) == 0) - val = 1; - else - val = 0; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) - left_arm.config.simulate = 1; - else if (strcmp_P(res->arg1, PSTR("right")) == 0) - right_arm.config.simulate = 1; - else { - left_arm.config.simulate = 1; - right_arm.config.simulate = 1; - } -} - -prog_char str_arm_simulate_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_simulate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg0, str_arm_simulate_arg0); -prog_char str_arm_simulate_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_simulate_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg1, str_arm_simulate_arg1); -prog_char str_arm_simulate_arg2[] = "simulate#real"; -parse_pgm_token_string_t cmd_arm_simulate_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg2, str_arm_simulate_arg2); - -prog_char help_arm_simulate[] = "Simulation or real for arm"; -parse_pgm_inst_t cmd_arm_simulate = { - .f = cmd_arm_simulate_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_simulate, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_simulate_arg0, - (prog_void *)&cmd_arm_simulate_arg1, - (prog_void *)&cmd_arm_simulate_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* finger */ - -/* this structure is filled when cmd_finger is parsed successfully */ -struct cmd_finger_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_finger is parsed successfully */ -static void cmd_finger_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_finger_result *res = parsed_result; - uint16_t dest = 0; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) - dest = FINGER_LEFT; - else if (strcmp_P(res->arg1, PSTR("right")) == 0) - dest = FINGER_RIGHT; - else if (strcmp_P(res->arg1, PSTR("center")) == 0) - dest = FINGER_CENTER; - finger_goto(dest); -} - -prog_char str_finger_arg0[] = "finger"; -parse_pgm_token_string_t cmd_finger_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg0, str_finger_arg0); -prog_char str_finger_arg1[] = "left#right#center"; -parse_pgm_token_string_t cmd_finger_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg1, str_finger_arg1); - -prog_char help_finger[] = "Move finger"; -parse_pgm_inst_t cmd_finger = { - .f = cmd_finger_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_finger, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_finger_arg0, - (prog_void *)&cmd_finger_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* pump */ - -/* this structure is filled when cmd_pump is parsed successfully */ -struct cmd_pump_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - -/* function called when cmd_pump is parsed successfully */ -static void cmd_pump_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_pump_result *res = parsed_result; - int8_t pump_num = 0; - int16_t val = 0; - - if (strcmp_P(res->arg1, PSTR("left1")) == 0) - pump_num = PUMP_LEFT1_NUM; - else if (strcmp_P(res->arg1, PSTR("right1")) == 0) - pump_num = PUMP_RIGHT1_NUM; - else if (strcmp_P(res->arg1, PSTR("left2")) == 0) - pump_num = PUMP_LEFT2_NUM; - else if (strcmp_P(res->arg1, PSTR("right2")) == 0) - pump_num = PUMP_RIGHT2_NUM; - - if (strcmp_P(res->arg2, PSTR("on")) == 0) - val = PUMP_ON; - else if (strcmp_P(res->arg2, PSTR("off")) == 0) - val = PUMP_OFF; - else if (strcmp_P(res->arg2, PSTR("reverse")) == 0) - val = PUMP_REVERSE; - - pump_set(pump_num, val); -} - -prog_char str_pump_arg0[] = "pump"; -parse_pgm_token_string_t cmd_pump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg0, str_pump_arg0); -prog_char str_pump_arg1[] = "left1#left2#right1#right2"; -parse_pgm_token_string_t cmd_pump_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg1, str_pump_arg1); -prog_char str_pump_arg2[] = "on#off#reverse"; -parse_pgm_token_string_t cmd_pump_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg2, str_pump_arg2); - -prog_char help_pump[] = "activate pump"; -parse_pgm_inst_t cmd_pump = { - .f = cmd_pump_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pump, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pump_arg0, - (prog_void *)&cmd_pump_arg1, - (prog_void *)&cmd_pump_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* State1 */ - -/* this structure is filled when cmd_state1 is parsed successfully */ -struct cmd_state1_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_state1 is parsed successfully */ -static void cmd_state1_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state1_result *res = parsed_result; - struct i2c_cmd_mechboard_set_mode command; - - if (!strcmp_P(res->arg1, PSTR("init"))) { - state_init(); - return; - } - - if (!strcmp_P(res->arg1, PSTR("manual"))) - command.mode = I2C_MECHBOARD_MODE_MANUAL; - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - command.mode = I2C_MECHBOARD_MODE_HARVEST; - else if (!strcmp_P(res->arg1, PSTR("lazy_harvest"))) - command.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; - else if (!strcmp_P(res->arg1, PSTR("pickup"))) - command.mode = I2C_MECHBOARD_MODE_PICKUP; - else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel"))) - command.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("get_lintel"))) - command.mode = I2C_MECHBOARD_MODE_GET_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("put_lintel"))) - command.mode = I2C_MECHBOARD_MODE_PUT_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("clear"))) - command.mode = I2C_MECHBOARD_MODE_CLEAR; - else if (!strcmp_P(res->arg1, PSTR("loaded"))) - command.mode = I2C_MECHBOARD_MODE_LOADED; - else if (!strcmp_P(res->arg1, PSTR("store"))) - command.mode = I2C_MECHBOARD_MODE_STORE; - else if (!strcmp_P(res->arg1, PSTR("lazy_pickup"))) - command.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - state_set_mode(&command); -} - -prog_char str_state1_arg0[] = "mechboard"; -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#manual#pickup#prepare_get_lintel#get_lintel#put_lintel#clear#lazy_harvest#harvest#loaded#store#lazy_pickup"; -parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); - -prog_char help_state1[] = "set mechboard mode"; -parse_pgm_inst_t cmd_state1 = { - .f = cmd_state1_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state1, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state1_arg0, - (prog_void *)&cmd_state1_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* State2 */ - -/* this structure is filled when cmd_state2 is parsed successfully */ -struct cmd_state2_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - -/* function called when cmd_state2 is parsed successfully */ -static void cmd_state2_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state2_result *res = parsed_result; - struct i2c_cmd_mechboard_set_mode command; - uint8_t side; - - if (!strcmp_P(res->arg2, PSTR("left"))) - side = I2C_LEFT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("right"))) - side = I2C_RIGHT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else - side = I2C_AUTO_SIDE; - - if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) { - command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - command.prep_pickup.side = side; - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - } - else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) { - command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC; - command.push_temple_disc.side = side; - } - - - state_set_mode(&command); -} - -prog_char str_state2_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0); -prog_char str_state2_arg1[] = "prepare_pickup#push_temple_disc"; -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#auto#center"; -parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2); - -prog_char help_state2[] = "set mechboard mode"; -parse_pgm_inst_t cmd_state2 = { - .f = cmd_state2_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state2, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state2_arg0, - (prog_void *)&cmd_state2_arg1, - (prog_void *)&cmd_state2_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* State3 */ - -/* this structure is filled when cmd_state3 is parsed successfully */ -struct cmd_state3_result { - fixed_string_t arg0; - fixed_string_t arg1; - uint8_t level; -}; - -/* function called when cmd_state3 is parsed successfully */ -static void cmd_state3_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state3_result *res = parsed_result; - struct i2c_cmd_mechboard_set_mode command; - - if (!strcmp_P(res->arg1, PSTR("prepare_build"))) { - command.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD; - command.prep_build.level_l = res->level; - command.prep_build.level_r = res->level; - } - else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) { - command.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE; - command.prep_inside.level_l = res->level; - command.prep_inside.level_r = res->level; - } - else if (!strcmp_P(res->arg1, PSTR("autobuild"))) { - command.mode = I2C_MECHBOARD_MODE_AUTOBUILD; - command.autobuild.level_left = res->level; - command.autobuild.level_right = res->level; - command.autobuild.count_left = 2; - command.autobuild.count_right = 2; - command.autobuild.distance_left = I2C_AUTOBUILD_DEFAULT_DIST; - command.autobuild.distance_right = I2C_AUTOBUILD_DEFAULT_DIST; - command.autobuild.do_lintel = 1; - } - else if (!strcmp_P(res->arg1, PSTR("push_temple"))) { - command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE; - command.push_temple.level = res->level; - } - state_set_mode(&command); -} - -prog_char str_state3_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0); -prog_char str_state3_arg1[] = "prepare_build#autobuild#prepare_inside"; -parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1); -parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, level, UINT8); - -prog_char help_state3[] = "set mechboard mode"; -parse_pgm_inst_t cmd_state3 = { - .f = cmd_state3_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state3, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state3_arg0, - (prog_void *)&cmd_state3_arg1, - (prog_void *)&cmd_state3_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* State4 */ - -/* this structure is filled when cmd_state4 is parsed successfully */ -struct cmd_state4_result { - fixed_string_t arg0; - fixed_string_t arg1; - uint8_t level_l; - uint8_t count_l; - uint8_t dist_l; - uint8_t level_r; - uint8_t count_r; - uint8_t dist_r; - uint8_t do_lintel; -}; - -/* function called when cmd_state4 is parsed successfully */ -static void cmd_state4_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state4_result *res = parsed_result; - struct i2c_cmd_mechboard_set_mode command; - - if (!strcmp_P(res->arg1, PSTR("autobuild"))) { - command.mode = I2C_MECHBOARD_MODE_AUTOBUILD; - command.autobuild.distance_left = res->dist_l; - command.autobuild.distance_right = res->dist_r; - command.autobuild.level_left = res->level_l; - command.autobuild.level_right = res->level_r; - command.autobuild.count_left = res->count_l; - command.autobuild.count_right = res->count_r; - command.autobuild.do_lintel = res->do_lintel; - } - state_set_mode(&command); -} - -prog_char str_state4_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg0, str_state4_arg0); -prog_char str_state4_arg1[] = "autobuild"; -parse_pgm_token_string_t cmd_state4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg1, str_state4_arg1); -parse_pgm_token_num_t cmd_state4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_l, UINT8); -parse_pgm_token_num_t cmd_state4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_l, UINT8); -parse_pgm_token_num_t cmd_state4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_l, UINT8); -parse_pgm_token_num_t cmd_state4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_r, UINT8); -parse_pgm_token_num_t cmd_state4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_r, UINT8); -parse_pgm_token_num_t cmd_state4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_r, UINT8); -parse_pgm_token_num_t cmd_state4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, do_lintel, UINT8); - -prog_char help_state4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)"; -parse_pgm_inst_t cmd_state4 = { - .f = cmd_state4_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state4, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state4_arg0, - (prog_void *)&cmd_state4_arg1, - (prog_void *)&cmd_state4_arg2, - (prog_void *)&cmd_state4_arg3, - (prog_void *)&cmd_state4_arg4, - (prog_void *)&cmd_state4_arg5, - (prog_void *)&cmd_state4_arg6, - (prog_void *)&cmd_state4_arg7, - (prog_void *)&cmd_state4_arg8, - NULL, - }, -}; - -/**********************************************************/ -/* State5 */ - -/* this structure is filled when cmd_state5 is parsed successfully */ -struct cmd_state5_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; - fixed_string_t arg3; -}; - -/* function called when cmd_state5 is parsed successfully */ -static void cmd_state5_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state5_result *res = parsed_result; - struct i2c_cmd_mechboard_set_mode command; - uint8_t side; - - if (!strcmp_P(res->arg2, PSTR("left"))) - side = I2C_LEFT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("right"))) - side = I2C_RIGHT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else - side = I2C_AUTO_SIDE; - - command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - command.prep_pickup.side = side; - - if (!strcmp_P(res->arg3, PSTR("harvest"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("lazy_harvest"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("pickup"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PICKUP; - else if (!strcmp_P(res->arg3, PSTR("clear"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_CLEAR; - else if (!strcmp_P(res->arg3, PSTR("store"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_STORE; - else if (!strcmp_P(res->arg3, PSTR("lazy_pickup"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - - state_set_mode(&command); -} - -prog_char str_state5_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg0, str_state5_arg0); -prog_char str_state5_arg1[] = "prepare_pickup"; -parse_pgm_token_string_t cmd_state5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg1, str_state5_arg1); -prog_char str_state5_arg2[] = "left#right#auto#center"; -parse_pgm_token_string_t cmd_state5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg2, str_state5_arg2); -prog_char str_state5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear"; -parse_pgm_token_string_t cmd_state5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg3, str_state5_arg3); - -prog_char help_state5[] = "set mechboard mode 2"; -parse_pgm_inst_t cmd_state5 = { - .f = cmd_state5_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state5, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state5_arg0, - (prog_void *)&cmd_state5_arg1, - (prog_void *)&cmd_state5_arg2, - (prog_void *)&cmd_state5_arg3, - NULL, - }, -}; - -/**********************************************************/ -/* State_Machine */ - -/* this structure is filled when cmd_state_machine is parsed successfully */ -struct cmd_state_machine_result { - fixed_string_t arg0; -}; - -/* function called when cmd_state_machine is parsed successfully */ -static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ - state_machine(); -} - -prog_char str_state_machine_arg0[] = "state_machine"; -parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0); - -prog_char help_state_machine[] = "launch state machine"; -parse_pgm_inst_t cmd_state_machine = { - .f = cmd_state_machine_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state_machine, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_machine_arg0, - NULL, - }, -}; - -/**********************************************************/ -/* State_Debug */ - -/* this structure is filled when cmd_state_debug is parsed successfully */ -struct cmd_state_debug_result { - fixed_string_t arg0; - uint8_t on; -}; - -/* function called when cmd_state_debug is parsed successfully */ -static void cmd_state_debug_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state_debug_result *res = parsed_result; - state_debug = res->on; -} - -prog_char str_state_debug_arg0[] = "state_debug"; -parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0); -parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8); - -prog_char help_state_debug[] = "Set debug timer for state machine"; -parse_pgm_inst_t cmd_state_debug = { - .f = cmd_state_debug_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state_debug, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_debug_arg0, - (prog_void *)&cmd_state_debug_on, - NULL, - }, -}; - -/**********************************************************/ -/* Servo_Lintel */ - -/* this structure is filled when cmd_servo_lintel is parsed successfully */ -struct cmd_servo_lintel_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_servo_lintel is parsed successfully */ -static void cmd_servo_lintel_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_servo_lintel_result *res = parsed_result; - if (!strcmp_P(res->arg1, PSTR("out"))) - servo_lintel_out(); - else if (!strcmp_P(res->arg1, PSTR("1lin"))) - servo_lintel_1lin(); - else if (!strcmp_P(res->arg1, PSTR("2lin"))) - servo_lintel_2lin(); - -} - -prog_char str_servo_lintel_arg0[] = "servo_lintel"; -parse_pgm_token_string_t cmd_servo_lintel_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg0, str_servo_lintel_arg0); -prog_char str_servo_lintel_arg1[] = "out#1lin#2lin"; -parse_pgm_token_string_t cmd_servo_lintel_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg1, str_servo_lintel_arg1); - -prog_char help_servo_lintel[] = "Servo_Lintel function"; -parse_pgm_inst_t cmd_servo_lintel = { - .f = cmd_servo_lintel_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_servo_lintel, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_lintel_arg0, - (prog_void *)&cmd_servo_lintel_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Pump_Current */ - -/* this structure is filled when cmd_pump_current is parsed successfully */ -struct cmd_pump_current_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_pump_current is parsed successfully */ -static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ - printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"), - mechboard.pump_left1_current, mechboard.pump_left2_current, - sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4)); -} - -prog_char str_pump_current_arg0[] = "pump_current"; -parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0); -prog_char str_pump_current_arg1[] = "show"; -parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1); - -prog_char help_pump_current[] = "dump pump current"; -parse_pgm_inst_t cmd_pump_current = { - .f = cmd_pump_current_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pump_current, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pump_current_arg0, - (prog_void *)&cmd_pump_current_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Manivelle */ - -/* this structure is filled when cmd_manivelle is parsed successfully */ -struct cmd_manivelle_result { - fixed_string_t arg0; - uint8_t step; -}; - -/* function called when cmd_manivelle is parsed successfully */ -static void cmd_manivelle_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_manivelle_result *res = parsed_result; - state_manivelle(res->step); -} - -prog_char str_manivelle_arg0[] = "manivelle"; -parse_pgm_token_string_t cmd_manivelle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_manivelle_result, arg0, str_manivelle_arg0); -parse_pgm_token_num_t cmd_manivelle_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_manivelle_result, step, UINT8); - -prog_char help_manivelle[] = "Manivelle function"; -parse_pgm_inst_t cmd_manivelle = { - .f = cmd_manivelle_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_manivelle, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_manivelle_arg0, - (prog_void *)&cmd_manivelle_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Test */ - -/* this structure is filled when cmd_test is parsed successfully */ -struct cmd_test_result { - fixed_string_t arg0; -}; - -/* function called when cmd_test is parsed successfully */ -static void cmd_test_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ -} - -prog_char str_test_arg0[] = "test"; -parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); - -prog_char help_test[] = "Test function"; -parse_pgm_inst_t cmd_test = { - .f = cmd_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_test_arg0, - NULL, - }, -}; diff --git a/projects/microb2010/mechboard/cs.c b/projects/microb2010/mechboard/cs.c deleted file mode 100644 index 29a2cab..0000000 --- a/projects/microb2010/mechboard/cs.c +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: cs.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $ - * - */ - -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "main.h" -#include "actuator.h" - -/* called every 5 ms */ -static void do_cs(__attribute__((unused)) void *dummy) -{ - /* read encoders */ - if (mechboard.flags & DO_ENCODERS) { - encoders_spi_manage(NULL); - } - /* control system */ - if (mechboard.flags & DO_CS) { - if (mechboard.left_arm.on) - cs_manage(&mechboard.left_arm.cs); - if (mechboard.right_arm.on) - cs_manage(&mechboard.right_arm.cs); - } - if (mechboard.flags & DO_BD) { - bd_manage_from_cs(&mechboard.left_arm.bd, &mechboard.left_arm.cs); - bd_manage_from_cs(&mechboard.right_arm.bd, &mechboard.right_arm.cs); - } - if (mechboard.flags & DO_POWER) - BRAKE_OFF(); - else - BRAKE_ON(); -} - -void dump_cs_debug(const char *name, struct cs *cs) -{ - DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld", - name, cs_get_consign(cs), cs_get_filtered_consign(cs), - cs_get_error(cs), cs_get_filtered_feedback(cs), - cs_get_out(cs)); -} - -void dump_cs(const char *name, struct cs *cs) -{ - printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld\r\n"), - name, cs_get_consign(cs), cs_get_filtered_consign(cs), - cs_get_error(cs), cs_get_filtered_feedback(cs), - cs_get_out(cs)); -} - -void dump_pid(const char *name, struct pid_filter *pid) -{ - printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), - name, - pid_get_value_in(pid) * pid_get_gain_P(pid), - pid_get_value_I(pid) * pid_get_gain_I(pid), - pid_get_value_D(pid) * pid_get_gain_D(pid), - pid_get_value_out(pid)); -} - -void microb_cs_init(void) -{ - /* ---- CS left_arm */ - /* PID */ - pid_init(&mechboard.left_arm.pid); - pid_set_gains(&mechboard.left_arm.pid, 500, 40, 5000); - pid_set_maximums(&mechboard.left_arm.pid, 0, 5000, 2400); /* max is 12 V */ - pid_set_out_shift(&mechboard.left_arm.pid, 10); - pid_set_derivate_filter(&mechboard.left_arm.pid, 4); - - /* QUADRAMP */ - quadramp_init(&mechboard.left_arm.qr); - quadramp_set_1st_order_vars(&mechboard.left_arm.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mechboard.left_arm.qr, 20, 20); /* set accel */ - - /* CS */ - cs_init(&mechboard.left_arm.cs); - cs_set_consign_filter(&mechboard.left_arm.cs, quadramp_do_filter, &mechboard.left_arm.qr); - cs_set_correct_filter(&mechboard.left_arm.cs, pid_do_filter, &mechboard.left_arm.pid); - cs_set_process_in(&mechboard.left_arm.cs, pwm_ng_set, LEFT_ARM_PWM); - cs_set_process_out(&mechboard.left_arm.cs, encoders_spi_get_value, LEFT_ARM_ENCODER); - cs_set_consign(&mechboard.left_arm.cs, 0); - - /* Blocking detection */ - bd_init(&mechboard.left_arm.bd); - bd_set_speed_threshold(&mechboard.left_arm.bd, 150); - bd_set_current_thresholds(&mechboard.left_arm.bd, 500, 8000, 1000000, 40); - - /* ---- CS right_arm */ - /* PID */ - pid_init(&mechboard.right_arm.pid); - pid_set_gains(&mechboard.right_arm.pid, 500, 40, 5000); - pid_set_maximums(&mechboard.right_arm.pid, 0, 5000, 2400); /* max is 12 V */ - pid_set_out_shift(&mechboard.right_arm.pid, 10); - pid_set_derivate_filter(&mechboard.right_arm.pid, 6); - - /* QUADRAMP */ - quadramp_init(&mechboard.right_arm.qr); - quadramp_set_1st_order_vars(&mechboard.right_arm.qr, 1000, 1000); /* set speed */ - quadramp_set_2nd_order_vars(&mechboard.right_arm.qr, 20, 20); /* set accel */ - - /* CS */ - cs_init(&mechboard.right_arm.cs); - cs_set_consign_filter(&mechboard.right_arm.cs, quadramp_do_filter, &mechboard.right_arm.qr); - cs_set_correct_filter(&mechboard.right_arm.cs, pid_do_filter, &mechboard.right_arm.pid); - cs_set_process_in(&mechboard.right_arm.cs, pwm_ng_set, RIGHT_ARM_PWM); - cs_set_process_out(&mechboard.right_arm.cs, encoders_spi_get_value, RIGHT_ARM_ENCODER); - cs_set_consign(&mechboard.right_arm.cs, 0); - - /* Blocking detection */ - bd_init(&mechboard.right_arm.bd); - bd_set_speed_threshold(&mechboard.right_arm.bd, 150); - bd_set_current_thresholds(&mechboard.right_arm.bd, 500, 8000, 1000000, 40); - - /* set them on !! */ - mechboard.left_arm.on = 1; - mechboard.right_arm.on = 1; - - - scheduler_add_periodical_event_priority(do_cs, NULL, - CS_PERIOD / SCHEDULER_UNIT, - CS_PRIO); -} diff --git a/projects/microb2010/mechboard/state.c b/projects/microb2010/mechboard/state.c deleted file mode 100644 index 10af0f2..0000000 --- a/projects/microb2010/mechboard/state.c +++ /dev/null @@ -1,1407 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "cmdline.h" -#include "sensor.h" -#include "actuator.h" -#include "arm_xy.h" -#include "arm_highlevel.h" -#include "state.h" - -#define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args) -#define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args) -#define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args) - -/* shorter aliases for this file */ -#define MANUAL I2C_MECHBOARD_MODE_MANUAL -#define HARVEST I2C_MECHBOARD_MODE_HARVEST -#define PREPARE_PICKUP I2C_MECHBOARD_MODE_PREPARE_PICKUP -#define PICKUP I2C_MECHBOARD_MODE_PICKUP -#define PREPARE_BUILD I2C_MECHBOARD_MODE_PREPARE_BUILD -#define AUTOBUILD I2C_MECHBOARD_MODE_AUTOBUILD -#define WAIT I2C_MECHBOARD_MODE_WAIT -#define INIT I2C_MECHBOARD_MODE_INIT -#define PREPARE_GET_LINTEL I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL -#define GET_LINTEL I2C_MECHBOARD_MODE_GET_LINTEL -#define PUT_LINTEL I2C_MECHBOARD_MODE_PUT_LINTEL -#define PREPARE_EJECT I2C_MECHBOARD_MODE_PREPARE_EJECT -#define EJECT I2C_MECHBOARD_MODE_EJECT -#define CLEAR I2C_MECHBOARD_MODE_CLEAR -#define LAZY_HARVEST I2C_MECHBOARD_MODE_LAZY_HARVEST -#define LOADED I2C_MECHBOARD_MODE_LOADED -#define PREPARE_INSIDE I2C_MECHBOARD_MODE_PREPARE_INSIDE -#define STORE I2C_MECHBOARD_MODE_STORE -#define LAZY_PICKUP I2C_MECHBOARD_MODE_LAZY_PICKUP -#define MANIVELLE I2C_MECHBOARD_MODE_MANIVELLE -#define PUSH_TEMPLE I2C_MECHBOARD_MODE_PUSH_TEMPLE -#define PUSH_TEMPLE_DISC I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC -#define EXIT I2C_MECHBOARD_MODE_EXIT - -static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode); - -static struct i2c_cmd_mechboard_set_mode mainboard_command; -static struct vt100 local_vt100; -static volatile uint8_t prev_state; -static uint8_t pickup_side; -static volatile uint8_t changed = 0; - -uint8_t state_debug = 0; - -void state_dump_sensors(void) -{ - uint16_t tmp = sensor_get_all(); - prog_char *front = PSTR("no_front"); - prog_char *left = PSTR("no_left"); - prog_char *right = PSTR("no_right"); - - if (tmp & _BV(S_FRONT)) - front = PSTR("FRONT"); - if (tmp & _BV(S_LEFT)) { - if (tmp & _BV(S_COL_LEFT)) - left = PSTR("LEFT(red)"); - else - left = PSTR("LEFT(green)"); - } - if (tmp & _BV(S_RIGHT)) { - if (tmp & _BV(S_COL_RIGHT)) - right = PSTR("RIGHT(red)"); - else - right = PSTR("RIGHT(green)"); - } - - STMCH_DEBUG("sensors = %S %S %S", front, left, right); -} - -/* return 1 if column is there */ -uint8_t arm_get_sensor(uint8_t arm_num) -{ - if (arm_num == ARM_LEFT_NUM) { - return sensor_get(S_LEFT); - } - else if (arm_num == ARM_RIGHT_NUM) { - return sensor_get(S_RIGHT); - } - return 0; -} - -/* return 0 if color is correct, else return -1 */ -int8_t arm_get_color_sensor(uint8_t arm_num) -{ - uint8_t col = 0; - if (arm_num == ARM_LEFT_NUM) { - col = sensor_get(S_COL_LEFT); - } - else if (arm_num == ARM_RIGHT_NUM) { - col = sensor_get(S_COL_RIGHT); - } - - /* if col != 0, column is red */ - if (col) { - if (mechboard.our_color == I2C_COLOR_RED) - return 0; - return -1; - } - else { - if (mechboard.our_color == I2C_COLOR_GREEN) - return 0; - return -1; - } -} - -void state_debug_wait_key_pressed(void) -{ - if (!state_debug) - return; - printf_P(PSTR("press a key\r\n")); - while(!cmdline_keypressed()); -} - -/* set a new state, return 0 on success */ -int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd) -{ - changed = 1; - prev_state = mainboard_command.mode; - memcpy(&mainboard_command, cmd, sizeof(mainboard_command)); - STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode); - return 0; -} - -/* check that state is the one in parameter and that state did not - * changed */ -uint8_t state_check(uint8_t mode) -{ - int16_t c; - if (mode != mainboard_command.mode) - return 0; - - if (changed) - return 0; - - /* force quit when CTRL-C is typed */ - c = cmdline_getchar(); - if (c == -1) - return 1; - if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) { - mainboard_command.mode = EXIT; - return 0; - } - return 1; -} - -uint8_t state_get_mode(void) -{ - return mainboard_command.mode; -} - -void pump_reset_all(void) -{ - uint8_t i; - for (i=0; i<4; i++) { - pump_set(i, PUMP_OFF); - pump_mark_free(i); - } -} - -void pump_check_all(void) -{ - if (pump_is_busy(PUMP_LEFT1_NUM) && - mechboard.pump_left1_current < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark l1 as free"); - pump_mark_free(PUMP_LEFT1_NUM); - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - } - - if (pump_is_busy(PUMP_LEFT2_NUM) && - mechboard.pump_left2_current < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark l2 as free"); - pump_mark_free(PUMP_LEFT2_NUM); - pump_set(PUMP_LEFT2_NUM, PUMP_OFF); - } - - if (pump_is_busy(PUMP_RIGHT1_NUM) && - sensor_get_adc(ADC_CSENSE3) < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark r1 as free"); - pump_mark_free(PUMP_RIGHT1_NUM); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - } - - if (pump_is_busy(PUMP_RIGHT2_NUM) && - sensor_get_adc(ADC_CSENSE4) < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark r2 as free"); - pump_mark_free(PUMP_RIGHT2_NUM); - pump_set(PUMP_RIGHT2_NUM, PUMP_OFF); - } -} - -uint8_t get_free_pump_count(void) -{ - uint8_t i, free_pump_count = 0; - for (i=0; i<4; i++) { - if (pump_is_free(i)) - free_pump_count++; - } - return free_pump_count; -} - -/* move finger if we are not in lazy harvest */ -void state_finger_goto(uint8_t mode, uint16_t position) -{ - if (mode == LAZY_HARVEST) - return; - finger_goto(position); -} - -void state_manivelle(int16_t step_deg) -{ - double add_h = 0.; - double add_d = 160.; - double l = 70.; - double step = RAD(step_deg); - microseconds us; - double al = RAD(0); - double ar = RAD(180); - - time_wait_ms(500); - - us = time_get_us2(); - while (1) { - al += step; - ar += step; - arm_do_xy(&left_arm, add_d+l*sin(al), add_h+l*cos(al), 10); - arm_do_xy(&right_arm, add_d+l*sin(ar), add_h+l*cos(ar), 10); - time_wait_ms(25); - if (time_get_us2() - us > (4000L * 1000L)) - break; - } -} - -static void state_do_manivelle(void) -{ - if (!state_check(MANIVELLE)) - return; - state_manivelle(30); - while (state_check(MANIVELLE)); -} - -/* common function for pickup/harvest */ -static void state_pickup_or_harvest(uint8_t mode) -{ - int8_t arm_num, pump_num; - int8_t other_arm_num, other_pump_num; - struct arm *arm; - microseconds us; - uint8_t flags, bad_color = 0, have_2cols = 0; - - pump_check_all(); - - /* get arm num */ - if (pickup_side == I2C_LEFT_SIDE) { - arm_num = ARM_LEFT_NUM; - other_arm_num = ARM_RIGHT_NUM; - } - else { - arm_num = ARM_RIGHT_NUM; - other_arm_num = ARM_LEFT_NUM; - } - - pump_num = arm_get_free_pump(arm_num); - other_pump_num = arm_get_free_pump(other_arm_num); - - /* pump is not free... skip to other arm */ - if (mode == HARVEST && pump_num == -1) { - STMCH_DEBUG("%s no free pump", __FUNCTION__); - if (arm_num == ARM_RIGHT_NUM) { - state_finger_goto(mode, FINGER_CENTER_RIGHT); - pickup_side = I2C_LEFT_SIDE; - } - else { - state_finger_goto(mode, FINGER_CENTER_LEFT); - pickup_side = I2C_RIGHT_SIDE; - } - return; - } - else if (mode == PICKUP && pump_num == -1) { - /* or exit when we are in pickup mode */ - IRQ_LOCK(flags); - if (mainboard_command.mode == mode) - mainboard_command.mode = WAIT; - IRQ_UNLOCK(flags); - } - - us = time_get_us2(); - /* wait front sensor */ - if (mode == HARVEST || mode == LAZY_HARVEST) { - STMCH_DEBUG("%s wait front", __FUNCTION__); - - while (1) { - if (sensor_get(S_FRONT)) - break; - if (state_check(mode) == 0) - return; - /* wait 500ms before reading other - sensors */ - if (time_get_us2() - us < (500 * 1000L)) - continue; - if (arm_get_sensor(arm_num)) - break; - if (arm_get_sensor(other_arm_num)) { - uint8_t tmp; - tmp = arm_num; - arm_num = other_arm_num; - other_arm_num = tmp; - pump_num = arm_get_free_pump(arm_num); - other_pump_num = arm_get_free_pump(other_arm_num); - if (other_pump_num == -1) - return; // XXX - break; - } - } - } - - - STMCH_DEBUG("%s arm_num=%d pump_num=%d", - __FUNCTION__, arm_num, pump_num); - - /* when ready, move finger */ - if (arm_num == ARM_RIGHT_NUM) - state_finger_goto(mode, FINGER_RIGHT); - else - state_finger_goto(mode, FINGER_LEFT); - - state_debug_wait_key_pressed(); - - - arm = arm_num2ptr(arm_num); - - /* prepare arm, should be already done */ - arm_goto_prepare_get(arm_num, pump_num); - while (arm_test_traj_end(arm, ARM_TRAJ_ALL) && - state_check(mode)); - - STMCH_DEBUG("%s arm pos ok", __FUNCTION__); - - state_debug_wait_key_pressed(); - - /* wait to see the column on the sensor */ - us = time_get_us2(); - while (1) { - if (arm_get_sensor(arm_num)) - break; - if (state_check(mode) == 0) - return; - if (mode == PICKUP) /* no timeout in pickup */ - continue; - /* 500ms timeout in harvest, go back */ - if (time_get_us2() - us > 500*1000L) { - STMCH_DEBUG("%s timeout", __FUNCTION__); - - if (arm_num == ARM_RIGHT_NUM) - state_finger_goto(mode, FINGER_LEFT); - else - state_finger_goto(mode, FINGER_RIGHT); - - if (sensor_get(S_FRONT)) - time_wait_ms(500); - - pump_set(pump_num, PUMP_OFF); - return; - } - } - - state_dump_sensors(); - - pump_set(pump_num, PUMP_ON); - /* bad color */ - if (arm_get_color_sensor(arm_num) == -1) { - bad_color = 1; - STMCH_DEBUG("%s prepare eject", __FUNCTION__); - mainboard_command.mode = PREPARE_EJECT; - state_do_eject(arm_num, pump_num, mode); - return; - } - - STMCH_DEBUG("%s sensor ok", __FUNCTION__); - - /* by the way, prepare the other arm */ - if (other_pump_num != -1) - arm_goto_prepare_get(other_arm_num, other_pump_num); - - /* get the column */ - arm_goto_get_column(arm_num, pump_num); - - us = time_get_us2(); - while (1) { - /* wait 50 ms */ - if (time_get_us2() - us > 50*1000L) - break; - if (mode != HARVEST) - continue; - /* if we still see the front sensor, it's because - * there are 2 columns instead of one or because there - * is another column, so send the arm on other - * side. */ - if (sensor_get(S_FRONT) && have_2cols == 0) { - STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__); - have_2cols = 1; - if (finger_get_side() == I2C_LEFT_SIDE) - state_finger_goto(mode, FINGER_RIGHT); - else - state_finger_goto(mode, FINGER_LEFT); - } - } - - if (mode == HARVEST && have_2cols == 0) { - /* just release a bit of effort */ - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(mode, FINGER_RIGHT_RELAX); - } - } - else if (mode == PICKUP) { - /* no free pump on other arm */ - if (other_pump_num == -1) { - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(mode, FINGER_RIGHT_RELAX); - } - } - /* else send finger on the other side */ - else { - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(mode, FINGER_RIGHT); - } - else { - state_finger_goto(mode, FINGER_LEFT); - } - } - } - - us = time_get_us2(); - while (1) { - /* wait 100 ms */ - if (time_get_us2() - us > 100*1000L) - break; - if (mode != HARVEST) - continue; - /* if we still see the front sensor, it's because - * there are 2 columns instead of one or because there - * is another column, so send the arm on other - * side. */ - if (sensor_get(S_FRONT) && have_2cols == 0) { - STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__); - have_2cols = 1; - if (finger_get_side() == I2C_LEFT_SIDE) - state_finger_goto(mode, FINGER_RIGHT); - else - state_finger_goto(mode, FINGER_LEFT); - } - } - - /* consider the column as taken */ - pump_mark_busy(pump_num); - - state_debug_wait_key_pressed(); - - arm_goto_intermediate_get(arm_num, pump_num); - arm_wait_traj_end(arm, ARM_TRAJ_ALL_NEAR); - - /* prepare next */ - pump_num = arm_get_free_pump(arm_num); - if (pump_num == -1) - arm_goto_loaded(arm_num); - else - arm_goto_intermediate_get(arm_num, pump_num); - - state_debug_wait_key_pressed(); - - /* switch to wait state */ - if (get_free_pump_count() == 0) { - IRQ_LOCK(flags); - if (mainboard_command.mode == mode) - mainboard_command.mode = WAIT; - IRQ_UNLOCK(flags); - } - - /* next pickup/harvest will be on the other side */ - if (pickup_side == I2C_LEFT_SIDE) - pickup_side = I2C_RIGHT_SIDE; - else - pickup_side = I2C_LEFT_SIDE; -} - - -/* manual mode, arm position is sent from mainboard */ -static void state_do_manual(void) -{ - if (!state_check(MANUAL)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(MANUAL)); -} - -/* wait mode */ -static void state_do_wait(void) -{ - if (!state_check(WAIT)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(WAIT)); -} - -/* init mode */ -static void state_do_init(void) -{ - if (!state_check(INIT)) - return; - state_init(); - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(INIT)); -} - -/* harvest columns elts from area */ -static void state_do_harvest(void) -{ - if (!state_check(HARVEST)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - state_pickup_or_harvest(HARVEST); -} - -/* harvest columns elts from area without moving finger */ -static void state_do_lazy_harvest(void) -{ - if (!state_check(LAZY_HARVEST)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - state_pickup_or_harvest(LAZY_HARVEST); -} - -/* eject a column. always called from pickup mode. */ -static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode) -{ - struct arm *arm; - arm = arm_num2ptr(arm_num); - - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(old_mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(old_mode, FINGER_RIGHT_RELAX); - } - - /* wait mainboard to eject */ - while (state_check(PREPARE_EJECT)); - - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(old_mode, FINGER_CENTER_LEFT); - } - else { - state_finger_goto(old_mode, FINGER_CENTER_RIGHT); - } - - arm_goto_get_column(arm_num, pump_num); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - time_wait_ms(150); - - state_debug_wait_key_pressed(); - - arm_goto_prepare_eject(arm_num, pump_num); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - - state_debug_wait_key_pressed(); - - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(old_mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(old_mode, FINGER_RIGHT_RELAX); - } - - state_debug_wait_key_pressed(); - - time_wait_ms(300); - arm_goto_eject(arm_num, pump_num); - time_wait_ms(200); - pump_set(pump_num, PUMP_REVERSE); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - - arm_goto_intermediate_get(arm_num, pump_num); - pump_set(pump_num, PUMP_OFF); -} - - -/* prepare pickup in a dispenser, or harvest */ -static void state_do_prepare_pickup(void) -{ - uint8_t left_count = 0, right_count = 0; - int8_t pump_l, pump_r; - - if (!state_check(PREPARE_PICKUP)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_check_all(); - - pump_l = arm_get_free_pump(ARM_LEFT_NUM); - if (pump_l == -1) { - arm_goto_loaded(ARM_LEFT_NUM); - } - else { - arm_goto_intermediate_front_get(ARM_LEFT_NUM, pump_l); - } - - pump_r = arm_get_free_pump(ARM_RIGHT_NUM); - if (pump_r == -1) { - arm_goto_loaded(ARM_RIGHT_NUM); - } - else { - arm_goto_intermediate_front_get(ARM_RIGHT_NUM, pump_r); - } - - arm_wait_both(ARM_TRAJ_ALL); - - if (pump_l != -1) - arm_goto_prepare_get(ARM_LEFT_NUM, pump_l); - if (pump_r != -1) - arm_goto_prepare_get(ARM_RIGHT_NUM, pump_r); - - if (mainboard_command.prep_pickup.side == I2C_AUTO_SIDE) { - left_count += pump_is_busy(PUMP_LEFT1_NUM); - left_count += pump_is_busy(PUMP_LEFT2_NUM); - right_count += pump_is_busy(PUMP_RIGHT1_NUM); - right_count += pump_is_busy(PUMP_RIGHT2_NUM); - if (left_count < right_count) - finger_goto(FINGER_RIGHT); - else - finger_goto(FINGER_LEFT); - } - else if (mainboard_command.prep_pickup.side == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else if (mainboard_command.prep_pickup.side == I2C_RIGHT_SIDE) - finger_goto(FINGER_RIGHT); - else if (mainboard_command.prep_pickup.side == I2C_CENTER_SIDE) - finger_goto(FINGER_CENTER_LEFT); - - /* try to know on which side we have to pickup */ - if (finger_get_side() == I2C_RIGHT_SIDE) { - pickup_side = I2C_LEFT_SIDE; - } - else { - pickup_side = I2C_RIGHT_SIDE; - } - - arm_prepare_free_pumps(); - - mainboard_command.mode = mainboard_command.prep_pickup.next_mode; - - while (state_check(PREPARE_PICKUP)); -} - -/* clear pickup zone, will switch to harvest if needed */ -static void state_do_clear(void) -{ - uint8_t flags, err; - - if (!state_check(CLEAR)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - finger_goto(FINGER_LEFT); - err = WAIT_COND_OR_TIMEOUT(sensor_get(S_LEFT), 500); - if (err) { - IRQ_LOCK(flags); - if (mainboard_command.mode == CLEAR) - mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; - IRQ_UNLOCK(flags); - pickup_side = I2C_LEFT_SIDE; - return; - } - - finger_goto(FINGER_RIGHT); - err = WAIT_COND_OR_TIMEOUT(sensor_get(S_RIGHT), 500); - if (err) { - IRQ_LOCK(flags); - if (mainboard_command.mode == CLEAR) - mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; - IRQ_UNLOCK(flags); - pickup_side = I2C_RIGHT_SIDE; - return; - } - - IRQ_LOCK(flags); - if (mainboard_command.mode == CLEAR) - mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; - IRQ_UNLOCK(flags); -} - -/* do a lazy pickup */ -static void state_do_lazy_pickup(void) -{ - int8_t flags, arm_num, pump_num; - uint32_t us; - - if (!state_check(LAZY_PICKUP)) - return; - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - if (arm_get_sensor(ARM_LEFT_NUM) && - arm_get_sensor(ARM_RIGHT_NUM)) { - IRQ_LOCK(flags); - if (mainboard_command.mode == LAZY_PICKUP) { - mainboard_command.mode = WAIT; - } - IRQ_UNLOCK(flags); - return; - } - - if (finger_get_side() == I2C_RIGHT_SIDE) { - finger_goto(FINGER_LEFT); - arm_num = ARM_LEFT_NUM; - } - else { - finger_goto(FINGER_RIGHT); - arm_num = ARM_RIGHT_NUM; - } - - us = time_get_us2(); - while(1) { - if (state_check(LAZY_PICKUP) == 0) - return; - if (arm_get_sensor(arm_num)) - break; - if (time_get_us2() - us > 500*1000L) { - if (finger_get_side() == I2C_RIGHT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - return; - } - } - - if (arm_get_color_sensor(arm_num) == -1) { - pump_num = arm_get_free_pump(arm_num); - if (pump_num == -1) - return; /* XXX */ - pump_set(pump_num, PUMP_ON); - STMCH_DEBUG("%s prepare eject", __FUNCTION__); - mainboard_command.mode = PREPARE_EJECT; - state_do_eject(arm_num, pump_num, LAZY_PICKUP); - } -} - -/* pickup from a dispenser automatically */ -static void state_do_pickup(void) -{ - if (!state_check(PICKUP)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - /* XXX check that finger is at correct place */ - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - state_pickup_or_harvest(PICKUP); -} - -/* store columns without using arms */ -static void state_do_store(void) -{ - int8_t arm_num; - int8_t other_arm_num; - microseconds us; - - if (!state_check(STORE)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - /* get arm num */ - if (pickup_side == I2C_LEFT_SIDE) { - arm_num = ARM_LEFT_NUM; - other_arm_num = ARM_RIGHT_NUM; - } - else { - arm_num = ARM_RIGHT_NUM; - other_arm_num = ARM_LEFT_NUM; - } - - while (1) { - if (sensor_get(S_FRONT)) - break; - if (state_check(STORE) == 0) - return; - } - - /* when ready, move finger */ - if (arm_num == ARM_RIGHT_NUM) - finger_goto(FINGER_RIGHT); - else - finger_goto(FINGER_LEFT); - - /* wait to see the column on the sensor */ - us = time_get_us2(); - while (1) { - if (arm_get_sensor(arm_num)) - break; - if (state_check(STORE) == 0) - return; - /* 500ms timeout in harvest, go back */ - if (time_get_us2() - us > 500*1000L) { - STMCH_DEBUG("%s timeout", __FUNCTION__); - - if (arm_num == ARM_RIGHT_NUM) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - return; - } - } - - if (arm_get_sensor(arm_num) && arm_get_sensor(other_arm_num)) { - STMCH_DEBUG("%s full", __FUNCTION__); - while (state_check(STORE)); - return; - } - - /* next store will be on the other side */ - if (pickup_side == I2C_LEFT_SIDE) - pickup_side = I2C_RIGHT_SIDE; - else - pickup_side = I2C_LEFT_SIDE; -} - -/* prepare the building of a temple */ -static void state_do_prepare_build(void) -{ - int8_t pump_num, level; - if (!state_check(PREPARE_BUILD)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_check_all(); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - pump_num = arm_get_busy_pump(ARM_LEFT_NUM); - level = mainboard_command.prep_build.level_l; - if (pump_num != -1 && level != -1) - arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_num, - level, I2C_AUTOBUILD_DEFAULT_DIST); - - pump_num = arm_get_busy_pump(ARM_RIGHT_NUM); - level = mainboard_command.prep_build.level_r; - if (pump_num != -1 && level != -1) - arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_num, - level, I2C_AUTOBUILD_DEFAULT_DIST); - - while (state_check(PREPARE_BUILD)); -} - -/* prepare the building of a temple */ -static void state_do_push_temple(void) -{ - uint8_t level; - - level = mainboard_command.push_temple.level; - - if (!state_check(PUSH_TEMPLE)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - arm_goto_prepare_push_temple(ARM_LEFT_NUM); - arm_goto_prepare_push_temple(ARM_RIGHT_NUM); - arm_wait_both(ARM_TRAJ_ALL); - - arm_goto_push_temple(ARM_LEFT_NUM, level); - arm_goto_push_temple(ARM_RIGHT_NUM, level); - - while (state_check(PUSH_TEMPLE)); -} - -/* prepare the building of a temple */ -static void state_do_push_temple_disc(void) -{ - uint8_t side; - struct arm *arm; - - if (!state_check(PUSH_TEMPLE_DISC)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - side = mainboard_command.push_temple_disc.side; - - if (side == I2C_LEFT_SIDE) { - arm = arm_num2ptr(ARM_LEFT_NUM); - arm_goto_prepare_push_temple_disc(ARM_LEFT_NUM); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - arm_goto_push_temple_disc(ARM_LEFT_NUM); - } - else { - arm = arm_num2ptr(ARM_RIGHT_NUM); - arm_goto_prepare_push_temple_disc(ARM_RIGHT_NUM); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - arm_goto_push_temple_disc(ARM_RIGHT_NUM); - } - - while (state_check(PUSH_TEMPLE_DISC)); -} - -/* prepare the building of a temple (mainly for columns) */ -static void state_do_prepare_inside(void) -{ - int8_t pump_num, level_l, level_r; - if (!state_check(PREPARE_INSIDE)) - return; - - level_l = mainboard_command.prep_inside.level_l; - level_r = mainboard_command.prep_inside.level_r; - STMCH_DEBUG("%s mode=%d level_l=%d, level_r=%d", __FUNCTION__, - state_get_mode(), level_l, level_r); - - pump_check_all(); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - pump_num = arm_get_busy_pump(ARM_LEFT_NUM); - if (pump_num == -1) - pump_num = PUMP_LEFT1_NUM; - if (level_l != -1) - arm_goto_prepare_build_inside(ARM_LEFT_NUM, pump_num, - level_l); - - pump_num = arm_get_busy_pump(ARM_RIGHT_NUM); - if (pump_num == -1) - pump_num = PUMP_RIGHT1_NUM; - if (level_r != -1) - arm_goto_prepare_build_inside(ARM_RIGHT_NUM, pump_num, - level_r); - - while (state_check(PREPARE_INSIDE)); -} - -/* moving position */ -static void state_do_loaded(void) -{ - if (!state_check(LOADED)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_check_all(); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - arm_goto_loaded(ARM_LEFT_NUM); - arm_goto_loaded(ARM_RIGHT_NUM); - - while (state_check(LOADED)); -} - -static void state_do_build_lintel(uint8_t level) -{ - STMCH_DEBUG("%s() level=%d have_lintel=%d", - __FUNCTION__, level, mechboard.lintel_count); - - servo_lintel_out(); - - arm_goto_prepare_get_lintel_inside1(); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE); - pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE); - arm_goto_prepare_get_lintel_inside2(mechboard.lintel_count); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - arm_goto_get_lintel_inside(mechboard.lintel_count); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - time_wait_ms(150); - arm_goto_prepare_build_lintel1(); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - arm_goto_prepare_build_lintel2(level); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - arm_goto_prepare_build_lintel3(level); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - if (mechboard.lintel_count == 1) - servo_lintel_1lin(); - else - servo_lintel_2lin(); - - arm_goto_build_lintel(level); - arm_wait_both(ARM_TRAJ_ALL); - time_wait_ms(170); - pump_set(PUMP_LEFT1_NUM, PUMP_ON); - time_wait_ms(50); /* right arm a bit after */ - pump_set(PUMP_RIGHT1_NUM, PUMP_ON); - time_wait_ms(130); - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - - mechboard.lintel_count --; -} - -/* Build one level of column. If pump_r or pump_l is -1, don't build - * with this arm. */ -static void state_do_build_column(uint8_t level_l, int8_t pump_l, - uint8_t dist_l, - uint8_t level_r, int8_t pump_r, - uint8_t dist_r) -{ - STMCH_DEBUG("%s() level_l=%d pump_l=%d level_r=%d pump_r=%d", - __FUNCTION__, level_l, pump_l, level_r, pump_r); - - /* nothing to do */ - if (pump_l == -1 && pump_r == -1) - return; - - /* go above the selected level */ - if (pump_l != -1) - arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_l, level_l, dist_l); - if (pump_r != -1) - arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_r, level_r, dist_r); - STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL), - arm_test_traj_end(&right_arm, ARM_TRAJ_ALL)); - arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL); - STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL), - arm_test_traj_end(&right_arm, ARM_TRAJ_ALL)); - - state_debug_wait_key_pressed(); - - /* drop columns of P2 */ - if (pump_l != -1) - arm_goto_autobuild(ARM_LEFT_NUM, pump_l, level_l, dist_l); - if (pump_r != -1) - arm_goto_autobuild(ARM_RIGHT_NUM, pump_r, level_r, dist_r); - arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL); - - state_debug_wait_key_pressed(); - - time_wait_ms(150); - if (pump_l != -1) - pump_set(pump_l, PUMP_REVERSE); - if (pump_r != -1) - pump_set(pump_r, PUMP_REVERSE); - time_wait_ms(150); - if (pump_l != -1) { - pump_set(pump_l, PUMP_OFF); - pump_mark_free(pump_l); - } - if (pump_r != -1) { - pump_set(pump_r, PUMP_OFF); - pump_mark_free(pump_r); - } - - state_debug_wait_key_pressed(); -} - -/* autobuild columns elts from area */ -/* check level to avoid bad things ? */ -/* check if enough cols ? */ -static void state_do_autobuild(void) -{ - int8_t pump_l, pump_r; - /* copy command into local data */ - int8_t level_l = mainboard_command.autobuild.level_left; - int8_t level_r = mainboard_command.autobuild.level_right; - uint8_t count_l = mainboard_command.autobuild.count_left; - uint8_t count_r = mainboard_command.autobuild.count_right; - uint8_t dist_l = mainboard_command.autobuild.distance_left; - uint8_t dist_r = mainboard_command.autobuild.distance_right; - uint8_t do_lintel = mainboard_command.autobuild.do_lintel; - int8_t max_level = level_l; - - - if (!state_check(AUTOBUILD)) - return; - - STMCH_DEBUG("%s mode=%d do_lintel=%d", __FUNCTION__, - state_get_mode(), do_lintel); - STMCH_DEBUG(" left: level=%d count=%d", level_l, count_l); - STMCH_DEBUG(" right: level=%d count=%d", level_r, count_r); - - /* - * build the first level of column if needed - */ - - /* don't build with this arm if no pump or if we don't ask to */ - pump_l = arm_get_busy_pump(ARM_LEFT_NUM); - if (count_l == 0) - pump_l = -1; - pump_r = arm_get_busy_pump(ARM_RIGHT_NUM); - if (count_r == 0) - pump_r = -1; - - if (pump_l == -1 && pump_r == -1) - goto lintel_only; - - state_do_build_column(level_l, pump_l, dist_l, - level_r, pump_r, dist_r); - - /* one level up */ - if (pump_l != -1) { - count_l --; - level_l ++; - max_level = level_l; - } - if (pump_r != -1) { - count_r --; - level_r ++; - if (level_r > max_level) - max_level = level_r; - } - - /* - * build the second level of column if needed - */ - - /* don't build with this arm if no pump or if we don't ask to */ - pump_l = arm_get_busy_pump(ARM_LEFT_NUM); - if (count_l == 0) - pump_l = -1; - pump_r = arm_get_busy_pump(ARM_RIGHT_NUM); - if (count_r == 0) - pump_r = -1; - - state_do_build_column(level_l, pump_l, dist_l, - level_r, pump_r, dist_r); - - /* one level up */ - if (pump_l != -1) { - count_l --; - level_l ++; - max_level = level_l; - } - if (pump_r != -1) { - count_r --; - level_r ++; - if (level_r > max_level) - max_level = level_r; - } - - state_debug_wait_key_pressed(); - - if (mechboard.lintel_count != 0 && do_lintel != 0) { - arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, - PUMP_LEFT1_NUM, - max_level, - I2C_AUTOBUILD_DEFAULT_DIST); - arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, - PUMP_RIGHT1_NUM, - max_level, - I2C_AUTOBUILD_DEFAULT_DIST); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - state_debug_wait_key_pressed(); - - arm_goto_prepare_autobuild_inside(ARM_LEFT_NUM, - PUMP_LEFT1_NUM, - max_level); - arm_goto_prepare_autobuild_inside(ARM_RIGHT_NUM, - PUMP_RIGHT1_NUM, - max_level); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - state_debug_wait_key_pressed(); - } - - lintel_only: - if (mechboard.lintel_count == 0 || do_lintel == 0) { - mainboard_command.mode = WAIT; - return; - } - - state_do_build_lintel(max_level); - mainboard_command.mode = WAIT; -} - -/* prepare to get the lintel */ -static void state_do_prepare_get_lintel(void) -{ - if (!state_check(PREPARE_GET_LINTEL)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - arm_goto_prepare_get_lintel_disp(); - arm_wait_both(ARM_TRAJ_ALL); - - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - - /* go fully left or right */ - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - while (state_check(PREPARE_GET_LINTEL)); -} - -/* get the lintel from the dispenser */ -static void state_do_get_lintel(void) -{ - if (!state_check(GET_LINTEL)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE); - pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE); - - arm_goto_get_lintel_disp(); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - - time_wait_ms(200); - - STMCH_DEBUG("%s left1=%d left2=%d", __FUNCTION__, - mechboard.pump_left1_current, - sensor_get_adc(ADC_CSENSE3)); - - while (state_check(GET_LINTEL)); - - /* mainboard asked to release lintel, so release pump first */ - if (state_get_mode() == PREPARE_GET_LINTEL) { - pump_set(PUMP_LEFT1_NUM, PUMP_ON); - pump_set(PUMP_RIGHT1_NUM, PUMP_ON); - time_wait_ms(200); - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - } -} - -/* put the lintel inside the robot */ -static void state_do_put_lintel(void) -{ - uint8_t prev_lin_count; - - if (!state_check(PUT_LINTEL)) - return; - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - prev_lin_count = mechboard.lintel_count; - mechboard.lintel_count ++; - - arm_goto_prepare_get_lintel_disp(); - arm_wait_both(ARM_TRAJ_ALL); - - servo_lintel_out(); - - arm_goto_prepare_put_lintel(); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - - arm_goto_put_lintel(prev_lin_count); - arm_wait_both(ARM_TRAJ_ALL); - - pump_set(PUMP_LEFT1_NUM, PUMP_ON); - pump_set(PUMP_RIGHT1_NUM, PUMP_ON); - - if (mechboard.lintel_count == 1) - servo_lintel_1lin(); - else - servo_lintel_2lin(); - - time_wait_ms(300); - - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - - arm_goto_prepare_put_lintel(); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - - while (state_check(PUT_LINTEL)); -} - -/* main state machine */ -void state_machine(void) -{ - while (state_get_mode() != EXIT) { - changed = 0; - state_do_init(); - state_do_manual(); - state_do_harvest(); - state_do_lazy_harvest(); - state_do_prepare_pickup(); - state_do_pickup(); - state_do_prepare_inside(); - state_do_prepare_build(); - state_do_autobuild(); - state_do_prepare_get_lintel(); - state_do_get_lintel(); - state_do_put_lintel(); - state_do_loaded(); - state_do_clear(); - state_do_lazy_pickup(); - state_do_wait(); - state_do_store(); - state_do_manivelle(); - state_do_push_temple(); - state_do_push_temple_disc(); - } -} - -void state_init(void) -{ - vt100_init(&local_vt100); - mainboard_command.mode = WAIT; - pump_reset_all(); - mechboard.lintel_count = 1; - mechboard.column_flags = 0; - servo_lintel_1lin(); - finger_goto(FINGER_LEFT); -} diff --git a/projects/microb2010/sensorboard/beacon.c b/projects/microb2010/sensorboard/beacon.c deleted file mode 100755 index 34f1686..0000000 --- a/projects/microb2010/sensorboard/beacon.c +++ /dev/null @@ -1,397 +0,0 @@ - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "sensor.h" - -#include "../common/i2c_commands.h" -#include "main.h" -#include "beacon.h" - -struct beacon beacon; - -#define BEACON_PWM_VALUE 1000 -#define IR_SENSOR() (!!(PINK&(1<<2))) -#define MODULO_TIMER (1023L) -#define COEFF_TIMER (2) -#define COEFF_MULT (100L) -#define COEFF_MULT2 (1000L) -#define BEACON_SIZE (9) -#define BEACON_MAX_SAMPLE (3) - -#define OPPONENT_POS_X (11) -#define OPPONENT_POS_Y (22) - -#define BEACON_DEBUG(args...) DEBUG(E_USER_BEACON, args) -#define BEACON_NOTICE(args...) NOTICE(E_USER_BEACON, args) -#define BEACON_ERROR(args...) ERROR(E_USER_BEACON, args) - -static volatile int32_t rising = -1; -static volatile int32_t falling = -1; - -static int32_t get_dist(float size); -static int32_t get_angle(int32_t middle, int32_t ref); - -static int32_t pos_ref = 0; -static int32_t invalid_count = 0; - -static volatile int32_t beacon_speed; -static volatile int32_t beacon_save_count = 0; -static volatile int32_t beacon_prev_save_count = 0; -static volatile int32_t count = 0; -static volatile int32_t count_diff_rising = 0; -static volatile int32_t count_diff_falling = 0; -static int32_t beacon_coeff = 0; - -static volatile int8_t valid_beacon = 0; - -static volatile int32_t beacon_pos; - -//static int32_t beacon_sample_size[BEACON_MAX_SAMPLE]; - -int32_t encoders_spi_get_value_beacon(void *number) -{ - int32_t ret; - - ret = encoders_spi_get_value(number); - return ret*4; -} - - -void encoders_spi_set_value_beacon(void * number, int32_t v) -{ - encoders_spi_set_value(number, v/4); -} - -int32_t encoders_spi_update_beacon_speed(void * number) -{ - int32_t ret; - uint8_t flags; - - IRQ_LOCK(flags); - ret = encoders_spi_get_value_beacon(number); - beacon_speed = ret - beacon_pos; - beacon_pos = ret; - beacon_prev_save_count = beacon_save_count; - beacon_save_count = TCNT3; - IRQ_UNLOCK(flags); - - beacon_coeff = COEFF_TIMER * COEFF_MULT;//beacon_speed * COEFF_MULT / ((beacon_prev_save_count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER); - - return beacon_speed; -} - - -void beacon_init(void) -{ - //int8_t i; - - beacon_reset_pos(); - pos_ref = encoders_spi_get_value_beacon(BEACON_ENCODER); - - memset(&beacon, 0, sizeof(struct beacon)); - beacon.opponent_x = I2C_OPPONENT_NOT_THERE; - - beacon_speed = 0; - - /*for(i=0;i falling, so invert both and recalculate size and middle */ - if(local_falling < local_rising){ - temp = local_rising; - local_rising = local_falling; - local_falling = temp; - size = BEACON_STEP_TOUR - local_falling + local_rising; - middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR); - edge = local_falling; - } - /* else rising > falling */ - else{ - size = local_falling - local_rising; - middle = local_rising + (size / 2); - edge = local_rising; - } - - //for(i=BEACON_MAX_SAMPLE-1;i>0;i--){ - // beacon_sample_size[i] = beacon_sample_size[i-1]; - // total_size += beacon_sample_size[i]; - //} - //beacon_sample_size[0] = size; - //total_size += size; - //total_size /= BEACON_MAX_SAMPLE; - - //BEACON_DEBUG("rising2= %ld\t",local_rising); - //BEACON_DEBUG("falling2= %ld\r\n",local_falling); - /* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */ - BEACON_DEBUG("size= %f\r\n",size); - //BEACON_DEBUG("middle= %ld\r\n",middle); - - local_angle = get_angle(middle,0); - BEACON_NOTICE("opponent angle= %ld\t",local_angle); - - local_dist = get_dist(size); - BEACON_NOTICE("opponent dist= %ld\r\n",local_dist); - - beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y); - - IRQ_LOCK(flags); - beacon.opponent_x = result_x; - beacon.opponent_y = result_y; - beacon.opponent_angle = local_angle; - beacon.opponent_dist = local_dist; - /* for I2C test */ - //beacon.opponent_x = OPPONENT_POS_X; - //beacon.opponent_y = OPPONENT_POS_Y; - IRQ_UNLOCK(flags); - - BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x); - BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y); - } - else { - BEACON_NOTICE("non valid\r\n\n"); - } - - falling = -1; -} - -static int32_t get_dist(float size) -{ - int32_t dist=0; - //int32_t alpha=0; - - //alpha = (size*2*M_PI*COEFF_MULT2); - //dist = ((2*BEACON_SIZE*BEACON_STEP_TOUR*COEFF_MULT2)/alpha)/2; - - /* function found by measuring points up to 80cm */ - //dist = ((size - 600)*(size - 600)) / 2400 +28; - - /* new function */ - /* dist = a0 + a1*x + a2*x² + a3x³ */ - dist = 1157.3 + (1.4146*size) + (-0.013508*size*size) + (0.00001488*size*size*size); - - return dist; - -} - -static int32_t get_angle(int32_t middle, int32_t ref) -{ - int32_t ret_angle; - - ret_angle = (middle - ref) * 360 / BEACON_STEP_TOUR; - - if(ret_angle > 360) - ret_angle -= 360; - - return ret_angle; -} - -void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y) -{ - uint8_t flags; - - int32_t local_x; - int32_t local_y; - int32_t x_opponent; - int32_t y_opponent; - int32_t local_robot_angle; - - IRQ_LOCK(flags); - local_x = beacon.robot_x; - local_y = beacon.robot_y; - local_robot_angle = beacon.robot_angle; - IRQ_UNLOCK(flags); - - if (local_robot_angle < 0) - local_robot_angle += 360; - - x_opponent = cos((local_robot_angle + angle)* 2 * M_PI / 360)* dist; - y_opponent = sin((local_robot_angle + angle)* 2 * M_PI / 360)* dist; - - //BEACON_DEBUG("x_op= %ld\t",x_opponent); - //BEACON_DEBUG("y_op= %ld\r\n",y_opponent); - //BEACON_NOTICE("robot_x= %ld\t",local_x); - //BEACON_NOTICE("robot_y= %ld\t",local_y); - //BEACON_NOTICE("robot_angle= %ld\r\n",local_robot_angle); - - *x = local_x + x_opponent; - *y = local_y + y_opponent; - -} diff --git a/projects/microb2010/sensorboard/beacon.h b/projects/microb2010/sensorboard/beacon.h deleted file mode 100755 index 21d7dd5..0000000 --- a/projects/microb2010/sensorboard/beacon.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright Droids Corporation (2008) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: beacon.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $ - * - */ - -struct beacon { - int32_t beacon_speed; - - int32_t opponent_angle; - int32_t opponent_dist; - int32_t prev_opponent_angle; - int32_t prev_opponent_dist; - int32_t robot_x; - int32_t robot_y; - int32_t robot_angle; - int32_t opponent_x; - int32_t opponent_y; -}; - -/* real encoder value: 3531.75 so, multiple by 4 to have round - * value */ -#define BEACON_STEP_TOUR (14127L) -#define BEACON_OFFSET_CALIBRE 40 - -extern struct beacon beacon; - -void beacon_dump(void); -void beacon_init(void); -void beacon_calibre_pos(void); -void beacon_start(void); -void beacon_stop(void); -void beacon_calc(void *dummy); -void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y); - -void beacon_reset_pos(void); -void beacon_set_consign(int32_t val); - -int32_t encoders_spi_get_beacon_speed(void *dummy); -int32_t encoders_spi_update_beacon_speed(void *number); -int32_t encoders_spi_get_value_beacon(void *number); - - diff --git a/projects/microb2010/sensorboard/commands_scan.c b/projects/microb2010/sensorboard/commands_scan.c deleted file mode 100644 index e252298..0000000 --- a/projects/microb2010/sensorboard/commands_scan.c +++ /dev/null @@ -1,401 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: commands_scan.c,v 1.1 2009-05-27 20:04:07 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include "main.h" -#include "cmdline.h" -#include "../common/i2c_commands.h" -#include "i2c_protocol.h" -#include "actuator.h" - -#include -#include -#include - -#include "img_processing.h" -#include "scanner.h" - - - -/**********************************************************/ -/* sample ADC */ -/* -extern uint16_t sample_i; -extern float scan_offset_a; -extern float scan_offset_b; -*/ -extern struct scan_params scan_params; - -//extern uint16_t sample_tab[MAX_SAMPLE]; -/* this structure is filled when cmd_sample is parsed successfully */ -struct cmd_sample_result { - fixed_string_t arg0; - fixed_string_t arg1; - uint16_t offset_b; - float offset_a; - uint16_t dump_speed; - uint8_t filter; -}; - -extern int32_t pos_start_scan; -/* function called when cmd_sample is parsed successfully */ - -#define MAX_OBJECTS 10 -Object_bb sac_obj[MAX_OBJECTS]; -static void cmd_sample_parsed(void * parsed_result, void * data) -{ - struct cmd_sample_result * res = parsed_result; - uint16_t i; - - printf_P(PSTR("cmd sample called\r\n")); - printf_P(PSTR("arg %s %d\r\n"), res->arg1, res->offset_b); - - quadramp_set_1st_order_vars(&sensorboard.scanner.qr, res->dump_speed, res->dump_speed); /* set speed */ - - - scan_params.offset_b = (((float)res->offset_b)*M_PI/180.); - scan_params.offset_a = (((float)res->offset_a)*M_PI/180.); - scan_params.filter = res->filter; - - if (!strcmp_P(res->arg1, PSTR("start"))) { - scan_params.sample_i = MAX_SAMPLE; - scan_params.pos_start_scan = encoders_spi_get_value_scanner(SCANNER_ENC); - //printf_P(PSTR("start scan at pos %ld\r\n"), scan_params.pos_start_scan); - - memset(scan_params.sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t)); - - - cs_set_consign(&sensorboard.scanner.cs, scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L); - //printf_P(PSTR("scan dst %ld\r\n"), scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L); - - scan_params.last_col_n = 0; - scan_params.last_row_n = 0; - scan_params.last_sample = 0; - - } - else if (!strcmp_P(res->arg1, PSTR("dump"))) { - printf_P(PSTR("start object detection\r\n")); - for (i=0;ispeed; - scan_params.debug = res->debug; - -} - -prog_char str_scan_params_arg0[] = "scan_params"; -parse_pgm_token_string_t cmd_scan_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_params_result, arg0, str_scan_params_arg0); -parse_pgm_token_num_t cmd_scan_params_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, speed, INT16); -parse_pgm_token_num_t cmd_scan_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, debug, UINT8); - -prog_char help_scan_params[] = "Set scanner params (speed, debug)"; -parse_pgm_inst_t cmd_scan_params = { - .f = cmd_scan_params_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_sample, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_params_arg0, - (prog_void *)&cmd_scan_params_arg1, - (prog_void *)&cmd_scan_params_arg2, - NULL, - }, -}; - - - -/**********************************************************/ -/* set scanner calibration */ - -/* this structure is filled when cmd_scan_calibre is parsed successfully */ -struct cmd_scan_calibre_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_scan_calibre is parsed successfully */ -static void cmd_scan_calibre_parsed(void * parsed_result, void * data) -{ - struct cmd_scan_calibre_result * res = parsed_result; - - printf_P(PSTR("Starting scanner autocalibration\r\n")); - - - if (!strcmp_P(res->arg1, PSTR("mirror"))) { - scanner_calibre_mirror(); - } - else{ - scanner_calibre_laser(); - } -} - -prog_char str_scan_calibre_arg0[] = "scan_calibre"; - -prog_char str_scan_calibre_what_arg1[] = "mirror#laser"; -parse_pgm_token_string_t cmd_scan_calibre_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg0, str_scan_calibre_arg0); -parse_pgm_token_string_t cmd_scan_calibre_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg1, str_scan_calibre_what_arg1); - - -prog_char help_scan_calibre[] = "Scanner auto calibration (mirror|laser)"; -parse_pgm_inst_t cmd_scan_calibre = { - .f = cmd_scan_calibre_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_calibre, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_calibre_arg0, - (prog_void *)&cmd_scan_calibre_arg1, - NULL, - }, -}; - - - -/**********************************************************/ -/* start scan */ - -/* this structure is filled when cmd_scan_do is parsed successfully */ -struct cmd_scan_do_result { - fixed_string_t arg0; -}; - -/* function called when cmd_scan_do is parsed successfully */ -static void cmd_scan_do_parsed(void * parsed_result, void * data) -{ - printf_P(PSTR("Starting scan\r\n")); - scanner_scan_autonomous(); -} - -prog_char str_scan_do_arg0[] = "scan_do"; -parse_pgm_token_string_t cmd_scan_do_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_do_result, arg0, str_scan_do_arg0); - -prog_char help_scan_do[] = "Scan zone"; -parse_pgm_inst_t cmd_scan_do = { - .f = cmd_scan_do_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_do, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_do_arg0, - NULL, - }, -}; - - - - -/**********************************************************/ -/* set scanner img params */ - -/* this structure is filled when cmd_scan_img is parsed successfully */ -struct cmd_scan_img_result { - fixed_string_t arg0; - uint8_t algo; - uint8_t h; - int16_t x; - int16_t y; - -}; - -/* function called when cmd_scan_img is parsed successfully */ -static void cmd_scan_img_parsed(void * parsed_result, void * data) -{ - struct cmd_scan_img_result * res = parsed_result; - - scan_params.algo = res->algo; - if (res->algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE) { - scan_params.drop_zone.working_zone = res->h; - scan_params.drop_zone.center_x = res->x; - scan_params.drop_zone.center_y = res->y; - } else if (res->algo == I2C_SCANNER_ALGO_CHECK_TEMPLE) { - scan_params.check_temple.level = res->h; - scan_params.check_temple.temple_x = res->x; - scan_params.check_temple.temple_y = res->y; - } else if (res->algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE) { - scan_params.drop_zone.working_zone = res->h; - scan_params.drop_zone.center_x = res->x; - scan_params.drop_zone.center_y = res->y; - } - - - -} - -prog_char str_scan_img_arg0[] = "scan_img"; -parse_pgm_token_string_t cmd_scan_img_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_img_result, arg0, str_scan_img_arg0); -parse_pgm_token_num_t cmd_scan_img_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, algo, UINT8); -parse_pgm_token_num_t cmd_scan_img_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, h, UINT8); -parse_pgm_token_num_t cmd_scan_img_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, x, INT16); -parse_pgm_token_num_t cmd_scan_img_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, y, INT16); - - -prog_char help_scan_img[] = "Set scanner img processing params (algo, H, x, y)"; -parse_pgm_inst_t cmd_scan_img = { - .f = cmd_scan_img_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_img, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_img_arg0, - (prog_void *)&cmd_scan_img_arg1, - (prog_void *)&cmd_scan_img_arg2, - (prog_void *)&cmd_scan_img_arg3, - (prog_void *)&cmd_scan_img_arg4, - NULL, - }, -}; - - diff --git a/projects/microb2010/sensorboard/cs.c b/projects/microb2010/sensorboard/cs.c deleted file mode 100644 index 010b49e..0000000 --- a/projects/microb2010/sensorboard/cs.c +++ /dev/null @@ -1,145 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: cs.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $ - * - */ - -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "main.h" -#include "beacon.h" -#include "scanner.h" -#include "actuator.h" - -/* called every 5 ms */ -static void do_cs(void *dummy) -{ - /* read encoders */ - if (sensorboard.flags & DO_ENCODERS) { - encoders_spi_manage(NULL); - } - /* control system */ - if (sensorboard.flags & DO_CS) { - if (sensorboard.beacon.on) - cs_manage(&sensorboard.beacon.cs); - if (sensorboard.scanner.on) - cs_manage(&sensorboard.scanner.cs); - } - if (sensorboard.flags & DO_BD) { - bd_manage_from_cs(&sensorboard.beacon.bd, &sensorboard.beacon.cs); - bd_manage_from_cs(&sensorboard.scanner.bd, &sensorboard.scanner.cs); - } - if (sensorboard.flags & DO_POWER) - BRAKE_OFF(); - else - BRAKE_ON(); -} - -void dump_cs(const char *name, struct cs *cs) -{ - printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld\r\n"), - name, cs_get_consign(cs), cs_get_filtered_consign(cs), - cs_get_error(cs), cs_get_filtered_feedback(cs), - cs_get_out(cs)); -} - -void dump_pid(const char *name, struct pid_filter *pid) -{ - printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), - name, - pid_get_value_in(pid) * pid_get_gain_P(pid), - pid_get_value_I(pid) * pid_get_gain_I(pid), - pid_get_value_D(pid) * pid_get_gain_D(pid), - pid_get_value_out(pid)); -} - -void microb_cs_init(void) -{ - /* ---- CS beacon */ - /* PID */ - pid_init(&sensorboard.beacon.pid); - pid_set_gains(&sensorboard.beacon.pid, 80, 80, 250); - pid_set_maximums(&sensorboard.beacon.pid, 0, 10000, 2000); - pid_set_out_shift(&sensorboard.beacon.pid, 6); - pid_set_derivate_filter(&sensorboard.beacon.pid, 6); - - /* CS */ - cs_init(&sensorboard.beacon.cs); - cs_set_correct_filter(&sensorboard.beacon.cs, pid_do_filter, &sensorboard.beacon.pid); - cs_set_process_in(&sensorboard.beacon.cs, pwm_ng_set, BEACON_PWM); - cs_set_process_out(&sensorboard.beacon.cs, encoders_spi_update_beacon_speed, BEACON_ENCODER); - cs_set_consign(&sensorboard.beacon.cs, 0); - - /* ---- CS scanner */ - /* PID */ - pid_init(&sensorboard.scanner.pid); - pid_set_gains(&sensorboard.scanner.pid, 200, 5, 250); - pid_set_maximums(&sensorboard.scanner.pid, 0, 10000, 2047); - pid_set_out_shift(&sensorboard.scanner.pid, 6); - pid_set_derivate_filter(&sensorboard.scanner.pid, 6); - - /* QUADRAMP */ - quadramp_init(&sensorboard.scanner.qr); - quadramp_set_1st_order_vars(&sensorboard.scanner.qr, 200, 200); /* set speed */ - quadramp_set_2nd_order_vars(&sensorboard.scanner.qr, 20, 20); /* set accel */ - - /* CS */ - cs_init(&sensorboard.scanner.cs); - cs_set_consign_filter(&sensorboard.scanner.cs, quadramp_do_filter, &sensorboard.scanner.qr); - cs_set_correct_filter(&sensorboard.scanner.cs, pid_do_filter, &sensorboard.scanner.pid); - cs_set_process_in(&sensorboard.scanner.cs, pwm_ng_set, SCANNER_PWM); - cs_set_process_out(&sensorboard.scanner.cs, encoders_spi_update_scanner, SCANNER_ENCODER); - cs_set_consign(&sensorboard.scanner.cs, 0); - - /* Blocking detection */ - bd_init(&sensorboard.scanner.bd); - bd_set_speed_threshold(&sensorboard.scanner.bd, 150); - bd_set_current_thresholds(&sensorboard.scanner.bd, 500, 8000, 1000000, 40); - - /* set them on !! */ - sensorboard.beacon.on = 0; - sensorboard.scanner.on = 1; - - - scheduler_add_periodical_event_priority(do_cs, NULL, - 5000L / SCHEDULER_UNIT, - CS_PRIO); -} diff --git a/projects/microb2010/sensorboard/gen_scan_tab.c b/projects/microb2010/sensorboard/gen_scan_tab.c deleted file mode 100644 index 7b94bfd..0000000 --- a/projects/microb2010/sensorboard/gen_scan_tab.c +++ /dev/null @@ -1,680 +0,0 @@ -#include -#include -#include -#include - -#include - - -#include "scanner.h" - -#define ADC_REF_AVCC 0 -#define MUX_ADC13 0 - - - -double TELEMETRE_A = TELEMETRE_A_INIT; -double TELEMETRE_B = TELEMETRE_B_INIT; - - -#define printf_P printf -#define PSTR(a) (a) -int32_t motor_angle = 0; -int32_t scan_dist = 0; - - -int32_t H_fin, L_fin; -double L, H; - -#define nop() - -#define ABS(val) ({ \ - __typeof(val) __val = (val); \ - if (__val < 0) \ - __val = - __val; \ - __val; \ - }) - -struct scan_params scan_params; - -int32_t encoders_spi_get_value_scanner_interpolation(void *a) -{ - return motor_angle; -} - - -int16_t adc_get_value(uint8_t a) -{ - return scan_dist; -} - - - -/* get motor angle in radian; return mirror angle in radian, cos a sin a */ -void ang2_a_mirror(double b, double * c_a, double* s_a, double* a) -{ - double x2, y2; - double A, DELTA, B, D; - - b+=scan_params.offset_b; - x2 = X + l1*cos(b); - y2 = Y + l1*sin(b); - - A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3); - - DELTA = -(A*A - x2*x2 - y2*y2); - B = sqrt(DELTA); - - D = x2*x2 + y2*y2; - - *c_a = (x2*A + y2*B)/D; - *s_a = -(x2*B - y2*A)/D; - - *a = atan2(*s_a, *c_a); - - *a += scan_params.offset_a; - // *s_a = sin(*a); - // *c_a = cos(*a); - -} - -/* get telemeter dist , cos a, sin a, a and return H, L of scanned point */ -void ang2_H_L(double l_telemetre, double c_a, double s_a, double a, double *H, double *L) -{ - double d; - d = h_mirror*c_a/s_a; - *H = (l_telemetre - l_mirror - d)*sin(2*a); - *L = l_mirror + d + *H/tan(2*a); - - //*H+= 8*sin(a-scan_params.offset_a); -} - - - -//int32_t last_col_n; -//int32_t last_row_n; -//uint8_t last_sample; - -//uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123}; -//uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123}; - -/* last high is dummy, to deal higher columns */ -//uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250}; - - -//uint8_t h_limit[] = {60, 72, 85, 98, 112, 129, 250}; - -//uint8_t h_limit[] = {80, 97, 118, 134, 145, 160, 250}; - - -//uint8_t h_limit[] = {79, 94, 108, 117, 129, 144, 250}; - -uint8_t h_limit[] = {83, 95, 108, 120, 135, 147, 164, 250}; -#define H_MARGIN (-6) - -#define cs_set_consign(a, b) - - - -void do_scan(void * dummy) -{ - - unsigned int i; - int16_t a; - int32_t row_n; - int32_t col_n; - - - int32_t tour_pos; - int32_t pos, last_pos; - int32_t pos_tmp ; - int32_t mot_pos; - double dist; - uint8_t min_sample; - - double b, c_a, s_a, /*H, L,*/ m_a; - // int32_t H_fin; - - - //printf("scan\n"); - if (scan_params.sample_i==0) - return; - - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); -#if 0 - if (scan_params.sample_i==1){ - printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN); - //scanner.flags &= (~CS_ON); - - - - /* stop scan at cur pos + 10 round */ - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); - mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ; - - printf_P(PSTR("set to %ld \r\n"), mot_pos); - - cs_set_consign(&sensorboard.scanner.cs, mot_pos); - //pwm_ng_set(SCANNER_MOT_PWM, 0); - - - } - - mot_pos-= scan_params.pos_start_scan; -#endif - a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 ); - - - //dist = (a-TELEMETRE_B)/TELEMETRE_A; - //dist = TELEMETRE_A * a +TELEMETRE_B; - dist = telemetre_to_cm(a); - - //printf_P(PSTR("enc val = %ld\r\n"), encoders_microb_get_value((void *)SCANNER_ENC)); - - - //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a; - //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0; - - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); -#if 0 - /* separe scan forward/backword */ - if (row_n%2){ - row_n/=2; - } - else{ - row_n/=2; - row_n+=30; - } -#endif - - tour_pos = (mot_pos)%(SCANNER_STEP_TOUR); - - b = (2.*M_PI)*(double)tour_pos/(double)(SCANNER_STEP_TOUR); - - ang2_a_mirror(b, &c_a, &s_a, &m_a); - ang2_H_L(dist, c_a, s_a, m_a, &H, &L); - - - if (H >0){ - printf("zarb H\n"); - H = 0; - } - - if (dist> SCAN_MAX_DIST){ - H = 0; - L = 0; - } - - H = H;//m_a*180/M_PI; - L = L;//(L-100)*PIX_PER_SCAN; - - //printf_P(PSTR("polling : ADC0 = %i %f\r\n"),a, dist); - //printf_P(PSTR("%f %f %2.2f %f\r\n"), H, L, m_a*180./M_PI, dist); - - - //printf_P(PSTR("dist, b, a: %2.2f %2.2f %2.2f\r\n"), dist, b*180/M_PI, m_a*180/M_PI); - - H=(H+SCAN_H_MAX)*SCAN_H_COEF; - L-=SCAN_L_MIN; - - - /* XXX may never append */ - if (L<0) - L=0; - - - /* first filter => pixel modulo level */ -#define H_BASE 10 -#define H_MUL 14 - H_fin = H;//+SCAN_H_MAX; - //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE; - - if (scan_params.filter){ - H_fin = 11; // default is level 11 - for (i=0;iPIX_PER_SCAN) - printf("BUG!!! RECALC MAX L\r\n"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos =DIM_DIST) - j = DIM_DIST-1; - - if (i>=DIM_ANGLE) - i = DIM_ANGLE-1; - - - val.u16 = pgm_read_word_near(&array_h_l[j][i]); - - - //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - //val.u16 = pgm_read_word_near(&array_h_l[a][tp]); - H = val.h_l.h; - L = val.h_l.l; - /* - val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - - H = val.h_l.h; - L = val.h_l.l; - */ - H_fin = H; - L_fin = L; - - - - col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN); - if (col_n>PIX_PER_SCAN) - printf("BUG!!! RECALC MAX L\r\n"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos 1 && !strcmp(argv[1], "1")) - scan_params.filter = 1; - - - printf("max i max j %d %d \n", i, j); - dist_max = j; - angle_max = i; - - f_header = fopen("scan_h_l.h", "w"); - fprintf(f_header, "PROGMEM lookup_h_l array_h_l[%d][%d] = {\n", dist_max, angle_max); - for (j = 0, scan_dist = 0; j - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "actuator.h" -#include "beacon.h" -#include "scanner.h" - -void i2c_protocol_init(void) -{ -} - -/*** LED CONTROL ***/ -void i2c_led_control(uint8_t l, uint8_t state) -{ - switch(l) { - case 1: - state? LED1_ON():LED1_OFF(); - break; - case 2: - state? LED2_ON():LED2_OFF(); - break; - default: - break; - } -} - -void i2c_send_status(void) -{ - struct i2c_ans_sensorboard_status ans; - i2c_flush(); - ans.hdr.cmd = I2C_ANS_SENSORBOARD_STATUS; - ans.status = 0x55; /* XXX */ - ans.opponent_x = beacon.opponent_x; - ans.opponent_y = beacon.opponent_y; - ans.opponent_a = beacon.opponent_angle; - ans.opponent_d = beacon.opponent_dist; - - ans.scan_status = 0; - ans.scan_status |= scan_params.working ? 0 : I2C_SCAN_DONE; - ans.scan_status |= scan_params.max_column_detected ? I2C_SCAN_MAX_COLUMN : 0; - - - ans.dropzone_x = scan_params.dropzone_x; - ans.dropzone_y = scan_params.dropzone_y; - ans.dropzone_h = scan_params.dropzone_h; - - i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, - sizeof(ans), I2C_CTRL_GENERIC); -} - - -void i2c_scanner_calibre_laser(void* dummy) -{ - scanner_calibre_laser(); -} - -void i2c_scanner_end_process(void* dummy) -{ - scanner_end_process(); -} - -void i2c_recvevent(uint8_t * buf, int8_t size) -{ - void *void_cmd = buf; - static uint8_t a=0; - a=!a; - if (a) - LED2_ON(); - else - LED2_OFF(); - - if (size <= 0) { - goto error; - } - - switch (buf[0]) { - - /* Commands (no answer needed) */ - case I2C_CMD_GENERIC_LED_CONTROL: - { - struct i2c_cmd_led_control *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - i2c_led_control(cmd->led_num, cmd->state); - break; - } - - case I2C_CMD_GENERIC_SET_COLOR: - { - struct i2c_cmd_generic_color *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - sensorboard.our_color = cmd->color; - break; - } - - case I2C_CMD_SENSORBOARD_SET_BEACON: - { - struct i2c_cmd_sensorboard_start_beacon *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - if (cmd->enable) - beacon_start(); - else - beacon_stop(); - - break; - } - - case I2C_CMD_SENSORBOARD_SET_SCANNER: - { - struct i2c_cmd_sensorboard_scanner *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - scanner_set_mode(cmd->mode); - break; - - } - - - case I2C_CMD_SENSORBOARD_SCANNER_ALGO: - { - struct i2c_cmd_sensorboard_scanner_algo *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - scan_params.algo = cmd->algo; - - if (cmd->algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE){ - scan_params.drop_zone.working_zone = cmd->drop_zone.working_zone; - scan_params.drop_zone.center_x = cmd->drop_zone.center_x; - scan_params.drop_zone.center_y = cmd->drop_zone.center_y; - } - else if (cmd->algo == I2C_SCANNER_ALGO_CHECK_TEMPLE) { - scan_params.check_temple.level = cmd->check_temple.level; - scan_params.check_temple.temple_x = cmd->check_temple.temple_x; - scan_params.check_temple.temple_y = cmd->check_temple.temple_y; - } - else if (cmd->algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE){ - scan_params.drop_zone.working_zone = cmd->drop_zone.working_zone; - scan_params.drop_zone.center_x = cmd->drop_zone.center_x; - scan_params.drop_zone.center_y = cmd->drop_zone.center_y; - } - else{ - /* new command */ - } - - scan_params.working = 1; - scheduler_add_single_event_priority(i2c_scanner_end_process, NULL, - 1, - CS_PRIO-1); - break; - - } - - case I2C_CMD_SENSORBOARD_CALIB_SCANNER: - { - struct i2c_cmd_sensorboard_calib_scanner *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - scheduler_add_single_event_priority(i2c_scanner_calibre_laser, NULL, - 1, - CS_PRIO-1); - break; - } - - - - /* Add other commands here ...*/ - - - case I2C_REQ_SENSORBOARD_STATUS: - { - struct i2c_req_sensorboard_status *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - beacon.robot_x = cmd->x; - beacon.robot_y = cmd->y; - beacon.robot_angle = cmd->a; - - if (cmd->enable_pickup_wheels) - pickup_wheels_on(); - else - pickup_wheels_off(); - - i2c_send_status(); - break; - } - - default: - goto error; - } - - error: - /* log error on a led ? */ - return; -} - -void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c) -{ -} - -void i2c_sendevent(int8_t size) -{ -} - - diff --git a/projects/microb2010/sensorboard/img_processing.c b/projects/microb2010/sensorboard/img_processing.c deleted file mode 100644 index d030478..0000000 --- a/projects/microb2010/sensorboard/img_processing.c +++ /dev/null @@ -1,1619 +0,0 @@ -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include - -#include "img_processing.h" - - -#define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) - -#ifndef HOST_VERSION - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "main.h" - -#define IMG_DEBUG(args...) DEBUG(E_USER_IMGPROCESS, args) -#define IMG_NOTICE(args...) NOTICE(E_USER_IMGPROCESS, args) -#define IMG_ERROR(args...) ERROR(E_USER_IMGPROCESS, args) - -#else - -#define IMG_DEBUG(args...) debug_printf(args) -#define IMG_NOTICE(args...) debug_printf(args) -#define IMG_ERROR(args...) debug_printf(args) - -#endif - - - - -#define OBJECT_MINIMUM_DEMI_PERIMETER (2*3) -/* store object in pool if never seen - * returns: - * 1 if new object - * 0 if already known - */ -int store_obj(Object_bb* tab_o, int total, Object_bb*o) -{ - uint8_t i; - - if (o->x_max - o->x_min + o->y_max - o->y_min < OBJECT_MINIMUM_DEMI_PERIMETER) - return 0; - - for (i=0;ix_max = 0; - o->y_max = 0; - o->x_min = x_in; - o->y_min = y_in; - - - while(1){ - if (pos_x == start_x && pos_y == start_y){ - count_stand++; - if (count_stand>4) - break; - if( v.x == vi.x && v.y == vi.y && start) - break; - } - - if (pos_xx_min) - o->x_min = pos_x; - if (pos_yy_min) - o->y_min = pos_y; - if (pos_x>o->x_max) - o->x_max = pos_x; - if (pos_y>o->y_max) - o->y_max = pos_y; - - /* is next pixel is good color */ - if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ - pos_x = pos_x+v.x; - pos_y = pos_y+v.y; - len++; - vect_rot_retro(&v); - start = 1; - continue; - } - vect_rot_trigo(&v); - } - - o->len = len; -} - - - - - -/* step around object of given color and computes its polygon */ -void object_find_poly(unsigned char* data, - int16_t x_in, int16_t y_in, - int16_t start_x, int16_t start_y, - int16_t color, int16_t color_w, Object_poly* o) -{ - int16_t pos_x = start_x; - int16_t pos_y = start_y; - int16_t start =0; - int16_t len = 0; - int16_t count_stand = 0; - uint16_t pt_step, pt_num; - vect_t v, vi; - - v.x = 1; - v.y = 0; - - vi = v; - - pt_step = o->len/POLY_MAX_PTS + 1; - - pt_num = 0; - - while(1){ - if (pos_x == start_x && pos_y == start_y){ - count_stand++; - if (count_stand>4) - break; - if( v.x == vi.x && v.y == vi.y && start) - break; - } - - /* is next pixel is good color */ - if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ - pos_x = pos_x+v.x; - pos_y = pos_y+v.y; - len++; - - if (len >pt_num*pt_step){ - o->pts[pt_num].x = pos_x; - o->pts[pt_num].y = pos_y; - pt_num+=1; - if (pt_num>=POLY_MAX_PTS) - break; - } - - vect_rot_retro(&v); - start = 1; - continue; - } - vect_rot_trigo(&v); - } - - o->pts_num = pt_num; - -} - -#define PT_LEFT 0 -#define PT_RIGHT 2 -#define PT_TOP 1 -#define PT_DOWN 3 - -/* return most left/right/top/down pts indexes of given polygon */ -void object_find_extrem_points_index(Object_poly* o, - unsigned int *pts) -{ - unsigned int i; - for (i=0;i<4;i++) - pts[i] = 0; - - - for (i=1;ipts_num;i++){ - if (o->pts[i].x < o->pts[pts[PT_LEFT]].x) - pts[PT_LEFT] = i; - if (o->pts[i].x > o->pts[pts[PT_RIGHT]].x) - pts[PT_RIGHT] = i; - if (o->pts[i].y < o->pts[pts[PT_TOP]].y) - pts[PT_TOP] = i; - if (o->pts[i].y > o->pts[pts[PT_DOWN]].y) - pts[PT_DOWN] = i; - } - -} - - -#define NUM_CALIPERS 4 -/* for debug purpose: display a vector on image */ -void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in, - vect_t *v, point_t p) -{ - unsigned int i; - float n; - int16_t x, y; - float coef=1.0; - - if (!v->x && !v->y) - return; - - n = vect_norm(v); - coef = 1/n; - for (i=0;i<5;i++){ - x = p.x; - y = p.y; - - x+=(float)((v->x)*(float)i*(float)coef); - y+=(float)((v->y)*(float)i*(float)coef); - if ((x== p.x) && (y == p.y)) - buf[y*x_in+x] = 0x0; - else - buf[y*x_in+x] = 0x0; - } - -} - -#define CAL_X 3 -#define CAL_Y 0 - - -/* compute minimum rectangle area including the given convex polygon */ -void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out, - vect_t *v_out, vect_t *r1, vect_t*r2) -{ - vect_t calipers[NUM_CALIPERS]; - vect_t edges[NUM_CALIPERS]; - - unsigned int i; - unsigned int calipers_pts_index[NUM_CALIPERS]; - float angles[NUM_CALIPERS]; - float min_angle; - float total_rot_angle = 0; - int caliper_result_index; - - vect_t res1, res2; - float ps, n1, n2, caliper_n; - - float aera_tmp; - /* XXX hack sould be max*/ - float aera_min=0x100000; - - object_find_extrem_points_index(o, calipers_pts_index); - - calipers[0].x = 0; - calipers[0].y = 1; - - calipers[1].x = -1; - calipers[1].y = 0; - - calipers[2].x = 0; - calipers[2].y = -1; - - calipers[3].x = 1; - calipers[3].y = 0; - - - while (total_rot_angle <= M_PI/2){ - - for (i=0;ipts[(calipers_pts_index[i] + 1)%o->pts_num].x - - o->pts[calipers_pts_index[i]].x; - edges[i].y = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].y - - o->pts[calipers_pts_index[i]].y; - - /* compute angle between caliper and polygon edge */ - angles[i] = vect_get_angle(&edges[i], &calipers[i]); - } - - /* find min angle */ - min_angle = angles[0]; - caliper_result_index = 0; - for (i=1;ipts_num; i++){ - calipers[(i+1) % NUM_CALIPERS] = calipers[i % NUM_CALIPERS]; - vect_rot_trigo(&calipers[(i+1) % NUM_CALIPERS]); - } - - /* update calipers point */ - for (i=0;ipts_num; - } - - res1.x = o->pts[calipers_pts_index[2]].x - o->pts[calipers_pts_index[0]].x; - res1.y = o->pts[calipers_pts_index[2]].y - o->pts[calipers_pts_index[0]].y; - - res2.x = o->pts[calipers_pts_index[3]].x - o->pts[calipers_pts_index[1]].x; - res2.y = o->pts[calipers_pts_index[3]].y - o->pts[calipers_pts_index[1]].y; - - ps = vect_pscal(&res1, &calipers[CAL_X]); - n1 = vect_norm(&res1); - caliper_n = vect_norm(&calipers[CAL_X]); - caliper_n*=caliper_n; - - res1 = calipers[CAL_X]; - - res1.x *= ps/(caliper_n); - res1.y *= ps/(caliper_n); - - - ps = vect_pscal(&res2, &calipers[CAL_Y]); - n1 = vect_norm(&res2); - - res2 = calipers[CAL_Y]; - - res2.x *= ps/(caliper_n); - res2.y *= ps/(caliper_n); - - n1 = vect_norm(&res1); - n2 = vect_norm(&res2); - - aera_tmp = n1*n2; - - if (aera_min >aera_tmp){ - aera_min = aera_tmp; - for (i=0;ipts[pts_index_out[i]].x; - p1.y = o->pts[pts_index_out[i]].y; - - p2.x = p1.x+COEF_CALIP*caliper_tmp.x; - p2.y = p1.y+COEF_CALIP*caliper_tmp.y; - - pts2line(&p1, &p2, &l1); - - vect_rot_trigo(&caliper_tmp); - - p1.x = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].x; - p1.y = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].y; - - p2.x = p1.x+COEF_CALIP*caliper_tmp.x; - p2.y = p1.y+COEF_CALIP*caliper_tmp.y; - - pts2line(&p1, &p2, &l2); - - ret = intersect_line(&l1, &l2, &p_int1); - if (ret!=1) - return 0; - //IMG_DEBUG("int1 (%d): %" PRIi32 " %" PRIi32 " ", ret, p_int1.x, p_int1.y); - - mp_x+=p_int1.x; - mp_y+=p_int1.y; - - } - - - - p->x = lround(mp_x/NUM_CALIPERS); - p->y = lround(mp_y/NUM_CALIPERS); - - - return 1; - -} - -#define OBJECT_DIM 5 -/* split zone in many column's sized area */ -int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color) -{ - int n1, n2; - int i, j; - int index=0; - int r1_s, r2_s; - point_t ptmp; - - - n1 = vect_norm(r1); - n2 = vect_norm(r2); - - r1_s = n1/OBJECT_DIM; - r2_s = n2/OBJECT_DIM; - - if (!r1_s || ! r2_s) - return 0; - - ptmp.x = p->x - r1->x/2 - r2->x/2 + (r1->x/(r1_s*2))+(r2->x/(r2_s*2)); - ptmp.y = p->y - r1->y/2 - r2->y/2 + (r1->y/(r1_s*2))+(r2->y/(r2_s*2)); - - for(i=0;ix)/r1_s+(j*r2->x)/r2_s; - zones[index].p.y = ptmp.y + (i*r1->y)/r1_s+(j*r2->y)/r2_s; - zones[index].h = color; - zones[index].valid = 1; - - index++; - if (index>=max_zone) - return index; - - - } - } - - return index; -} - -#define OBJECT_SEMI_DIM (OBJECT_DIM/2) -#define MIN_SURFACE_PERCENT 50 -#define HIGHER_MAX_PIXEL 5 - - -int zone_has_enought_pixels(unsigned char* data, int16_t x_in, int16_t y_in, zone* z) -{ - int x, y; - uint16_t count, total_pix, higher_pixels; - - count = 0; - total_pix=0; - higher_pixels = 0; - - for (x = -OBJECT_SEMI_DIM; - (x <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); - x++){ - for (y = -OBJECT_SEMI_DIM; - (y <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); - y++){ - total_pix++; - if (data[x_in * (y + z->p.y) + x + z->p.x] == z->h) - count++; - - if (data[x_in * (y + z->p.y) + x + z->p.x] > z->h) - higher_pixels++; - - } - - } - - IMG_DEBUG("has enougth pixel (h: %d x %"PRIi32": y:%"PRIi32") total: %d/%d (tt: %d, hmax: %d)", z->h, z->p.x, z->p.y, - count, (total_pix * MIN_SURFACE_PERCENT) / 100, total_pix, higher_pixels); - - if ((count > (total_pix * MIN_SURFACE_PERCENT) / 100) && - (higher_pixels (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin)) - continue; - - p[i].valid = 0; - - } - - return zones_num; -} - -#define MAX_DIST_TO_ZONE 2 - -unsigned int zone_filter_zone_rect(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y , uint8_t working_zone) -{ - int i; - - for (i = 0; i < zones_num; i++){ - - IMG_DEBUG("rct x:%"PRIi32" y:%"PRIi32" (centerx: %d)",p[i].p.x , p[i].p.y, center_x); - - if ((p[i].p.x > center_x - MAX_DIST_TO_ZONE) && (p[i].h > working_zone)) - continue; - - p[i].valid = 0; - } - - return zones_num; -} - - -/* delete point to render polygon convex */ -int object_poly_to_convex(Object_poly *o) -{ - unsigned int i, j; - vect_t v, w; - int16_t z; - unsigned int del_pts_num = 0; - - for (i=0;ipts_num;){ - v.x = o->pts[(i + o->pts_num - 1)%o->pts_num].x - o->pts[i].x; - v.y = o->pts[(i + o->pts_num - 1)%o->pts_num].y - o->pts[i].y; - - w.x = o->pts[(i+1)%o->pts_num].x - o->pts[i].x; - w.y = o->pts[(i+1)%o->pts_num].y - o->pts[i].y; - - z = vect_pvect(&v, &w); - if (z>0){ - i+=1; - continue; - } - - /* found a convex angle (or colinear points) */ - for (j = i; j < o->pts_num-1; j++){ - o->pts[j] = o->pts[j+1]; - } - if (i!=0) - i-=1; - o->pts_num--; - del_pts_num++; - } - - return del_pts_num; -} - - - - - -#define DEFAULT_COLOR 255 -/* scan all image and find objects*/ -unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in, - Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj) -{ - int16_t i, obj_num; - - uint8_t in_color=0; - int16_t etat; - - Object_bb o; - int ret; - - obj_num = 0; - /* - first, delete borders - */ - for (i=0;i= MAX_SISTER_PER_ZONE) - break; - - - if (!zones[i].valid) - continue; - - - if (i == z_n) - continue; - - /* twin tower must have same high */ - if (zones[i].h != zones[z_n].h) - continue; - - IMG_DEBUG("test sisters (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - zones[z_n].p.x, zones[z_n].p.y, - zones[i].p.x, zones[i].p.y); - - - dist = sqrt( (zones[i].p.x - zones[z_n].p.x) * (zones[i].p.x - zones[z_n].p.x) + - (zones[i].p.y - zones[z_n].p.y) * (zones[i].p.y - zones[z_n].p.y) ); - - - IMG_DEBUG("sister space is %2.2f may be near %d", dist, SPACE_INTER_TWIN_TOWER); - - /* - twin tower must be close/far enought to drop lintel - */ - if (ABS(dist - SPACE_INTER_TWIN_TOWER) > SPACE_INTER_TWIN_TOWER_TOLERENCE) - continue; - - - pts2line(&zones[i].p, &zones[z_n].p, &l); - - /* - test the paralelism of the temple: - zone may be on same distance from center - */ - - - v1.x = zones[z_n].p.x - center_x; - v1.y = zones[z_n].p.y - center_y; - - dist = vect_norm(&v1); - - v2.x = zones[i].p.x - center_x; - v2.y = zones[i].p.y - center_y; - - dist2 = vect_norm(&v2); - - IMG_DEBUG("zone dist %2.2f %2.2f", dist, dist2); - if (ABS(dist-dist2) > 3){ - IMG_DEBUG("bad parallelism %2.2f", ABS(dist-dist2)); - continue; - } - - - - - /* no other aligned tower to avoid dropping on a lintel - * (3 aligned zone may mean lintel) - */ - - good_zone = 1; - - for (j = 0; j < zones_num; j++){ - if (j==i ||j == z_n) - continue; - - /* if third zone, but lower */ - if (zones[j].h <= zones[i].h) - continue; - - /* - check distance from dropping zone to - line formed by twin towers - */ - - proj_pt_line(&zones[j].p, &l, &p); - - - /* test if projected point is in the segement */ - - - v.x = zones[z_n].p.x - zones[i].p.x; - v.y = zones[z_n].p.y - zones[i].p.y; - - v1.x = p.x - zones[i].p.x; - v1.y = p.y - zones[i].p.y; - - n1 = vect_pscal_sign(&v, &v1); - - v.x = -v.x; - v.y = -v.y; - - v1.x = p.x - zones[z_n].p.x; - v1.y = p.y - zones[z_n].p.y; - - - n2 =vect_pscal_sign(&v, &v1); - - v.x = p.x - zones[j].p.x; - v.y = p.y - zones[j].p.y; - - dist = vect_norm(&v); - IMG_DEBUG("dist pt h %d n: (%d %d) (%"PRIi32" %"PRIi32") to line %2.2f", zones[j].h, n1, n2, zones[j].p.x, zones[j].p.y, dist); - - - if ((n1>=0 && n2>=0) && dist < OBJECT_DIM+2.){ - good_zone = 0; - break; - } - - - /* test if zone is far from points*/ - - v1.x = zones[j].p.x - zones[z_n].p.x; - v1.y = zones[j].p.y - zones[z_n].p.y; - - dist = vect_norm(&v1); - IMG_DEBUG("dist pt to z1 %2.2f", dist); - - if (dist < OBJECT_DIM){ - good_zone = 0; - break; - } - - v2.x = zones[j].p.x - zones[i].p.x; - v2.y = zones[j].p.y - zones[i].p.y; - - - dist = vect_norm(&v2); - IMG_DEBUG("dist pt to z2 %2.2f", dist); - - if (dist < OBJECT_DIM){ - good_zone = 0; - break; - } - - - - z1 = i; - z2 = z_n; - z3 = j; - - - - - - /* - XXX may be a lintel on lintel !! - */ - - } - - if (!good_zone) - continue; - - IMG_DEBUG("sisters ok (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - zones[z_n].p.x, zones[z_n].p.y, - zones[i].p.x, zones[i].p.y); - - - sisters[z_n][current_sister] = i; - current_sister++; - } - } -} - -/* test if a higher zone is too close */ -int test_close_zone(uint8_t zones_num, zone* zones, unsigned int z_n) -{ - uint8_t i; - vect_t v; - double dist; - - for (i = 0; i < zones_num; i++){ - if (i == z_n) - continue; - if (zones[i].h <= zones[z_n].h) - continue; - - v.x = zones[i].p.x - zones[z_n].p.x; - v.y = zones[i].p.y - zones[z_n].p.y; - - dist = vect_norm(&v); - //IMG_DEBUG("dist pt to pt %2.2f", dist); - - if (dist < OBJECT_DIM){ - return 1; - } - - } - - return 0; -} - -#define MAX_COLUMN 4 -#define MAX_LINTEL 2 - -drop_column_zone drop_c[MAX_COLUMN]; -drop_lintel_zone drop_l[MAX_LINTEL]; - - -void reset_drop_zone(void) -{ - memset(drop_c, 0, sizeof(drop_c)); - memset(drop_l, 0, sizeof(drop_l)); - -} - - -void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones) -{ - unsigned int i; - - for (i=0;i(b)?(a):(b)) - - -#if 0 -#define MAX_DROP_HIGH 8 - - -/* - recursive function to maximize points during object - dropping, given lintel/column number - working zone may be 1, 2 or 3 - */ -unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max, - uint8_t n_columns, uint8_t n_lintels, - uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone) -{ - - uint8_t i, j; - unsigned int points_calc; - //unsigned int points_added = 0; - int sister; - int ret; - - - /* if no more objects, return points */ - if (n_columns == 0 && n_lintels == 0) - return MY_MAX(points, points_max); - - /* start by putting columns if so */ - for (i = 0; i < zones_num; i++){ - if (zones[i].h >= MAX_DROP_HIGH) - continue; - - if (n_columns){ - - ret = test_close_zone(zones_num, zones, i); - if (ret) - continue; - - zones[i].h++; - points_calc = solve_objects_dropping(points + zones[i].h, points_max, - n_columns - 1, n_lintels, - zones_num, zones, sisters, working_zone); - - if (points_calc > points_max){ - points_max = points_calc; - drop_c[n_columns - 1].z = i; - drop_c[n_columns - 1].h = zones[i].h; - drop_c[n_columns - 1].valid = 1; - - } - zones[i].h--; - } - /* we must depose all columns before dropping lintels */ - else if (n_lintels){ - - /* dont drop lintel on ground */ - if (zones[i].h <= working_zone) - continue; - - /* XXX todo need get second zone near selected one */ - //ret = find_twin_tower(zones_num, zones, i, &sister); - - for (j = 0; j < MAX_SISTER_PER_ZONE; j++){ - sister = sisters[i][j]; - if (sister == -1) - break; - if (zones[i].h != zones[sister].h){ - sister = -1; - } - - } - - if (sister == -1) - continue; - //IMG_DEBUG("sister found: %d %d (h=%d %p)", i, sister, zones[i].h, &zones[i].h); - - zones[i].h++; - zones[sister].h++; - - - points_calc = solve_objects_dropping(points + zones[i].h * 3, points_max, - n_columns, n_lintels - 1, - zones_num, zones, sisters, working_zone); - - if (points_calc > points_max){ - points_max = points_calc; - - drop_l[n_lintels - 1].z1 = i; - drop_l[n_lintels - 1].z2 = sister; - drop_l[n_lintels - 1].h = zones[i].h; - drop_l[n_lintels - 1].valid = 1; - - } - - - zones[sister].h--; - zones[i].h--; - } - } - - return MY_MAX(points, points_max); -} -#endif - - -/* */ -int find_column_dropzone(uint8_t zones_num, zone* zones) -{ - uint8_t i; - - uint8_t z_n = 0; - - if (zones_num <= 0) - return -1; - - for (i = 0; i < zones_num; i++){ - if (!zones[i].valid) - continue; - if (zones[i].h > zones[z_n].h) - z_n = i; - } - - - /* - now, chose dropzone closest to robot - meaning little x, big y - so maximise y-x - */ - for (i = 0; i < zones_num; i++){ - if (zones[i].h != zones[z_n].h) - continue; - if (!zones[i].valid) - continue; - if (zones[i].p.y - zones[i].p.x > zones[z_n].p.y - zones[z_n].p.x) - z_n = i; - } - - - - return z_n; -} - - - -uint8_t color2h(uint8_t color) -{ - return (0x100-color)/0x20; -} - -uint8_t h2color(uint8_t color) -{ - return color*0x20; -} - - -#define NUM_ZONE_GENERATE 8 -#define DIST_ZONE_GEN 9 - -/* - remove zone at ground level, and generate zones on - a circle at X cm from center - */ -unsigned int generate_center_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, - zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y) -{ - double c_a, s_a; - uint8_t i, j; - double px1, py1, px2, py2; - - - /* first del zone at level 2 */ - for (i = 0; i < zones_num; ){ - if (zones[i].h!=2){ - i++; - continue; - } - - for (j = i; j < zones_num-1; j++) - zones[j] = zones[j+1]; - - zones_num--; - - } - - /* generate zones around circle */ - - c_a = cos(2*M_PI/NUM_ZONE_GENERATE); - s_a = sin(2*M_PI/NUM_ZONE_GENERATE); - - px1 = DIST_ZONE_GEN; - py1 = 0; - - for (i = 0; i < NUM_ZONE_GENERATE; i++){ - - zones[zones_num].p.x = center_x + px1; - zones[zones_num].p.y = center_y + py1; - zones[zones_num].h = 2; - zones[zones_num].valid = 1; - - - px2 = px1*c_a + py1*s_a; - py2 = -px1*s_a + py1*c_a; - - px1 = px2; - py1 = py2; - - /* skip zone if it is not in img */ - if (zones[zones_num].p.x < 0 || zones[zones_num].p.y < 0 || - zones[zones_num].p.x >= x_in || zones[zones_num].p.y > y_in) - continue; - - /* skip zone if not enougth pixels */ - if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) - continue; - - zones_num++; - if (zones_num >= max_zones) - break; - - } - - return zones_num; - - -} - - -/* - remove zone at ground level, and generate zones on - a line at X cm from robot - */ -unsigned int generate_rectangle_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, - zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y, - uint8_t working_zone) -{ - uint8_t i, j; - uint8_t y; - - /* first del zone at level i */ - for (i = 0; i < zones_num; ){ - if (zones[i].h != working_zone ){ - i++; - continue; - } - - for (j = i; j < zones_num-1; j++) - zones[j] = zones[j+1]; - - zones_num--; - } - - - /* generate zones on a line */ - for (y = OBJECT_DIM; y < y_in; y+=OBJECT_DIM){ - - zones[zones_num].p.x = center_x; - zones[zones_num].p.y = y; - zones[zones_num].h = working_zone ; - zones[zones_num].valid = 1; - - if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) - continue; - zones_num++; - - if (zones_num >= max_zones) - break; - - } - return zones_num; - -} - - -#define MAX_DECAL_LINE 5 -#define ENOUGHT_ZONE_PIXEL 2 - -void recal_img_y(unsigned char* buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone) -{ - uint8_t i, j; - uint8_t cpt; - - /* recal img only for central zone */ - if (working_zone !=2) - return; - - for (i = 0; i < MAX_DECAL_LINE; i++){ - cpt = 0; - for (j = 0; j < x_in; j++){ - if (buffer[i*x_in + j] ==2) - cpt++; - } - - if (cpt >= ENOUGHT_ZONE_PIXEL) - break; - } - - memmove(buffer, &buffer[i * x_in], x_in * y_in - i*x_in); - memset(&buffer[x_in * y_in - i * x_in], 0, i*x_in); -} - - -#define MAX_OBJECTS 20 - -#define MAX_ZONES_PER_OBJECT 20 - - -uint8_t g_zone_num; -zone g_all_zones[MAX_ZONES]; - -uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in, - zone * all_zones, uint8_t max_zones) -{ - - int ret; - int i, j; - - zone zones[MAX_ZONES_PER_OBJECT]; - - vect_t caliper; - unsigned int pts_cal[4]; - point_t ptmp; - vect_t r1, r2; - int zone_len; - - - uint8_t zone_num = 0; - - Object_bb sac_obj[MAX_OBJECTS]; - Object_poly sac_obj_poly[MAX_OBJECTS]; - - - - /* - XXX fix: only decal for working zone 2/(1?) - but we dont have info yet - */ - recal_img_y(buffer, x_in, y_in, 2); - - memset(sac_obj, 0, sizeof(sac_obj)); - memset(sac_obj_poly, 0, sizeof(sac_obj_poly)); - - - /* first, find polygons*/ - parcour_img(buffer, x_in, y_in, sac_obj, sac_obj_poly, MAX_OBJECTS); - - /* enclose each poygon in the min area polygon - then, split each rectangle in dropping zone - */ - for (i=0;i 0; i--){ - z.h = i; - z.valid = 1; - ret = zone_has_enought_pixels(buffer, x_in, y_in, &z); - IMG_NOTICE("test z3:(h=%d) %d", i, ret); - - if (ret) - return 1; - } - - return 0; - -} - - -int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone, int16_t center_x, int16_t center_y, - int16_t * dropzone_x, int16_t * dropzone_y) -{ - uint8_t zone_num; - int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE]; - int8_t z_n = -1; - uint8_t i, j; - - - /* find all drop zone */ - zone_num = filter_zones(buffer, x_in, y_in, - g_all_zones, g_zone_num, MAX_ZONES, - working_zone, center_x, center_y, - -2); - - /* precompute possible twin towers */ - find_twin_tower(zone_num, g_all_zones, sisters, center_x, center_y); - - - for (i=0; i< zone_num; i++){ - IMG_DEBUG("all sisters: %d", i); - for (j=0;j g_all_zones[i].h) - continue; - - z_n = i; - } - - IMG_NOTICE("twin tower found :z_n=%d", z_n); - if (z_n == -1) - return -1; - - IMG_NOTICE("(%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - g_all_zones[z_n].p.x, g_all_zones[z_n].p.y, - g_all_zones[sisters[z_n][0]].p.x, g_all_zones[sisters[z_n][0]].p.y); - - - *dropzone_x = ((double)(g_all_zones[z_n].p.x + g_all_zones[sisters[z_n][0]].p.x)*PIXEL2CM ) / 2; - *dropzone_y = ((double)(g_all_zones[z_n].p.y + g_all_zones[sisters[z_n][0]].p.y)*PIXEL2CM ) / 2 + 30; - - - return g_all_zones[z_n].h; - -} - diff --git a/projects/microb2010/sensorboard/img_processing.h b/projects/microb2010/sensorboard/img_processing.h deleted file mode 100644 index 4c06237..0000000 --- a/projects/microb2010/sensorboard/img_processing.h +++ /dev/null @@ -1,111 +0,0 @@ - - - -typedef struct _Object_bb -{ - uint8_t x_min; - uint8_t x_max; - uint8_t y_min; - uint8_t y_max; - uint16_t len; -}Object_bb; - -#define POLY_MAX_PTS 12 -typedef struct _Object_poly -{ - uint16_t pixels_perim; - uint16_t pts_num; - point_t pts[POLY_MAX_PTS]; - uint16_t len; - uint8_t color; -}Object_poly; - - -typedef struct _zone -{ - point_t p; - uint8_t h:7; - uint8_t valid:1; -}zone; - - -typedef struct _drop_column_zone -{ - uint8_t valid; - uint8_t z; - uint8_t h; -}drop_column_zone; - -typedef struct _drop_lintel_zone -{ - uint8_t valid; - uint8_t z1; - uint8_t h; - uint8_t z2; -}drop_lintel_zone; - - - - -#define MAX_ZONES 30 - -#define MAX_SISTER_PER_ZONE 1 - - - -unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in, Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj); - -float vect_get_angle(vect_t*v, vect_t* w); - -void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out, vect_t *v_out, vect_t *r1, vect_t*r2); - -void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in, vect_t *v, point_t p); - -void vect_rot_trigo(vect_t* v); - -int object_poly_caliper_to_rectangle(Object_poly *o, - unsigned int *pts_index_out, vect_t* caliper, - vect_t *r1, vect_t*r2, point_t *p); - -int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color); - - - -int zone_filtre_min_surface(unsigned char* data, int16_t x_in, int16_t y_in, - uint8_t color, unsigned int num_zone, zone* p); - -void reset_drop_zone(void); -void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones); - -unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max, - uint8_t n_columns, uint8_t n_lintels, - uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone); - -uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in, - zone * all_zones, uint8_t max_zones); - - -uint8_t color2h(uint8_t color); -uint8_t h2color(uint8_t color); - -int8_t get_column_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone, int16_t center_x, int16_t center_y, - int16_t * dropzone_x, int16_t * dropzone_y); - - -uint8_t is_temple_there(unsigned char * buffer, int16_t x_in, int16_t y_in, - uint8_t h, int16_t center_x, int16_t center_y); - - - -int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone, int16_t center_x, int16_t center_y, - int16_t * dropzone_x, int16_t * dropzone_y); - -void process_img_to_zone(unsigned char *buffer, int16_t x_in, int16_t y_in); - -#define PIXEL2CM (300./27.) - - -extern uint8_t g_zone_num; -extern zone g_all_zones[MAX_ZONES]; diff --git a/projects/microb2010/sensorboard/scanner.c b/projects/microb2010/sensorboard/scanner.c deleted file mode 100644 index dfc869e..0000000 --- a/projects/microb2010/sensorboard/scanner.c +++ /dev/null @@ -1,898 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include "sensor.h" - -#include - -#include "main.h" -#include "scanner.h" - -#include "cmdline.h" - - -#include "scan_h_l.h" - - -#include -#include "img_processing.h" - - -#include "../common/i2c_commands.h" - -#define SCANNER_DEBUG(args...) DEBUG(E_USER_SCANNER, args) -#define SCANNER_NOTICE(args...) NOTICE(E_USER_SCANNER, args) -#define SCANNER_ERROR(args...) ERROR(E_USER_SCANNER, args) - - -#define MODULO_TIMER (1023L) - -#define COEFF_TIMER (2) -#define COEFF_MULT (1000L) -#define COEFF_MULT2 (1000L) - -double TELEMETRE_A = TELEMETRE_A_INIT; -double TELEMETRE_B = TELEMETRE_B_INIT; - - - - -struct scan_params scan_params; - -static volatile int32_t scan_delta_pos; -static volatile int32_t scan_tick_cur = 0; -static volatile int32_t scan_tick_prev = 0; -/*static volatile int32_t count = 0; -static volatile int32_t count_diff_rising = 0; -static volatile int32_t count_diff_falling = 0; -*/ -static int32_t scanner_coeff = 0; - -//static volatile int8_t valid_scanner = 0; - -static volatile int32_t scan_pos_cur = 0; - -static int32_t pos_ref = 0; - - -int32_t encoders_spi_get_value_scanner(void *number) -{ - int32_t ret; - - ret = encoders_spi_get_value(number); - return ret*4; -} - - -void encoders_spi_set_value_scanner(void * number, int32_t v) -{ - encoders_spi_set_value(number, v/4); -} - -int32_t encoders_spi_update_scanner(void * number) -{ - int32_t ret; - uint8_t flags; - - IRQ_LOCK(flags); - ret = encoders_spi_get_value_scanner(number); - scan_delta_pos = ret - scan_pos_cur; - scan_pos_cur = ret; - scan_tick_prev = scan_tick_cur; - scan_tick_cur = TCNT3; - - scanner_coeff = (scan_delta_pos * COEFF_MULT) / - ((scan_tick_cur - scan_tick_prev + MODULO_TIMER + 1) & MODULO_TIMER); - - IRQ_UNLOCK(flags); - - - return ret; -} - -int32_t encoders_spi_get_value_scanner_interpolation(void * number) -{ - uint8_t flags; - int32_t pos; - - IRQ_LOCK(flags); - pos = scan_pos_cur; - pos += (scanner_coeff * ((TCNT3 - scan_tick_cur + MODULO_TIMER + 1)& MODULO_TIMER ))/ - COEFF_MULT; - - IRQ_UNLOCK(flags); - - return pos; -} - - -void scanner_reset_pos(void) -{ - pwm_ng_set(SCANNER_PWM, 0); - encoders_spi_set_value_scanner(SCANNER_ENCODER, 0); -} - -void scanner_init(void) -{ - - scan_params.working = 0; - scan_params.must_stop = 0; - - scanner_reset_pos(); - pos_ref = encoders_spi_get_value_scanner(SCANNER_ENCODER); - - //memset(&scanner, 0, sizeof(struct scanner)); - - scan_delta_pos = 0; - - /*for(i=0;i 0){ - time_wait_ms(10); - } - - /* arm in */ - pwm_ng_set(&gen.servo3, SCANNER_POS_IN); - -} - - -/* - * called from IRQ: - * mode can be off/prepare/start, see in i2c_commands.h - */ -void scanner_set_mode(uint8_t mode) -{ - if (mode == I2C_SENSORBOARD_SCANNER_PREPARE){ - /* reset flag max pos */ - scan_params.max_column_detected = 0; - - /* arm out */ - pwm_ng_set(&gen.servo3, SCANNER_POS_OUT); - - } - else if (mode == I2C_SENSORBOARD_SCANNER_STOP){ - /* arm in */ - pwm_ng_set(&gen.servo3, SCANNER_POS_IN); - scan_params.must_stop = 1; - } - else if (mode == I2C_SENSORBOARD_SCANNER_START){ - scan_params.max_column_detected = 0; - scan_params.must_stop = 0; - - - /* start scan in background */ - scanner_do_scan(); - } - - -} - -/* -void scanner_stop(void) -{ - sensorboard.scanner.on = 0; - pwm_ng_set(SCANNER_PWM, 0); -} -*/ - - -/* -int32_t encoders_spi_get_scanner_speed(void * dummy) -{ - return scanner_speed; -} -*/ - - -//uint8_t sample_tab[MAX_SAMPLE]; -//uint16_t sample_i = 0; - - -//#define offset_a (75.*M_PI/180.) -//float offset_a; -//float offset_b; - -//int32_t pos_start_scan; - - -/* get motor angle in radian; return mirror angle in radian, cos a sin a */ -void ang2_a_mirror(float b, float * c_a, float* s_a, float* a) -{ - float x2, y2; - float A, DELTA, B, D; - - b+=scan_params.offset_b; - x2 = X + l1*cos(b); - y2 = Y + l1*sin(b); - - A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3); - - DELTA = -(A*A - x2*x2 - y2*y2); - B = sqrt(DELTA); - - D = x2*x2 + y2*y2; - - *c_a = (x2*A + y2*B)/D; - *s_a = -(x2*B - y2*A)/D; - - *a = atan2(*s_a, *c_a); - - *a += scan_params.offset_a; - // *s_a = sin(*a); - // *c_a = cos(*a); - -} - -/* get telemeter dist , cos a, sin a, a and return H, L of scanned point */ -void ang2_H_L(float l_telemetre, float c_a, float s_a, float a, float *H, float *L) -{ - float d; - d = h_mirror*c_a/s_a; - *H = (l_telemetre - l_mirror - d)*sin(2*a); - *L = l_mirror + d + *H/tan(2*a); - - //*H+= 8*sin(a-scan_params.offset_a); -} - - - -//int32_t last_col_n; -//int32_t last_row_n; -//uint8_t last_sample; - -//uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123}; -//uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123}; - -/* last high is dummy, to deal higher columns */ -uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250}; -#define H_MARGIN 7 - - -#if 0 -void do_scan(void * dummy) -{ - - unsigned int i; - int16_t a; - int32_t row_n; - int32_t col_n; - - - int32_t tour_pos; - int32_t pos, last_pos; - int32_t pos_tmp ; - int32_t mot_pos; - float dist; - uint8_t min_sample; - - float b, c_a, s_a, H, L, m_a; - int32_t H_fin; - - - if (scan_params.sample_i==0) - return; - - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); - - if (scan_params.sample_i==1){ - SCANNER_DEBUG("dump end enc %ld %d ", mot_pos, PIX_PER_SCAN); - //scanner.flags &= (~CS_ON); - - - - /* stop scan at cur pos + 10 round */ - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); - mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ; - - SCANNER_DEBUG("set to %ld ", mot_pos); - - cs_set_consign(&sensorboard.scanner.cs, mot_pos); - //pwm_ng_set(SCANNER_MOT_PWM, 0); - - - } - - mot_pos-= scan_params.pos_start_scan; - - a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 ); - - - //dist = (a-TELEMETRE_B)/TELEMETRE_A; - dist = TELEMETRE_A * a +TELEMETRE_B; - - //SCANNER_DEBUG("enc val = %ld", encoders_microb_get_value((void *)SCANNER_ENC)); - - - //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a; - //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0; - - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); -#if 0 - /* separe scan forward/backword */ - if (row_n%2){ - row_n/=2; - } - else{ - row_n/=2; - row_n+=30; - } -#endif - - tour_pos = (mot_pos)%(SCANNER_STEP_TOUR); - - b = (2.*M_PI)*(float)tour_pos/(float)(SCANNER_STEP_TOUR); - - ang2_a_mirror(b, &c_a, &s_a, &m_a); - ang2_H_L(dist, c_a, s_a, m_a, &H, &L); - - - SCANNER_DEBUG("%ld %d", tour_pos, a); - - if (H >0){ - printf("zarb H\n"); - H = 0; - } - - if (dist> SCAN_MAX_DIST){ - H = 0; - L = 0; - } - - H = H;//m_a*180/M_PI; - L = L;//(L-100)*PIX_PER_SCAN; - - //SCANNER_DEBUG("polling : ADC0 = %i %f",a, dist); - //SCANNER_DEBUG("%f %f %2.2f %f", H, L, m_a*180./M_PI, dist); - - - //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI); - - H=(H+SCAN_H_MAX)*SCAN_H_COEF; - L-=SCAN_L_MIN; - - - /* XXX may never append */ - if (L<0) - L=0; - - - /* first filter => pixel modulo level */ -#define H_BASE 10 -#define H_MUL 14 - H_fin = H;//+SCAN_H_MAX; - //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE; - - if (scan_params.filter){ - H_fin = 11; // default is level 11 - for (i=0;iPIX_PER_SCAN) - printf("BUG!!! RECALC MAX L"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos =DIM_DIST) - j = DIM_DIST-1; - - if (i>=DIM_ANGLE) - i = DIM_ANGLE-1; - - - val.u16 = pgm_read_word_near(&array_h_l[j][i]); - - - //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - //val.u16 = pgm_read_word_near(&array_h_l[a][tp]); - H = val.h_l.h; - L = val.h_l.l; - /* - val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - - H = val.h_l.h; - L = val.h_l.l; - */ - H_fin = H; - L_fin = L; - - - if (L<=0) - L = 0; - - col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN); - if (col_n>=PIX_PER_SCAN) { - //printf("BUG!!! RECALC MAX L"); - col_n = PIX_PER_SCAN-1; - } - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos * * This program is free software; you can redistribute it and/or modify @@ -20,6 +20,8 @@ * */ +#include + #include #include @@ -28,69 +30,39 @@ #include #include #include +#include +#include +#include #include "cmdline.h" +#include "uart_proto.h" +#include "trigo.h" #include "main.h" - -/* beacon identifier: must be odd, 3 bits */ -#define BEACON_ID_MASK 0x7 -#define BEACON_ID_SHIFT 0 -#define FRAME_DATA_MASK 0xFF8 -#define FRAME_DATA_SHIFT 3 +#include "board2006.h" +//#include "board2010.h" /******************* TSOP */ -#define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */ -#ifdef BOARD2006 -#define INTx_TSOP INT6 -#define ISCx0_TSOP ISC60 -#define ISCx1_TSOP ISC61 -#define SIG_TSOP SIG_INTERRUPT6 -#define TSOP_READ() (!(PINE & 0x40)) -#else -#define INTx_TSOP INT4 -#define ISCx0_TSOP ISC40 -#define ISCx1_TSOP ISC41 -#define SIG_TSOP SIG_INTERRUPT4 -#define TSOP_READ() (!(PINE & 0x10)) -#endif - -//#define MODUL_455KHZ -#define MODUL_38KHZ - -#if (defined MODUL_455KHZ) -#define TSOP_FREQ_MHZ 0.455 -#define N_PERIODS 10. -#else -#define TSOP_FREQ_MHZ 0.038 -#define N_PERIODS 15. -#endif - -#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ) - -#define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US) -#define TSOP_TIME_LONG_US (2.5 * N_PERIODS * TSOP_PERIOD_US) - -#define TSOP_TIME_SHORT ((uint16_t)(TSOP_TIME_SHORT_US*2)) -#define TSOP_TIME_LONG ((uint16_t)(TSOP_TIME_LONG_US*2)) - -#define FRAME_LEN 16 -#define FRAME_MASK ((1UL << FRAME_LEN) - 1) - struct detected_frame { uint16_t frame; + uint16_t ref_time; uint16_t time; + uint16_t tick; }; /* frame */ struct frame_status { uint8_t led_cpt; - uint16_t start_angle_time; + uint16_t ref_time; + uint16_t start_time; uint16_t frame; uint16_t mask; uint16_t prev_time; + uint16_t time_long; + uint16_t time_short; uint8_t prev_tsop; uint8_t len; + uint8_t frame_len; uint8_t val; #define FRAME_RING_ORDER 4 #define FRAME_RING_SIZE (1<prev_time; /* first rising edge */ - if (status->len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) { + if (status->len == 0 && cur_tsop && diff_time > status->time_long) { status->len = 1; status->val = 1; status->frame = 0; - status->start_angle_time = cur_time - ref_time; + status->start_time = cur_time; + status->ref_time = ref_time; status->mask = 1; } /* any short edge */ - else if (status->len != 0 && diff_time < TSOP_TIME_SHORT) { + else if (status->len != 0 && diff_time < status->time_short) { if (status->len & 1) { if (status->val) status->frame |= status->mask; @@ -200,7 +186,7 @@ static inline void decode_frame(struct frame_status *status, status->len ++; } /* any long edge */ - else if (status->len != 0 && diff_time < TSOP_TIME_LONG) { + else if (status->len != 0 && diff_time < status->time_long) { status->val = !status->val; if (status->val) status->frame |= status->mask; @@ -213,12 +199,17 @@ static inline void decode_frame(struct frame_status *status, } /* end of frame */ - if (status->len == FRAME_LEN*2) { + if (status->len == status->frame_len*2) { uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK; + uint16_t frame_mask; + + frame_mask = (1 << status->frame_len) - 1; if (tail_next != status->head) { - status->ring[status->tail].frame = (status->frame & FRAME_MASK); - status->ring[status->tail].time = status->start_angle_time; + status->ring[status->tail].frame = (status->frame & frame_mask); + status->ring[status->tail].ref_time = status->ref_time; + status->ring[status->tail].time = status->start_time; + status->ring[status->tail].tick = tick; status->tail = tail_next; if ((status->led_cpt & 0x7) == 0) LED3_TOGGLE(); @@ -232,7 +223,7 @@ static inline void decode_frame(struct frame_status *status, } /* decode frame */ -SIGNAL(SIG_TSOP) { +SIGNAL(SIG_TSOP_STA) { static uint8_t running = 0; /* tsop status */ @@ -242,7 +233,7 @@ SIGNAL(SIG_TSOP) { ref_time = ICR3; cur_time = TCNT3; - cur_tsop = TSOP_READ(); + cur_tsop = TSOP_STA_READ(); /* avoid interruption stacking */ if (running) @@ -260,6 +251,35 @@ SIGNAL(SIG_TSOP) { running = 0; } +/* decode frame */ +SIGNAL(SIG_TSOP_OPP) { + static uint8_t running = 0; + + /* tsop status */ + uint8_t cur_tsop; + uint16_t ref_time; + uint16_t cur_time; + + ref_time = ICR3; + cur_time = TCNT3; + cur_tsop = TSOP_OPP_READ(); + + /* avoid interruption stacking */ + if (running) + return; + running = 1; + sei(); + +/* if (cur_tsop) */ +/* LED2_ON(); */ +/* else */ +/* LED2_OFF(); */ + + decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop); + + running = 0; +} + /* absolute value */ static inline int32_t AbS(int32_t x) { @@ -304,39 +324,192 @@ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff) } } - /* real time difference in 1/2 us */ + /* real time difference in timer unit (resolution 4us) */ diff = (best_cpt * 16384L) + (icr_diff & 0x3fff); - return 2000000000L/diff; + current_motor_period = diff; /* save it in global var */ + return 250000000L/diff; } /* process the received frame ring */ -void process_ring(struct frame_status *status) +static void process_sta_ring(struct frame_status *status) { - uint8_t head_next; - uint32_t frame; + uint8_t head, head_next; + uint16_t frame, frametick; + uint8_t found = 0; + uint8_t beacon_id; - /* after CS, check if we have a new frame in ring */ + /* beacon 0 */ + uint16_t data0, time0, ref_time0; + double angle0; + double dist0; + + /* beacon 1 */ + uint16_t data1, time1, ref_time1; + double angle1; + double dist1; + + point_t pos; + double a; + + /* remove too old captures from the ring */ while (status->head != status->tail) { head_next = (status->head+1) & FRAME_RING_MASK; - frame = status->ring[status->head].frame; + frametick = status->ring[status->head].tick; + if ((uint16_t)(tick - frametick) < MAX_CAP_AGE) + break; + status->head = head_next; + } + + head = status->head; + /* after CS, check if we have a new frame in ring */ + while (head != status->tail) { + head_next = (head+1) & FRAME_RING_MASK; + frame = status->ring[head].frame; + + /* ignore bad cksum */ + if (verify_cksum(frame) == 0xFFFF) + continue; + + beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK; + if (beacon_id != TSOP_STA_BEACON_ID0 && + beacon_id != TSOP_STA_BEACON_ID1) + continue; + + /* if motor speed is not good, skip values */ + if (current_motor_period < MOTOR_PERIOD_MIN) + continue; + if (current_motor_period > MOTOR_PERIOD_MAX) + continue; /* display if needed */ if (beacon_tsop.debug_frame) { - uint8_t beacon_id; - uint16_t data; + printf("STA ID=%d time=%d\r\n", + beacon_id, status->ring[head].time); + } + + if (beacon_id == TSOP_STA_BEACON_ID0) { + found |= 0x1; + data0 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK; + time0 = status->ring[head].time; + ref_time0 = status->ring[head].ref_time; + } + else if (beacon_id == TSOP_STA_BEACON_ID1) { + found |= 0x2; + data1 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK; + time1 = status->ring[head].time; + ref_time1 = status->ring[head].ref_time; + } + + head = head_next; + } + + /* if we didn't found beacon 0 and 1, return */ + if (found != 0x3) + return; + + /* update ring head */ + status->head = head; + + /* beacon 0 */ + dist0 = data0; + dist0 /= 512.; + dist0 *= (MAX_DIST-MIN_DIST); + dist0 += MIN_DIST; + + time0 = time0 - ref_time0; + angle0 = (double)time0 / (double)current_motor_period; + if (angle0 > 1.) + angle0 -= 1.; + if (angle0 > 1.) + return; /* fail */ + angle0 *= (2 * M_PI); + if (angle0 > M_PI) + angle0 -= M_PI; + + /* beacon 1 */ + dist1 = data1; + dist1 /= 512.; + dist1 *= (MAX_DIST-MIN_DIST); + dist1 += MIN_DIST; + + time1 = time1 - ref_time1; + angle1 = (double)time1 / (double)current_motor_period; + if (angle1 > 1.) + angle1 -= 1.; + if (angle1 > 1.) + return; /* fail */ + angle1 *= (2 * M_PI); + if (angle0 > M_PI) + angle0 -= M_PI; + + if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0, + angle1, dist1) < 0) + return; + + xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a); +} + +/* process the received frame ring */ +static void process_opp_ring(struct frame_status *status) +{ + uint8_t head_next; + uint16_t frame; + uint8_t found = 0; + uint8_t beacon_id; + uint16_t data, time, ref_time; + double angle; + double dist; - /* ignore bad cksum */ - if (verify_cksum(frame) == 0xFFFF) - continue; + /* after CS, check if we have a new frame in ring */ + while (status->head != status->tail) { + head_next = (status->head+1) & FRAME_RING_MASK; + frame = status->ring[status->head].frame; + + /* ignore bad cksum */ + if (verify_cksum(frame) == 0xFFFF) + continue; + + beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK; + if (beacon_id != TSOP_OPP_BEACON_ID) + continue; + + /* if motor speed is not good, skip values */ + if (current_motor_period < MOTOR_PERIOD_MIN) + continue; + if (current_motor_period > MOTOR_PERIOD_MAX) + continue; + + found = 1; + data = (frame >> TSOP_OPP_FRAME_DATA_SHIFT) & TSOP_OPP_FRAME_DATA_MASK; + time = status->ring[status->head].time; + ref_time = status->ring[status->head].ref_time; - beacon_id = (frame >> BEACON_ID_SHIFT) & BEACON_ID_MASK; - data = (frame >> FRAME_DATA_SHIFT) & FRAME_DATA_MASK; - printf("ID=%d data=%d time=%d\r\n", + /* display if needed */ + if (beacon_tsop.debug_frame) { + printf("OPP ID=%d data=%d time=%d\r\n", beacon_id, data, status->ring[status->head].time); } status->head = head_next; } + + if (found == 0) + return; + + dist = data; + dist /= 512.; + dist *= (MAX_DIST-MIN_DIST); + dist += MIN_DIST; + + time = time - ref_time; + angle = (double)time / (double)current_motor_period; + if (angle > 1.) + angle -= 1.; + if (angle > 1.) + return; /* fail */ + angle *= 3600; /* angle in 1/10 deg */ + + xmit_opp((uint16_t)dist, (uint16_t)angle); } int main(void) @@ -351,6 +524,14 @@ int main(void) uint16_t tcnt3; uint8_t x = 0; + opp_beacon.frame_len = TSOP_OPP_FRAME_LEN; + opp_beacon.time_long = TSOP_OPP_TIME_LONG; + opp_beacon.time_short = TSOP_OPP_TIME_SHORT; + + static_beacon.frame_len = TSOP_STA_FRAME_LEN; + static_beacon.time_long = TSOP_STA_TIME_LONG; + static_beacon.time_short = TSOP_STA_TIME_SHORT; + /* LEDS */ LED1_DDR |= _BV(LED1_BIT); LED2_DDR |= _BV(LED2_BIT); @@ -375,8 +556,8 @@ int main(void) debug_serial(); /* configure external interrupt for TSOP */ - EICRx_TSOP |= _BV(ISCx0_TSOP); - EIMSK |= _BV(INTx_TSOP); + EICRx_TSOP |= _BV(ISCx0_TSOP_STA) | _BV(ISCx0_TSOP_OPP); + EIMSK |= _BV(INTx_TSOP_STA) | _BV(INTx_TSOP_OPP); /* pwm for motor */ PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, @@ -390,10 +571,10 @@ int main(void) TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); OCR0 = 80; /* f = 100 khz at 16 Mhz */ - /* configure timer 3: CLK/8 + /* configure timer 3: CLK/64 * it is used as a reference time * enable noise canceller for ICP3 */ - TCCR3B = _BV(ICNC3) | _BV(CS11); + TCCR3B = _BV(ICNC3) | _BV(CS11) | _BV(CS10); sei(); @@ -468,7 +649,11 @@ int main(void) if (cpt < CPT_ICR_MAX) cpt ++; - process_ring(&static_beacon); + process_sta_ring(&static_beacon); + process_opp_ring(&opp_beacon); + cli(); + tick ++; /* global imprecise time reference */ + sei(); } return 0; diff --git a/projects/microb2010/tests/beacon_tsop/main.h b/projects/microb2010/tests/beacon_tsop/main.h index 0a85337..42709ee 100644 --- a/projects/microb2010/tests/beacon_tsop/main.h +++ b/projects/microb2010/tests/beacon_tsop/main.h @@ -20,52 +20,6 @@ * */ -#define BOARD2006 - -/********************** LEDs */ -#define LED_TOGGLE(port, bit) do { \ - if (port & _BV(bit)) \ - port &= ~_BV(bit); \ - else \ - port |= _BV(bit); \ - } while(0) - -#ifdef BOARD2006 -#define LED1_PORT PORTE -#define LED1_DDR DDRE -#define LED1_BIT 2 -#define LED2_PORT PORTE -#define LED2_DDR DDRE -#define LED2_BIT 3 -#define LED3_PORT PORTB -#define LED3_DDR DDRB -#define LED3_BIT 3 -#define LED1_ON() sbi(LED1_PORT, LED1_BIT) -#define LED1_OFF() cbi(LED1_PORT, LED1_BIT) -#define LED1_TOGGLE() LED1_TOGGLE(LED_PORT, LED1_BIT) -#define LED2_ON() sbi(LED2_PORT, LED2_BIT) -#define LED2_OFF() cbi(LED2_PORT, LED2_BIT) -#define LED2_TOGGLE() LED_TOGGLE(LED2_PORT, LED2_BIT) -#define LED3_ON() sbi(LED3_PORT, LED3_BIT) -#define LED3_OFF() cbi(LED3_PORT, LED3_BIT) -#define LED3_TOGGLE() LED_TOGGLE(LED3_PORT, LED3_BIT) -#else -#define LED_PORT PORTD -#define LED_DDR DDRD -#define LED1_BIT 5 -#define LED2_BIT 6 -#define LED3_BIT 7 -#define LED1_ON() sbi(LED_PORT, LED1_BIT) -#define LED1_OFF() cbi(LED_PORT, LED1_BIT) -#define LED1_TOGGLE() LED_TOGGLE(LED_PORT, LED1_BIT) -#define LED2_ON() sbi(LED_PORT, LED2_BIT) -#define LED2_OFF() cbi(LED_PORT, LED2_BIT) -#define LED2_TOGGLE() LED_TOGGLE(LED_PORT, LED2_BIT) -#define LED3_ON() sbi(LED_PORT, LED3_BIT) -#define LED3_OFF() cbi(LED_PORT, LED3_BIT) -#define LED3_TOGGLE() LED_TOGGLE(LED_PORT, LED3_BIT) -#endif - struct beacon_tsop { struct rdline rdl; char prompt[RDLINE_PROMPT_SIZE]; diff --git a/projects/microb2010/tests/beacon_tsop/trigo.c b/projects/microb2010/tests/beacon_tsop/trigo.c index 391043c..df45edf 100644 --- a/projects/microb2010/tests/beacon_tsop/trigo.c +++ b/projects/microb2010/tests/beacon_tsop/trigo.c @@ -1,5 +1,5 @@ /* - * Copyright Droids Corporation (2010) + * Copyright Droids Corporation (2009) * * 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 @@ -15,7 +15,8 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Olivier MATZ + * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $ + * */ #include @@ -37,10 +38,23 @@ static int dprint = 0; #define dprintf(args...) if (dprint) printf(args) +#define AREA_X 3000 +#define AREA_Y 2100 + +#define ANGLE_EPSILON 0.005 + const point_t beacon0 = { 0, 1050 }; const point_t beacon1 = { 3000, 0 }; const point_t beacon2 = { 3000, 2100 }; +static inline double abs_dbl(double x) +{ + if (x > 0) + return x; + else + return -x; +} + /* Fill the 2 circles pointer given as parameter, each of those cross * both beacons b1 and b2. From any point of these circles (except b1 * and b2), we see b1 and b2 with the angle of a_rad (which must be @@ -79,8 +93,8 @@ int8_t angle_to_circles(circle_t *c1, circle_t *c2, vect_t v; float l, d; - /* reject negative or too small angles */ - if (a_rad <= 0.01) + /* reject too small angles */ + if (a_rad <= 0.01 && a_rad >= -0.01) return -1; /* get position of O */ @@ -159,22 +173,190 @@ int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20) return 0; } -/* get the angles of beacons from xy pos */ -int8_t posxy_to_angles(point_t pos, double *a01, double *a12, - double *a20, int err_num, float err_val) +static inline int8_t is_pt_in_area(point_t pt) { - double a0, a1, a2; + if (pt.x < 0 || pt.y < 0) + return 0; + if (pt.x > AREA_X || pt.y > AREA_Y) + return 0; + return 1; +} - a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x); - a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x); - a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x); +/* intersect the circle formed by one distance info with the circle + * crossing the 2 beacons */ +static int8_t +ad_to_posxy_one(point_t *pos, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd, const circle_t *c01, /* circles to intersect */ + double a01) /* seen angle of beacons */ +{ + double tmp_a01_p1, tmp_a01_p2; + point_t p1, p2; + uint8_t p1_ok=0, p2_ok=0; + + if (circle_intersect(c01, cd, &p1, &p2) == 0) + return -1; + + dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y); + dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y); + + dprintf("real a01: %2.2f\n", a01); + + tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) - + atan2(b0->y-p1.y, b0->x-p1.x); + if (tmp_a01_p1 > M_PI) + tmp_a01_p1 -= 2*M_PI; + if (tmp_a01_p1 < -M_PI) + tmp_a01_p1 += 2*M_PI; + dprintf("a01-1: %2.2f\n", tmp_a01_p1); + + tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) - + atan2(b0->y-p2.y, b0->x-p2.x); + if (tmp_a01_p2 > M_PI) + tmp_a01_p2 -= 2*M_PI; + if (tmp_a01_p2 < -M_PI) + tmp_a01_p2 += 2*M_PI; + dprintf("a01-2: %2.2f\n", tmp_a01_p2); + + /* in some conditions, we already know the position of the + * robot at this step */ + p1_ok = is_pt_in_area(p1) && + abs_dbl(tmp_a01_p1 - a01) < ANGLE_EPSILON; + + p2_ok = is_pt_in_area(p2) && + abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON; + if (!p1_ok && p2_ok) { + *pos = p2; + return 0; + } + if (p1_ok && !p2_ok) { + *pos = p1; + return 0; + } + return -1; +} + +static int8_t +ad_to_posxy_algo(point_t *pos, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd0, const circle_t *cd1, const circle_t *c01, /* circles to intersect */ + double a01, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) + +{ + switch (algo) { + + /* take the closer beacon first */ + case 0: + if (d0 < d1) { + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) + return 0; + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) + return 0; + return -1; + } + else { + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) + return 0; + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) + return 0; + return -1; + } + break; + case 1: + /* b0 only */ + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) + return 0; + break; + case 2: + /* b0 only */ + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) + return 0; + break; + default: + break; + } + /* not found */ + return -1; +} + +/* get the position and angle of the robot from the angle of the 2 + * beacons, and the distance of 2 beacons */ +int8_t +ad_to_posxya(point_t *pos, double *a, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + double a0, double a1, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) +{ + circle_t cd0, cd1, c01; + double a01; + + dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n", + algo, a0, a1, d0, d1); + + /* the first 2 circles depends on distance between robot and + * beacons */ + cd0.x = b0->x; + cd0.y = b0->y; + cd0.r = d0; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r); + cd1.x = b1->x; + cd1.y = b1->y; + cd1.r = d1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r); + + /* the third circle depends on angle between b0 and b1 */ + a01 = a1-a0; + if (a01 < -M_PI) + a01 += 2*M_PI; + if (a01 > M_PI) + a01 -= 2*M_PI; + + if (angle_to_circles(&c01, NULL, b0, b1, a01)) + return -1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r); + + /* get the xy position, depending on algo */ + if (ad_to_posxy_algo(pos, algo, b0, b1, &cd0, &cd1, &c01, a01, d0, d1) < 0) + return -1; + + /* now, we have the xy position, we can get angle thanks to + * the angles a0 and a1. Take the far beacon. */ + if (d0 < d1) + *a = atan2(beacon1.y - pos->y, beacon1.x - pos->x) - a1; + else + *a = atan2(beacon0.y - pos->y, beacon0.x - pos->x) - a0; + if (*a < -M_PI) + *a += 2*M_PI; + if (*a > M_PI) + *a -= 2*M_PI; + return 0; +} + +/* get the angles of beacons from xy pos */ +int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1, + double *a2, int err_num, float err_val) +{ + *a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x); + *a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x); + *a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x); if (err_num == 0 || err_num == 3) - a0 += (err_val * M_PI/180.); + *a0 += (err_val * M_PI/180.); if (err_num == 1 || err_num == 3) - a1 += (err_val * M_PI/180.); + *a1 += (err_val * M_PI/180.); if (err_num == 2 || err_num == 3) - a2 += (err_val * M_PI/180.); + *a2 += (err_val * M_PI/180.); + + return 0; +} + +/* get the angles of beacons from xy pos */ +int8_t posxy_to_angles(point_t pos, double *a01, double *a12, + double *a20, int err_num, float err_val) +{ + double a0, a1, a2; + posxy_to_abs_angles(pos, &a0, &a1, &a2, err_num, err_val); *a01 = a1-a0; if (*a01 < 0) @@ -245,3 +427,185 @@ int8_t process_move_error(double x, double y, double speed, *err = 50.; return 0; } + +#if 0 +/* whole process is around 3ms on atmega128 at 16Mhz */ +int main(int argc, char **argv) +{ + double a0, a1, a2; + double a01, a12, a20; + point_t pos, tmp; + const char *mode = "nothing"; + +#ifdef HOST_VERSION + if (argc < 2) { + printf("bad args\n"); + return -1; + } + mode = argv[1]; +#else + mode = "angle2pos"; + argc = 5; + a0 = -1.; + a1 = 1.65; + a2 = 3.03; +#endif + + /* + * angle2pos a0 a1 a2: + * (angles in radian, -pi < a < pi) + */ + if (argc == 5 && strcmp(mode, "angle2pos") == 0) { +#ifdef HOST_VERSION + dprint = 1; + a0 = atof(argv[2]); + a1 = atof(argv[3]); + a2 = atof(argv[4]); +#endif + + /* */ + a01 = a1-a0; + if (a01 < 0) + a01 += 2*M_PI; + a12 = a2-a1; + if (a12 < 0) + a12 += 2*M_PI; + a20 = a0-a2; + if (a20 < 0) + a20 += 2*M_PI; + + if (angles_to_posxy(&pos, a01, a12, a20) < 0) + return -1; + printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y); + return 0; + } + + /* + * ad2pos a0 a1 d0 d1: + * (angles in radian, -pi < a < pi) + * (distances in mm) + */ + if (argc == 6 && strcmp(mode, "ad2pos") == 0) { + double a, d0, d1; + + dprint = 1; + a0 = atof(argv[2]); + a1 = atof(argv[3]); + d0 = atof(argv[4]); + d1 = atof(argv[5]); + + /* */ + if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, + a0, a1, d0, d1) < 0) + return -1; + printf("p0: x=%2.2f y=%2.2f a=%2.2f\n", pos.x, pos.y, a); + return 0; + } + + if (argc == 4 && strcmp(mode, "simple_error") == 0) { + int x, y; + int err_num; + double err_val_deg; + double err; + + err_num = atof(argv[2]); /* which beacon sees an error */ + err_val_deg = atof(argv[3]); /* how many degrees of error */ + + for (x=0; x<300; x++) { + for (y=0; y<210; y++) { + + pos.x = x*10; + pos.y = y*10; + posxy_to_angles(pos, &a01, &a12, &a20, + err_num, err_val_deg); + if (angles_to_posxy(&tmp, a01, a12, a20)) + continue; + err = pt_norm(&tmp, &pos); + if (err > 50.) /* saturate error to 5cm */ + err = 50.; + printf("%d %d %2.2f\n", x, y, err); + } + } + return 0; + } + + /* da_error algo errpercent errdeg */ + if ((argc == 5 && strcmp(mode, "da_error") == 0) || + (argc == 5 && strcmp(mode, "da_error_mm") == 0)) { + int x, y, algo; + double err_val_deg; + double err_val_percent; + double err_val_mm; + double err, d0, d1, a; + + algo = atoi(argv[2]); + err_val_percent = atof(argv[3]); /* how many % of error for dist */ + err_val_mm = atof(argv[3]); /* how many mm of error for dist */ + err_val_deg = atof(argv[4]); /* how many degrees of error */ + + for (x=0; x<300; x++) { + for (y=0; y<210; y++) { + + pos.x = x*10; + pos.y = y*10; + posxy_to_abs_angles(pos, &a0, &a1, &a2, + 0, err_val_deg); + d0 = pt_norm(&pos, &beacon0); + d1 = pt_norm(&pos, &beacon1); + if (strcmp(mode, "da_error") == 0) { + d0 += d0 * err_val_percent / 100.; + d1 += d1 * err_val_percent / 100.; + } + else { + d0 += err_val_mm; + d1 += err_val_mm; + } + + if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1, + a0, a1, d0, d1) < 0) + continue; + + err = pt_norm(&tmp, &pos); + if (err > 50.) /* saturate error to 5cm */ + err = 50.; + printf("%d %d %2.2f\n", x, y, err); + } + } + return 0; + } + + if ((argc == 5 || argc == 7) + && strcmp(argv[1], "move_error") == 0) { + int x, y; + double angle, speed, period, err; + + speed = atof(argv[2]); /* speed in m/s ( = mm/ms) */ + period = atof(argv[3]); /* period of turret in ms */ + angle = atof(argv[4]); /* direction of moving */ + if (argc == 7) { + dprint = 1; + process_move_error(atof(argv[5]), atof(argv[6]), + speed, period, angle, &err); + printf("%2.2f %2.2f %2.2f\n", atof(argv[5]), + atof(argv[6]), err); + return 0; + } + + for (x=0; x<300; x++) { + for (y=0; y<210; y++) { + pos.x = x*10; + pos.y = y*10; + if (process_move_error(pos.x, pos.y, + speed, period, angle, + &err) < 0) + continue; + printf("%d %d %2.2f\n", x, y, err); + } + } + return 0; + } + + printf("bad args\n"); + return -1; +} +#endif diff --git a/projects/microb2010/tests/beacon_tsop/trigo.h b/projects/microb2010/tests/beacon_tsop/trigo.h index 1c3178f..8ac82f8 100644 --- a/projects/microb2010/tests/beacon_tsop/trigo.h +++ b/projects/microb2010/tests/beacon_tsop/trigo.h @@ -18,5 +18,16 @@ * Olivier MATZ */ +extern const point_t beacon0; +extern const point_t beacon1; +extern const point_t beacon2; + /* get the position of the robot from the angle of the 3 beacons */ int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20); + +/* get the position and angle of the robot from the angle of the 2 + * beacons, and the distance of 2 beacons */ +int8_t ad_to_posxya(point_t *pos, double *a, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + double a0, double a1, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ); diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c index d2accb3..caf0934 100755 --- a/projects/microb2010/tests/static_beacon/static_beacon.c +++ b/projects/microb2010/tests/static_beacon/static_beacon.c @@ -358,6 +358,23 @@ int main(void) while (1); #endif +#if 1 + /* test freq ~ 400khz */ + ICR1 = 38; + OCR1A = 19; + TCCR1B = _BV(WGM13) | _BV(WGM12); + TCNT1 = 37; + TCCR1A = _BV(COM1A1) | _BV(WGM11); + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10); + + /* motor PWM 50%, 8khz */ + DDRB |= 0x08; + OCR2 = 50; + TCCR2 = _BV(WGM21) | _BV(WGM20) | _BV(COM21) | _BV(CS21) ; + + while (1); +#endif + /* configure timer 0, prescaler = 64 */ TCCR0 = _BV(CS01) | _BV(CS00); -- 2.20.1