From: zer0 Date: Sun, 24 Jan 2010 23:47:45 +0000 (+0100) Subject: hostsim enhancements X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=87ccd3af8abb0da3e0fa98dc8e9216fc7b676f97 hostsim enhancements --- diff --git a/include/aversive.h b/include/aversive.h index c61c409..af91bf1 100644 --- a/include/aversive.h +++ b/include/aversive.h @@ -221,6 +221,7 @@ do { \ #define nothing() do {} while(0) #define cli() do {} while(0) #define sei() do {} while(0) +#define reset() exit(1) #endif /* HOST_VERSION */ /** diff --git a/modules/base/hostsim/hostsim.c b/modules/base/hostsim/hostsim.c index 850d23a..87bbfc6 100644 --- a/modules/base/hostsim/hostsim.c +++ b/modules/base/hostsim/hostsim.c @@ -34,19 +34,41 @@ #include #include #include +#include #include #include #include #include #include +#include #ifdef CONFIG_MODULE_SCHEDULER #include #endif +#ifdef CONFIG_MODULE_UART +#include +#endif pthread_mutex_t mut; static volatile int cpt = 0; +static struct termios oldterm; +/* static */ int old_stdin, old_stdout; +static int stdin_pipe[2]; +static int stdout_pipe[2]; + +enum msg_type { + SCHED, + UART_RCV, + UART_SND, +}; + +struct message { + enum msg_type type; + char c; +}; +static struct message g_msg; + #ifdef SA_SIGINFO static void sigusr1(__attribute__((unused)) int sig, __attribute__((unused)) siginfo_t *info, @@ -55,10 +77,25 @@ static void sigusr1(__attribute__((unused)) int sig, void sigusr1(__attribute__((unused)) int sig) #endif { - pthread_mutex_unlock(&mut); + struct message m; + m = g_msg; #ifdef CONFIG_MODULE_SCHEDULER - scheduler_interrupt(); + if (m.type == SCHED) { + pthread_mutex_unlock(&mut); + scheduler_interrupt(); + } +#endif + +#ifdef CONFIG_MODULE_UART + if (m.type == UART_RCV) { + uart_host_rx_event(m.c); + pthread_mutex_unlock(&mut); + } + if (m.type == UART_SND) { + uart_host_tx_event(m.c); + pthread_mutex_unlock(&mut); + } #endif } @@ -82,7 +119,7 @@ void host_wait_ms(int ms) /* sends signal to child */ void *parent(void *arg) { - pthread_t *thread = arg; + pthread_t thread = (pthread_t)arg; struct timeval cur_tv, prev_tv, tv_millisec; int n; @@ -101,7 +138,8 @@ void *parent(void *arg) n = 0; } pthread_mutex_lock(&mut); - pthread_kill(*thread, SIGUSR1); + g_msg.type = SCHED; + pthread_kill(thread, SIGUSR1); /* signal was acked */ tv_millisec.tv_sec = 0; @@ -115,10 +153,96 @@ void *parent(void *arg) return NULL; } +void *hostsim_uart_stdin(void *arg) +{ + pthread_t thread = (pthread_t)arg; + int n; + char c; + + /* read on old stdin and put it in pipe */ + while (1) { + n = read(old_stdin, &c, 1); + if (n <= 0) + break; + + pthread_mutex_lock(&mut); + g_msg.type = UART_RCV; + g_msg.c = c; + pthread_kill(thread, SIGUSR1); + + write(stdin_pipe[1], &c, 1); + } + pthread_exit(NULL); + return NULL; +} + +void *hostsim_uart_stdout(void *arg) +{ + pthread_t thread = (pthread_t)arg; + int n; + char c; + + /* read on our pipe, and forward it to the old stdout */ + while (1) { + n = read(stdout_pipe[0], &c, 1); + if (n <= 0) + break; + + pthread_mutex_lock(&mut); + g_msg.type = UART_SND; + g_msg.c = c; + pthread_kill(thread, SIGUSR1); + + write(old_stdout, &c, 1); + } + pthread_exit(NULL); + return NULL; +} + +int hostsim_uart_init(void) +{ + struct termios term; + + tcgetattr(0, &oldterm); + memcpy(&term, &oldterm, sizeof(term)); + term.c_lflag &= ~(ICANON | ECHO | ISIG); + tcsetattr(0, TCSANOW, &term); + + /* duplicate stdin */ + old_stdin = dup(0); + if (old_stdin < 0) + return 1; + + /* duplicate stdout */ + old_stdout = dup(1); + if (old_stdout < 0) + return -1; + + /* create 2 pipes */ + if (pipe(stdin_pipe) < 0) + return -1; + if (pipe(stdout_pipe) < 0) + return -1; + + /* replace file desc 0 (stdin) by our pipe */ + if (dup2(stdin_pipe[0], 0) < 0) + return -1; + close(stdin_pipe[0]); + + /* replace file desc 1 (stdout) by our pipe */ + if (dup2(stdout_pipe[1], 1) < 0) + return -1; + close(stdout_pipe[1]); + setbuf(stdin, NULL); + setbuf(stdout, NULL); + + return 0; +} + int hostsim_init(void) { struct sigaction sigact; - pthread_t parent_id, child_id; + pthread_t parent_id, child_id, child2_id, child3_id; int ret; pthread_mutex_init(&mut, NULL); @@ -126,12 +250,30 @@ int hostsim_init(void) parent_id = pthread_self(); pthread_mutex_lock(&mut); - ret = pthread_create(&child_id, NULL, parent, (void *)&parent_id); + ret = pthread_create(&child_id, NULL, parent, (void *)parent_id); + if (ret) { + printf("pthread_create() returned %d\n", ret); + pthread_mutex_unlock(&mut); + return -1; + } + +#ifdef CONFIG_MODULE_UART + if (hostsim_uart_init()) + return -1; + + ret = pthread_create(&child2_id, NULL, hostsim_uart_stdin, (void *)parent_id); if (ret) { printf("pthread_create() returned %d\n", ret); pthread_mutex_unlock(&mut); return -1; } + ret = pthread_create(&child3_id, NULL, hostsim_uart_stdout, (void *)parent_id); + if (ret) { + printf("pthread_create() returned %d\n", ret); + pthread_mutex_unlock(&mut); + return -1; + } +#endif /* register a signal handler, which is interruptible */ memset(&sigact, 0, sizeof(sigact)); @@ -148,4 +290,11 @@ int hostsim_init(void) return 0; } + +void hostsim_exit(void) +{ +#ifdef CONFIG_MODULE_UART + tcsetattr(0, TCSANOW, &oldterm); +#endif +} #endif /* HOST_VERSION */ diff --git a/modules/base/hostsim/hostsim.h b/modules/base/hostsim/hostsim.h index b61d9e3..cc41183 100644 --- a/modules/base/hostsim/hostsim.h +++ b/modules/base/hostsim/hostsim.h @@ -20,8 +20,9 @@ * */ -/* initialize hostsim framework */ +/* initialize / exit hostsim framework */ int hostsim_init(void); +int hostsim_exit(void); /* replacement for wait_ms() */ void host_wait_ms(int ms); diff --git a/modules/base/scheduler/Makefile b/modules/base/scheduler/Makefile index 1da307b..4db7b40 100644 --- a/modules/base/scheduler/Makefile +++ b/modules/base/scheduler/Makefile @@ -1,7 +1,8 @@ TARGET = scheduler # List C source files here. (C dependencies are automatically generated.) -SRC = scheduler_add.c scheduler_del.c scheduler_interrupt.c scheduler_dump.c +SRC := scheduler_add.c scheduler_del.c scheduler_interrupt.c +SRC += scheduler_dump.c scheduler_stats.c ifeq ($(H),1) SRC += scheduler_host.c else diff --git a/modules/base/scheduler/scheduler.c b/modules/base/scheduler/scheduler.c index 8d0f730..32e0a79 100644 --- a/modules/base/scheduler/scheduler.c +++ b/modules/base/scheduler/scheduler.c @@ -66,20 +66,3 @@ SIGNAL(SIG_OVERFLOW0) scheduler_interrupt(); } #endif /* CONFIG_MODULE_SCHEDULER_USE_TIMERS */ - - -void scheduler_stats_dump(void) -{ -#ifdef CONFIG_MODULE_SCHEDULER_STATS - uint8_t i; - - printf_P(PSTR("alloc_fails: %"PRIu32"\r\n"), sched_stats.alloc_fails); - printf_P(PSTR("add_event: %"PRIu32"\r\n"), sched_stats.add_event); - printf_P(PSTR("del_event: %"PRIu32"\r\n"), sched_stats.del_event); - printf_P(PSTR("max_stacking: %"PRIu32"\r\n"), sched_stats.max_stacking); - for (i=0; i +#include +#include + +#include +#include +#include + +#include +#include + +void scheduler_stats_dump(void) +{ +#ifdef CONFIG_MODULE_SCHEDULER_STATS + uint8_t i; + + printf_P(PSTR("alloc_fails: %"PRIu32"\r\n"), sched_stats.alloc_fails); + printf_P(PSTR("add_event: %"PRIu32"\r\n"), sched_stats.add_event); + printf_P(PSTR("del_event: %"PRIu32"\r\n"), sched_stats.del_event); + printf_P(PSTR("max_stacking: %"PRIu32"\r\n"), sched_stats.max_stacking); + for (i=0; i +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "main.h" +#include "robotsim.h" + +void pwm_set_and_save(void *pwm, int32_t val) +{ + /* we need to do the saturation here, before saving the + * value */ + if (val > 4095) + val = 4095; + if (val < -4095) + val = -4095; + + if (pwm == LEFT_PWM) + mainboard.pwm_l = val; + else if (pwm == RIGHT_PWM) + mainboard.pwm_r = val; +#ifdef HOST_VERSION + robotsim_pwm(pwm, val); +#else + pwm_ng_set(pwm, val); +#endif +} + +void pickup_wheels_on(void) +{ + mainboard.enable_pickup_wheels = 1; +} + +void pickup_wheels_off(void) +{ + mainboard.enable_pickup_wheels = 0; +} + diff --git a/projects/microb2010/tests/hostsim/actuator.h b/projects/microb2010/tests/hostsim/actuator.h new file mode 100644 index 0000000..9c7174a --- /dev/null +++ b/projects/microb2010/tests/hostsim/actuator.h @@ -0,0 +1,25 @@ +/* + * 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.2 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +void pwm_set_and_save(void *pwm, int32_t val); +void pickup_wheels_on(void); +void pickup_wheels_off(void); + diff --git a/projects/microb2010/tests/hostsim/cmdline.c b/projects/microb2010/tests/hostsim/cmdline.c new file mode 100644 index 0000000..3038036 --- /dev/null +++ b/projects/microb2010/tests/hostsim/cmdline.c @@ -0,0 +1,172 @@ +/* + * 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: cmdline.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#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 "strat_base.h" + + +/******** See in commands.c for the list of commands. */ +extern parse_pgm_ctx_t main_ctx[]; + +static void write_char(char c) +{ + uart_send(CMDLINE_UART, c); +} + +static void +valid_buffer(const char *buf, uint8_t size) +{ + int8_t ret; + + /* reset CTRL-C for trajectory interruption each time we + * receive a new command */ + interrupt_traj_reset(); + + ret = parse(main_ctx, buf); + if (ret == PARSE_AMBIGUOUS) + printf_P(PSTR("Ambiguous command\r\n")); + else if (ret == PARSE_NOMATCH) + printf_P(PSTR("Command not found\r\n")); + else if (ret == PARSE_BAD_ARGS) + printf_P(PSTR("Bad arguments\r\n")); +} + +static int8_t +complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize, + int16_t *state) +{ + return complete(main_ctx, buf, state, dstbuf, dstsize); +} + +/* sending "pop" on cmdline uart resets the robot */ +void emergency(char c) +{ + static uint8_t i = 0; + + /* interrupt traj here */ + if (c == '\003') + interrupt_traj(); + + if ((i == 0 && c == 'p') || + (i == 1 && c == 'o') || + (i == 2 && c == 'p')) + i++; + else if ( !(i == 1 && c == 'p') ) + i = 0; + if (i == 3) { + hostsim_exit(); + reset(); + } +} + +/* log function, add a command to configure + * it dynamically */ +void mylog(struct error * e, ...) +{ + va_list ap; + // u16 stream_flags = stdout->flags; + uint8_t i; + time_h tv; + + if (e->severity > ERROR_SEVERITY_ERROR) { + if (gen.log_level < e->severity) + return; + + for (i=0; ierr_num) + break; + if (i == NB_LOGS+1) + return; + } + + va_start(ap, e); + tv = time_get_time(); + printf_P(PSTR("%d.%.3d: "), (int)tv.s, (int)(tv.us/1000UL)); + + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + + vfprintf_P(stdout, e->text, ap); + printf_P(PSTR("\r\n")); + va_end(ap); + //stdout->flags = stream_flags; +} + +int cmdline_interact(void) +{ + const char *history, *buffer; + int8_t ret, same = 0; + int16_t c; + + rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); + snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > "); + rdline_newline(&gen.rdl, gen.prompt); + + while (1) { + c = uart_recv_nowait(CMDLINE_UART); + if (c == -1) + continue; + ret = rdline_char_in(&gen.rdl, c); + if (ret != 2 && ret != 0) { + buffer = rdline_get_buffer(&gen.rdl); + history = rdline_get_history_item(&gen.rdl, 0); + if (history) { + same = !memcmp(buffer, history, strlen(history)) && + buffer[strlen(history)] == '\n'; + } + else + same = 0; + if (strlen(buffer) > 1 && !same) + rdline_add_history(&gen.rdl, buffer); + rdline_newline(&gen.rdl, gen.prompt); + } + } + + return 0; +} diff --git a/projects/microb2010/tests/hostsim/cmdline.h b/projects/microb2010/tests/hostsim/cmdline.h new file mode 100644 index 0000000..7d4521d --- /dev/null +++ b/projects/microb2010/tests/hostsim/cmdline.h @@ -0,0 +1,44 @@ +/* + * 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: cmdline.h,v 1.4 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#define CMDLINE_UART 0 + +/* uart rx callback for reset() */ +void emergency(char c); + +/* log function */ +void mylog(struct error * e, ...); + +/* launch cmdline */ +int cmdline_interact(void); + +static inline uint8_t cmdline_keypressed(void) { + return (uart_recv_nowait(CMDLINE_UART) != -1); +} + +static inline int16_t cmdline_getchar(void) { + return uart_recv_nowait(CMDLINE_UART); +} + +static inline uint8_t cmdline_getchar_wait(void) { + return uart_recv(CMDLINE_UART); +} diff --git a/projects/microb2010/tests/hostsim/commands.c b/projects/microb2010/tests/hostsim/commands.c new file mode 100644 index 0000000..111de4a --- /dev/null +++ b/projects/microb2010/tests/hostsim/commands.c @@ -0,0 +1,223 @@ +/* + * 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.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include + +/* commands_gen.c */ +extern parse_pgm_inst_t cmd_reset; +extern parse_pgm_inst_t cmd_bootloader; +extern parse_pgm_inst_t cmd_encoders; +extern parse_pgm_inst_t cmd_pwm; +extern parse_pgm_inst_t cmd_adc; +extern parse_pgm_inst_t cmd_sensor; +extern parse_pgm_inst_t cmd_log; +extern parse_pgm_inst_t cmd_log_show; +extern parse_pgm_inst_t cmd_log_type; +extern parse_pgm_inst_t cmd_stack_space; +extern parse_pgm_inst_t cmd_scheduler; + +/* commands_ax12.c */ +extern parse_pgm_inst_t cmd_baudrate; +extern parse_pgm_inst_t cmd_uint16_read; +extern parse_pgm_inst_t cmd_uint16_write; +extern parse_pgm_inst_t cmd_uint8_read; +extern parse_pgm_inst_t cmd_uint8_write; + +/* commands_cs.c */ +extern parse_pgm_inst_t cmd_gain; +extern parse_pgm_inst_t cmd_gain_show; +extern parse_pgm_inst_t cmd_speed; +extern parse_pgm_inst_t cmd_speed_show; +extern parse_pgm_inst_t cmd_derivate_filter; +extern parse_pgm_inst_t cmd_derivate_filter_show; +extern parse_pgm_inst_t cmd_consign; +extern parse_pgm_inst_t cmd_maximum; +extern parse_pgm_inst_t cmd_maximum_show; +extern parse_pgm_inst_t cmd_quadramp; +extern parse_pgm_inst_t cmd_quadramp_show; +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_mainboard.c */ +extern parse_pgm_inst_t cmd_event; +extern parse_pgm_inst_t cmd_spi_test; +extern parse_pgm_inst_t cmd_opponent; +extern parse_pgm_inst_t cmd_opponent_set; +extern parse_pgm_inst_t cmd_start; +extern parse_pgm_inst_t cmd_interact; +extern parse_pgm_inst_t cmd_color; +extern parse_pgm_inst_t cmd_rs; +extern parse_pgm_inst_t cmd_i2cdebug; +extern parse_pgm_inst_t cmd_mechboard_show; +extern parse_pgm_inst_t cmd_mechboard_setmode1; +extern parse_pgm_inst_t cmd_mechboard_setmode2; +extern parse_pgm_inst_t cmd_mechboard_setmode3; +extern parse_pgm_inst_t cmd_mechboard_setmode4; +extern parse_pgm_inst_t cmd_mechboard_setmode5; +extern parse_pgm_inst_t cmd_pickup_wheels; +extern parse_pgm_inst_t cmd_beacon_start; +extern parse_pgm_inst_t cmd_pump_current; +extern parse_pgm_inst_t cmd_build_test; +extern parse_pgm_inst_t cmd_column_test; +extern parse_pgm_inst_t cmd_column_test2; +extern parse_pgm_inst_t cmd_lintel_test; +extern parse_pgm_inst_t cmd_pickup_test; +extern parse_pgm_inst_t cmd_scan_test; +extern parse_pgm_inst_t cmd_scan_test2; +extern parse_pgm_inst_t cmd_time_monitor; +extern parse_pgm_inst_t cmd_scanner; +extern parse_pgm_inst_t cmd_build_z1; +#ifdef TEST_BEACON +extern parse_pgm_inst_t cmd_beacon_opp_dump; +#endif +extern parse_pgm_inst_t cmd_test; + +/* commands_traj.c */ +extern parse_pgm_inst_t cmd_traj_speed; +extern parse_pgm_inst_t cmd_traj_speed_show; +extern parse_pgm_inst_t cmd_trajectory; +extern parse_pgm_inst_t cmd_trajectory_show; +extern parse_pgm_inst_t cmd_circle_coef; +extern parse_pgm_inst_t cmd_circle_coef_show; +extern parse_pgm_inst_t cmd_rs_gains; +extern parse_pgm_inst_t cmd_rs_gains_show; +extern parse_pgm_inst_t cmd_track; +extern parse_pgm_inst_t cmd_track_show; +extern parse_pgm_inst_t cmd_pt_list; +extern parse_pgm_inst_t cmd_pt_list_append; +extern parse_pgm_inst_t cmd_pt_list_del; +extern parse_pgm_inst_t cmd_pt_list_show; +extern parse_pgm_inst_t cmd_goto1; +extern parse_pgm_inst_t cmd_goto2; +extern parse_pgm_inst_t cmd_goto3; +extern parse_pgm_inst_t cmd_position; +extern parse_pgm_inst_t cmd_position_set; +extern parse_pgm_inst_t cmd_strat_infos; +extern parse_pgm_inst_t cmd_strat_conf; +extern parse_pgm_inst_t cmd_strat_conf2; +extern parse_pgm_inst_t cmd_strat_conf3; +extern parse_pgm_inst_t cmd_strat_conf4; +extern parse_pgm_inst_t cmd_subtraj; + +/* in progmem */ +parse_pgm_ctx_t main_ctx[] = { + + /* commands_gen.c */ + (parse_pgm_inst_t *)&cmd_reset, + (parse_pgm_inst_t *)&cmd_bootloader, + (parse_pgm_inst_t *)&cmd_encoders, + (parse_pgm_inst_t *)&cmd_pwm, + (parse_pgm_inst_t *)&cmd_adc, + (parse_pgm_inst_t *)&cmd_sensor, + (parse_pgm_inst_t *)&cmd_log, + (parse_pgm_inst_t *)&cmd_log_show, + (parse_pgm_inst_t *)&cmd_log_type, + (parse_pgm_inst_t *)&cmd_stack_space, + (parse_pgm_inst_t *)&cmd_scheduler, + + /* commands_ax12.c */ +/* (parse_pgm_inst_t *)&cmd_baudrate, */ +/* (parse_pgm_inst_t *)&cmd_uint16_read, */ +/* (parse_pgm_inst_t *)&cmd_uint16_write, */ +/* (parse_pgm_inst_t *)&cmd_uint8_read, */ +/* (parse_pgm_inst_t *)&cmd_uint8_write, */ + + /* commands_cs.c */ + (parse_pgm_inst_t *)&cmd_gain, + (parse_pgm_inst_t *)&cmd_gain_show, + (parse_pgm_inst_t *)&cmd_speed, + (parse_pgm_inst_t *)&cmd_speed_show, + (parse_pgm_inst_t *)&cmd_consign, + (parse_pgm_inst_t *)&cmd_derivate_filter, + (parse_pgm_inst_t *)&cmd_derivate_filter_show, + (parse_pgm_inst_t *)&cmd_maximum, + (parse_pgm_inst_t *)&cmd_maximum_show, + (parse_pgm_inst_t *)&cmd_quadramp, + (parse_pgm_inst_t *)&cmd_quadramp_show, + (parse_pgm_inst_t *)&cmd_cs_status, + (parse_pgm_inst_t *)&cmd_blocking_i, + (parse_pgm_inst_t *)&cmd_blocking_i_show, + + /* commands_mainboard.c */ + (parse_pgm_inst_t *)&cmd_event, + (parse_pgm_inst_t *)&cmd_spi_test, + (parse_pgm_inst_t *)&cmd_opponent, + (parse_pgm_inst_t *)&cmd_opponent_set, + (parse_pgm_inst_t *)&cmd_start, + (parse_pgm_inst_t *)&cmd_interact, + (parse_pgm_inst_t *)&cmd_color, + (parse_pgm_inst_t *)&cmd_rs, + (parse_pgm_inst_t *)&cmd_i2cdebug, + (parse_pgm_inst_t *)&cmd_mechboard_show, + (parse_pgm_inst_t *)&cmd_mechboard_setmode1, + (parse_pgm_inst_t *)&cmd_mechboard_setmode2, + (parse_pgm_inst_t *)&cmd_mechboard_setmode3, + (parse_pgm_inst_t *)&cmd_mechboard_setmode4, + (parse_pgm_inst_t *)&cmd_mechboard_setmode5, + (parse_pgm_inst_t *)&cmd_pickup_wheels, + (parse_pgm_inst_t *)&cmd_beacon_start, + (parse_pgm_inst_t *)&cmd_pump_current, + (parse_pgm_inst_t *)&cmd_build_test, + (parse_pgm_inst_t *)&cmd_column_test, + (parse_pgm_inst_t *)&cmd_column_test2, + (parse_pgm_inst_t *)&cmd_lintel_test, + (parse_pgm_inst_t *)&cmd_pickup_test, + (parse_pgm_inst_t *)&cmd_scan_test, + (parse_pgm_inst_t *)&cmd_scan_test2, + (parse_pgm_inst_t *)&cmd_time_monitor, + (parse_pgm_inst_t *)&cmd_scanner, + (parse_pgm_inst_t *)&cmd_build_z1, +#ifdef TEST_BEACON + (parse_pgm_inst_t *)&cmd_beacon_opp_dump, +#endif + (parse_pgm_inst_t *)&cmd_test, + + /* commands_traj.c */ + (parse_pgm_inst_t *)&cmd_traj_speed, + (parse_pgm_inst_t *)&cmd_traj_speed_show, + (parse_pgm_inst_t *)&cmd_trajectory, + (parse_pgm_inst_t *)&cmd_trajectory_show, + (parse_pgm_inst_t *)&cmd_circle_coef, + (parse_pgm_inst_t *)&cmd_circle_coef_show, + (parse_pgm_inst_t *)&cmd_rs_gains, + (parse_pgm_inst_t *)&cmd_rs_gains_show, + (parse_pgm_inst_t *)&cmd_track, + (parse_pgm_inst_t *)&cmd_track_show, + (parse_pgm_inst_t *)&cmd_pt_list, + (parse_pgm_inst_t *)&cmd_pt_list_append, + (parse_pgm_inst_t *)&cmd_pt_list_del, + (parse_pgm_inst_t *)&cmd_pt_list_show, + (parse_pgm_inst_t *)&cmd_goto1, + (parse_pgm_inst_t *)&cmd_goto2, + (parse_pgm_inst_t *)&cmd_position, + (parse_pgm_inst_t *)&cmd_position_set, + (parse_pgm_inst_t *)&cmd_strat_infos, + (parse_pgm_inst_t *)&cmd_strat_conf, + (parse_pgm_inst_t *)&cmd_strat_conf2, + (parse_pgm_inst_t *)&cmd_strat_conf3, + (parse_pgm_inst_t *)&cmd_strat_conf4, + (parse_pgm_inst_t *)&cmd_subtraj, + NULL, +}; diff --git a/projects/microb2010/tests/hostsim/commands_cs.c b/projects/microb2010/tests/hostsim/commands_cs.c new file mode 100644 index 0000000..c9e9729 --- /dev/null +++ b/projects/microb2010/tests/hostsim/commands_cs.c @@ -0,0 +1,677 @@ +/* + * 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: commands_cs.c,v 1.4 2009-05-02 10:08:09 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 +#include +#include + +#include "main.h" +#include "cs.h" +#include "cmdline.h" + +struct csb_list { + const prog_char *name; + struct cs_block *csb; +}; + +prog_char csb_angle_str[] = "angle"; +prog_char csb_distance_str[] = "distance"; +struct csb_list csb_list[] = { + { .name = csb_angle_str, .csb = &mainboard.angle }, + { .name = csb_distance_str, .csb = &mainboard.distance }, +}; + +struct cmd_cs_result { + fixed_string_t cmdname; + fixed_string_t csname; +}; + +/* token to be used for all cs-related commands */ +prog_char str_csb_name[] = "angle#distance"; +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) +{ + int i; + + for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) { + if (!strcmp_P(name, csb_list[i].name)) + return csb_list[i].csb; + } + return NULL; +} + +/**********************************************************/ +/* Gains for control system */ + +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_result { + struct cmd_cs_result cs; + int16_t p; + int16_t i; + int16_t d; +}; + +/* function called when cmd_gain is parsed successfully */ +static void cmd_gain_parsed(void * parsed_result, void *show) +{ + struct cmd_gain_result *res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_gains(&csb->pid, res->p, res->i, res->d); + + printf_P(PSTR("%s %s %d %d %d\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_gain_P(&csb->pid), + pid_get_gain_I(&csb->pid), + pid_get_gain_D(&csb->pid)); +} + +prog_char str_gain_arg0[] = "gain"; +parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0); +parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16); +parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16); +parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16); + +prog_char help_gain[] = "Set gain values for PID"; +parse_pgm_inst_t cmd_gain = { + .f = cmd_gain_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_gain, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_p, + (prog_void *)&cmd_gain_i, + (prog_void *)&cmd_gain_d, + NULL, + }, +}; + +/* show */ +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_gain_show_arg[] = "show"; +parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg); + +prog_char help_gain_show[] = "Show gain values for PID"; +parse_pgm_inst_t cmd_gain_show = { + .f = cmd_gain_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_gain_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Speeds for control system */ + +/* this structure is filled when cmd_speed is parsed successfully */ +struct cmd_speed_result { + struct cmd_cs_result cs; + uint16_t s; +}; + +/* function called when cmd_speed is parsed successfully */ +static void cmd_speed_parsed(void *parsed_result, void *show) +{ + struct cmd_speed_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + +#if notyet + if (!show) + ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */ + + printf_P(PSTR("%s %"PRIu32"\r\n"), + res->cs.csname, + ext.r_b.var_pos); +#else + printf_P(PSTR("TODO\r\n")); +#endif +} + +prog_char str_speed_arg0[] = "speed"; +parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0); +parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16); + +prog_char help_speed[] = "Set speed values for ramp filter"; +parse_pgm_inst_t cmd_speed = { + .f = cmd_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_s, + NULL, + }, +}; + +/* show */ +struct cmd_speed_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg); + +prog_char help_speed_show[] = "Show speed values for ramp filter"; +parse_pgm_inst_t cmd_speed_show = { + .f = cmd_speed_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Derivate_Filters for control system */ + +/* this structure is filled when cmd_derivate_filter is parsed successfully */ +struct cmd_derivate_filter_result { + struct cmd_cs_result cs; + uint8_t size; +}; + +/* function called when cmd_derivate_filter is parsed successfully */ +static void cmd_derivate_filter_parsed(void *parsed_result, void *show) +{ + struct cmd_derivate_filter_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_derivate_filter(&csb->pid, res->size); + + printf_P(PSTR("%s %s %u\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_derivate_filter(&csb->pid)); +} + +prog_char str_derivate_filter_arg0[] = "derivate_filter"; +parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0); +parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32); + +prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)"; +parse_pgm_inst_t cmd_derivate_filter = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_derivate_filter, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_size, + NULL, + }, +}; + +/* show */ + +struct cmd_derivate_filter_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_derivate_filter_show_arg[] = "show"; +parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg); + +prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID"; +parse_pgm_inst_t cmd_derivate_filter_show = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_derivate_filter_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_show_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Consign for control system */ + +/* this structure is filled when cmd_consign is parsed successfully */ +struct cmd_consign_result { + struct cmd_cs_result cs; + int32_t p; +}; + +/* function called when cmd_consign is parsed successfully */ +static void cmd_consign_parsed(void * parsed_result, void *data) +{ + struct cmd_consign_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + cs_set_consign(&csb->cs, res->p); +} + +prog_char str_consign_arg0[] = "consign"; +parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0); +parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32); + +prog_char help_consign[] = "Set consign value"; +parse_pgm_inst_t cmd_consign = { + .f = cmd_consign_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_consign, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_consign_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_consign_p, + NULL, + }, +}; + + +/**********************************************************/ +/* Maximums for control system */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_result { + struct cmd_cs_result cs; + uint32_t in; + uint32_t i; + uint32_t out; +}; + +/* function called when cmd_maximum is parsed successfully */ +static void cmd_maximum_parsed(void *parsed_result, void *show) +{ + struct cmd_maximum_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_maximums(&csb->pid, res->in, res->i, res->out); + + printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"), + res->cs.csname, + pid_get_max_in(&csb->pid), + pid_get_max_I(&csb->pid), + pid_get_max_out(&csb->pid)); +} + +prog_char str_maximum_arg0[] = "maximum"; +parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0); +parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32); +parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32); +parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32); + +prog_char help_maximum[] = "Set maximum values for PID (in, I, out)"; +parse_pgm_inst_t cmd_maximum = { + .f = cmd_maximum_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_maximum, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_in, + (prog_void *)&cmd_maximum_i, + (prog_void *)&cmd_maximum_out, + NULL, + }, +}; + +/* show */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; +prog_char str_maximum_show_arg[] = "show"; +parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg); + +prog_char help_maximum_show[] = "Show maximum values for PID"; +parse_pgm_inst_t cmd_maximum_show = { + .f = cmd_maximum_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_maximum_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Quadramp for control system */ + +/* this structure is filled when cmd_quadramp is parsed successfully */ +struct cmd_quadramp_result { + struct cmd_cs_result cs; + uint32_t ap; + uint32_t an; + uint32_t sp; + uint32_t sn; +}; + +/* function called when cmd_quadramp is parsed successfully */ +static void cmd_quadramp_parsed(void *parsed_result, void *show) +{ + struct cmd_quadramp_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) { + quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn); + quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); + } + + printf_P(PSTR("quadramp %s %"PRIi32" %"PRIi32" %"PRIi32" %"PRIi32"\r\n"), + res->cs.csname, + csb->qr.var_2nd_ord_pos, + csb->qr.var_2nd_ord_neg, + csb->qr.var_1st_ord_pos, + csb->qr.var_1st_ord_neg); +} + +prog_char str_quadramp_arg0[] = "quadramp"; +parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); + +prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; +parse_pgm_inst_t cmd_quadramp = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_quadramp, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_ap, + (prog_void *)&cmd_quadramp_an, + (prog_void *)&cmd_quadramp_sp, + (prog_void *)&cmd_quadramp_sn, + + NULL, + }, +}; + +/* show */ + +struct cmd_quadramp_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_quadramp_show_arg[] = "show"; +parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg); + +prog_char help_quadramp_show[] = "Get quadramp values for control system"; +parse_pgm_inst_t cmd_quadramp_show = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_quadramp_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* cs_status show for control system */ + +/* this structure is filled when cmd_cs_status is parsed successfully */ +struct cmd_cs_status_result { + struct cmd_cs_result cs; + fixed_string_t arg; +}; + +/* function called when cmd_cs_status is parsed successfully */ +static void cmd_cs_status_parsed(void *parsed_result, void *data) +{ + struct cmd_cs_status_result *res = parsed_result; + struct cs_block *csb; + uint8_t loop = 0; + uint8_t print_pid = 0, print_cs = 0; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + if (strcmp_P(res->arg, PSTR("on")) == 0) { + csb->on = 1; + printf_P(PSTR("%s is on\r\n"), res->cs.csname); + return; + } + else if (strcmp_P(res->arg, PSTR("off")) == 0) { + csb->on = 0; + printf_P(PSTR("%s is off\r\n"), res->cs.csname); + return; + } + else if (strcmp_P(res->arg, PSTR("show")) == 0) { + print_cs = 1; + } + else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) { + loop = 1; + print_cs = 1; + } + else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) { + print_pid = 1; + } + else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) { + print_pid = 1; + loop = 1; + } + + printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off"); + do { + if (print_cs) + dump_cs(res->cs.csname, &csb->cs); + if (print_pid) + dump_pid(res->cs.csname, &csb->pid); + wait_ms(100); + } while(loop && !cmdline_keypressed()); +} + +prog_char str_cs_status_arg0[] = "cs_status"; +parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0); +prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off"; +parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg); + +prog_char help_cs_status[] = "Show cs status"; +parse_pgm_inst_t cmd_cs_status = { + .f = cmd_cs_status_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_cs_status, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_cs_status_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_cs_status_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Blocking_I for control system */ + +/* this structure is filled when cmd_blocking_i is parsed successfully */ +struct cmd_blocking_i_result { + struct cmd_cs_result cs; + int32_t k1; + int32_t k2; + uint32_t i; + uint16_t cpt; +}; + +/* function called when cmd_blocking_i is parsed successfully */ +static void cmd_blocking_i_parsed(void *parsed_result, void *show) +{ + struct cmd_blocking_i_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + bd_set_current_thresholds(&csb->bd, res->k1, res->k2, + res->i, res->cpt); + + printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"), + res->cs.cmdname, + res->cs.csname, + csb->bd.k1, + csb->bd.k2, + csb->bd.i_thres, + csb->bd.cpt_thres); +} + +prog_char str_blocking_i_arg0[] = "blocking"; +parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0); +parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32); +parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32); +parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32); +parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16); + +prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)"; +parse_pgm_inst_t cmd_blocking_i = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_blocking_i, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_k1, + (prog_void *)&cmd_blocking_i_k2, + (prog_void *)&cmd_blocking_i_i, + (prog_void *)&cmd_blocking_i_cpt, + NULL, + }, +}; + +/* show */ + +struct cmd_blocking_i_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_blocking_i_show_arg[] = "show"; +parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg); + +prog_char help_blocking_i_show[] = "Show blocking detection values"; +parse_pgm_inst_t cmd_blocking_i_show = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_blocking_i_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_show_arg, + NULL, + }, +}; + + diff --git a/projects/microb2010/tests/hostsim/commands_gen.c b/projects/microb2010/tests/hostsim/commands_gen.c new file mode 100644 index 0000000..92f8969 --- /dev/null +++ b/projects/microb2010/tests/hostsim/commands_gen.c @@ -0,0 +1,609 @@ +/* + * 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: commands_gen.c,v 1.8 2009-11-08 17:24:33 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 +#include +#include +#include + +#include "main.h" +#include "cmdline.h" + +/**********************************************************/ +/* Reset */ + +/* this structure is filled when cmd_reset is parsed successfully */ +struct cmd_reset_result { + fixed_string_t arg0; +}; + +/* function called when cmd_reset is parsed successfully */ +static void cmd_reset_parsed(void * parsed_result, void * data) +{ +#ifdef HOST_VERSION + hostsim_exit(); +#endif + reset(); +} + +prog_char str_reset_arg0[] = "reset"; +parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0); + +prog_char help_reset[] = "Reset the board"; +parse_pgm_inst_t cmd_reset = { + .f = cmd_reset_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_reset, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_reset_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Bootloader */ + +/* this structure is filled when cmd_bootloader is parsed successfully */ +struct cmd_bootloader_result { + fixed_string_t arg0; +}; + +/* function called when cmd_bootloader is parsed successfully */ +static void cmd_bootloader_parsed(void *parsed_result, void *data) +{ +#ifndef HOST_VERSION + bootloader(); +#else + printf("not implemented\n"); +#endif +} + +prog_char str_bootloader_arg0[] = "bootloader"; +parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0); + +prog_char help_bootloader[] = "Launch the bootloader"; +parse_pgm_inst_t cmd_bootloader = { + .f = cmd_bootloader_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_bootloader, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_bootloader_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Encoders tests */ + +/* this structure is filled when cmd_encoders is parsed successfully */ +struct cmd_encoders_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_encoders is parsed successfully */ +static void cmd_encoders_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_encoders_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + encoders_spi_set_value((void *)0, 0); + encoders_spi_set_value((void *)1, 0); + encoders_spi_set_value((void *)2, 0); + encoders_spi_set_value((void *)3, 0); + return; + } + + /* show */ + while(!cmdline_keypressed()) { + printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"), + encoders_spi_get_value((void *)0), + encoders_spi_get_value((void *)1), + encoders_spi_get_value((void *)2), + encoders_spi_get_value((void *)3)); + wait_ms(100); + } +#endif +} + +prog_char str_encoders_arg0[] = "encoders"; +parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0); +prog_char str_encoders_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1); + +prog_char help_encoders[] = "Show encoders values"; +parse_pgm_inst_t cmd_encoders = { + .f = cmd_encoders_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_encoders, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_encoders_arg0, + (prog_void *)&cmd_encoders_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Scheduler show */ + +/* this structure is filled when cmd_scheduler is parsed successfully */ +struct cmd_scheduler_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scheduler is parsed successfully */ +static void cmd_scheduler_parsed(void *parsed_result, void *data) +{ + scheduler_dump_events(); + scheduler_stats_dump(); +} + +prog_char str_scheduler_arg0[] = "scheduler"; +parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0); +prog_char str_scheduler_arg1[] = "show"; +parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1); + +prog_char help_scheduler[] = "Show scheduler events"; +parse_pgm_inst_t cmd_scheduler = { + .f = cmd_scheduler_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scheduler, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scheduler_arg0, + (prog_void *)&cmd_scheduler_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Pwms tests */ + +/* this structure is filled when cmd_pwm is parsed successfully */ +struct cmd_pwm_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_pwm is parsed successfully */ +static void cmd_pwm_parsed(void * parsed_result, void * data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + void * pwm_ptr = NULL; + struct cmd_pwm_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("1(4A)"))) + pwm_ptr = &gen.pwm1_4A; + else if (!strcmp_P(res->arg1, PSTR("2(4B)"))) + pwm_ptr = &gen.pwm2_4B; + else if (!strcmp_P(res->arg1, PSTR("3(1A)"))) + pwm_ptr = &gen.pwm3_1A; + else if (!strcmp_P(res->arg1, PSTR("4(1B)"))) + pwm_ptr = &gen.pwm4_1B; + + else if (!strcmp_P(res->arg1, PSTR("s1(3C)"))) + pwm_ptr = &gen.servo1; + else if (!strcmp_P(res->arg1, PSTR("s2(5A)"))) + pwm_ptr = &gen.servo2; + else if (!strcmp_P(res->arg1, PSTR("s3(5B)"))) + pwm_ptr = &gen.servo3; + else if (!strcmp_P(res->arg1, PSTR("s3(5C)"))) + pwm_ptr = &gen.servo4; + + if (pwm_ptr) + pwm_ng_set(pwm_ptr, res->arg2); + + printf_P(PSTR("done\r\n")); +#endif +} + +prog_char str_pwm_arg0[] = "pwm"; +parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0); +prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)"; +parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1); +parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16); + +prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]"; +parse_pgm_inst_t cmd_pwm = { + .f = cmd_pwm_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pwm, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pwm_arg0, + (prog_void *)&cmd_pwm_arg1, + (prog_void *)&cmd_pwm_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Adcs tests */ + +/* this structure is filled when cmd_adc is parsed successfully */ +struct cmd_adc_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_adc is parsed successfully */ +static void cmd_adc_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_adc_result *res = parsed_result; + uint8_t i, loop = 0; + + if (!strcmp_P(res->arg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("ADC values: ")); + for (i=0; iarg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("SENSOR values: ")); + for (i=0; iarg1, PSTR("level"))) { + gen.log_level = res->arg2; + } + + /* else it is a show */ + cmd_log_do_show(); +} + +prog_char str_log_arg0[] = "log"; +parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0); +prog_char str_log_arg1[] = "level"; +parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1); +parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32); + +prog_char help_log[] = "Set log options: level (0 -> 5)"; +parse_pgm_inst_t cmd_log = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1, + (prog_void *)&cmd_log_arg2, + NULL, + }, +}; + +prog_char str_log_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show); + +prog_char help_log_show[] = "Show configured logs"; +parse_pgm_inst_t cmd_log_show = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1_show, + NULL, + }, +}; + +/* this structure is filled when cmd_log is parsed successfully */ +struct cmd_log_type_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_log is parsed successfully */ +static void cmd_log_type_parsed(void * parsed_result, void * data) +{ + struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result; + uint8_t lognum; + uint8_t i; + + lognum = log_name2num(res->arg2); + if (lognum == 0) { + printf_P(PSTR("Cannot find log num\r\n")); + return; + } + + if (!strcmp_P(res->arg3, PSTR("on"))) { + for (i=0; iarg3, PSTR("off"))) { + for (i=0; i + */ + +#include +#include + +#include +#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 "actuator.h" +#include "robotsim.h" +#include "cmdline.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" + +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, 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_RS | DO_POS | + DO_BD | DO_TIMER | DO_POWER; + if (!strcmp_P(res->arg2, PSTR("on"))) + mainboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) + mainboard.flags &= bit; + else { /* show */ + printf_P(PSTR("encoders is %s\r\n"), + (DO_ENCODERS & mainboard.flags) ? "on":"off"); + printf_P(PSTR("cs is %s\r\n"), + (DO_CS & mainboard.flags) ? "on":"off"); + printf_P(PSTR("rs is %s\r\n"), + (DO_RS & mainboard.flags) ? "on":"off"); + printf_P(PSTR("pos is %s\r\n"), + (DO_POS & mainboard.flags) ? "on":"off"); + printf_P(PSTR("bd is %s\r\n"), + (DO_BD & mainboard.flags) ? "on":"off"); + printf_P(PSTR("timer is %s\r\n"), + (DO_TIMER & mainboard.flags) ? "on":"off"); + printf_P(PSTR("power is %s\r\n"), + (DO_POWER & mainboard.flags) ? "on":"off"); + } + return; + } + + 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("rs"))) + bit = DO_RS; + else if (!strcmp_P(res->arg1, PSTR("pos"))) + bit = DO_POS; + else if (!strcmp_P(res->arg1, PSTR("bd"))) + bit = DO_BD; + else if (!strcmp_P(res->arg1, PSTR("timer"))) { + time_reset(); + bit = DO_TIMER; + } + else if (!strcmp_P(res->arg1, PSTR("power"))) + bit = DO_POWER; + + if (!strcmp_P(res->arg2, PSTR("on"))) + mainboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) { + if (!strcmp_P(res->arg1, PSTR("cs"))) { +#ifdef HOST_VERSION + robotsim_pwm(LEFT_PWM, 0); + robotsim_pwm(RIGHT_PWM, 0); +#else + pwm_ng_set(LEFT_PWM, 0); + pwm_ng_set(RIGHT_PWM, 0); +#endif + } + mainboard.flags &= (~bit); + } + printf_P(PSTR("%s is %s\r\n"), res->arg1, + (bit & mainboard.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#rs#pos#bd#timer#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, + }, +}; + + +/**********************************************************/ +/* Spi_Test */ + +/* this structure is filled when cmd_spi_test is parsed successfully */ +struct cmd_spi_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_spi_test is parsed successfully */ +static void cmd_spi_test_parsed(void * parsed_result, void * data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + uint16_t i = 0, ret = 0, ret2 = 0; + + if (mainboard.flags & DO_ENCODERS) { + printf_P(PSTR("Disable encoder event first\r\n")); + return; + } + + do { + spi_slave_select(0); + ret = spi_send_and_receive_byte(i); + ret2 = spi_send_and_receive_byte(i); + spi_slave_deselect(0); + + if ((i & 0x7ff) == 0) + printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"), + i, ret, ret2); + + i++; + } while(!cmdline_keypressed()); +#endif +} + +prog_char str_spi_test_arg0[] = "spi_test"; +parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0); + +prog_char help_spi_test[] = "Test the SPI"; +parse_pgm_inst_t cmd_spi_test = { + .f = cmd_spi_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_spi_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_spi_test_arg0, + NULL, + }, +}; + + + +/**********************************************************/ +/* Opponent tests */ + +/* this structure is filled when cmd_opponent is parsed successfully */ +struct cmd_opponent_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; +}; + +/* function called when cmd_opponent is parsed successfully */ +static void cmd_opponent_parsed(void *parsed_result, void *data) +{ + int16_t x,y,d,a; + + if (get_opponent_xyda(&x, &y, &d, &a)) + printf_P(PSTR("No opponent\r\n")); + else + printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a); +} + +prog_char str_opponent_arg0[] = "opponent"; +parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0); +prog_char str_opponent_arg1[] = "show"; +parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1); + +prog_char help_opponent[] = "Show (x,y) opponent"; +parse_pgm_inst_t cmd_opponent = { + .f = cmd_opponent_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_opponent, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1, + NULL, + }, +}; + + +prog_char str_opponent_arg1_set[] = "set"; +parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set); +parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32); +parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32); + +prog_char help_opponent_set[] = "Set (x,y) opponent"; +parse_pgm_inst_t cmd_opponent_set = { + .f = cmd_opponent_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_opponent_set, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1_set, + (prog_void *)&cmd_opponent_arg2, + (prog_void *)&cmd_opponent_arg3, + NULL, + }, +}; + + +/**********************************************************/ +/* Start */ + +/* this structure is filled when cmd_start is parsed successfully */ +struct cmd_start_result { + fixed_string_t arg0; + fixed_string_t color; + fixed_string_t debug; +}; + +/* function called when cmd_start is parsed successfully */ +static void cmd_start_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_start_result *res = parsed_result; + uint8_t old_level = gen.log_level; + + gen.logs[NB_LOGS] = E_USER_STRAT; + if (!strcmp_P(res->debug, PSTR("debug"))) { + strat_infos.dump_enabled = 1; + gen.log_level = 5; + } + else { + strat_infos.dump_enabled = 0; + gen.log_level = 0; + } + + if (!strcmp_P(res->color, PSTR("red"))) { + mainboard.our_color = I2C_COLOR_RED; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + } + else if (!strcmp_P(res->color, PSTR("green"))) { + mainboard.our_color = I2C_COLOR_GREEN; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + } + + printf_P(PSTR("Check that lintel is loaded\r\n")); + while(!cmdline_keypressed()); + + printf_P(PSTR("Press a key when beacon ready\r\n")); + i2c_sensorboard_set_beacon(0); + while(!cmdline_keypressed()); + i2c_sensorboard_set_beacon(1); + + strat_start(); + + gen.logs[NB_LOGS] = 0; + gen.log_level = old_level; +#endif +} + +prog_char str_start_arg0[] = "start"; +parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0); +prog_char str_start_color[] = "green#red"; +parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color); +prog_char str_start_debug[] = "debug#match"; +parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug); + +prog_char help_start[] = "Start the robot"; +parse_pgm_inst_t cmd_start = { + .f = cmd_start_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_start, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_start_arg0, + (prog_void *)&cmd_start_color, + (prog_void *)&cmd_start_debug, + NULL, + }, +}; + + + +/**********************************************************/ +/* Interact */ + +/* this structure is filled when cmd_interact is parsed successfully */ +struct cmd_interact_result { + fixed_string_t arg0; +}; + +static void print_cs(void) +{ + printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" " + "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), + cs_get_consign(&mainboard.distance.cs), + cs_get_consign(&mainboard.angle.cs), + cs_get_filtered_consign(&mainboard.distance.cs), + cs_get_filtered_consign(&mainboard.angle.cs), + cs_get_error(&mainboard.distance.cs), + cs_get_error(&mainboard.angle.cs), + cs_get_out(&mainboard.distance.cs), + cs_get_out(&mainboard.angle.cs)); +} + +static void print_pos(void) +{ + printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); +} + +static void print_time(void) +{ + printf_P(PSTR("time %d\r\n"),time_get_s()); +} + + +static void print_sensors(void) +{ +#ifdef notyet + if (sensor_start_switch()) + printf_P(PSTR("Start switch | ")); + else + printf_P(PSTR(" | ")); + + if (IR_DISP_SENSOR()) + printf_P(PSTR("IR disp | ")); + else + printf_P(PSTR(" | ")); + + printf_P(PSTR("\r\n")); +#endif +} + +static void print_pid(void) +{ + printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | " + "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"), + pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid), + pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid), + pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid), + pid_get_value_out(&mainboard.distance.pid), + pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid), + pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid), + pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid), + pid_get_value_out(&mainboard.angle.pid)); +} + +#define PRINT_POS (1<<0) +#define PRINT_PID (1<<1) +#define PRINT_CS (1<<2) +#define PRINT_SENSORS (1<<3) +#define PRINT_TIME (1<<4) +#define PRINT_BLOCKING (1<<5) + +static void cmd_interact_parsed(void * parsed_result, void * data) +{ + int c; + int8_t cmd; + uint8_t print = 0; + struct vt100 vt100; + + vt100_init(&vt100); + + printf_P(PSTR("Display debugs:\r\n" + " 1:pos\r\n" + " 2:pid\r\n" + " 3:cs\r\n" + " 4:sensors\r\n" + " 5:time\r\n" + /* " 6:blocking\r\n" */ + "Commands:\r\n" + " arrows:move\r\n" + " space:stop\r\n" + " q:quit\r\n")); + + /* stop motors */ + mainboard.flags &= (~DO_CS); + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + + while(1) { + if (print & PRINT_POS) { + print_pos(); + } + + if (print & PRINT_PID) { + print_pid(); + } + + if (print & PRINT_CS) { + print_cs(); + } + + if (print & PRINT_SENSORS) { + print_sensors(); + } + + if (print & PRINT_TIME) { + print_time(); + } +/* if (print & PRINT_BLOCKING) { */ +/* printf_P(PSTR("%s %s blocking=%d\r\n"), */ +/* mainboard.blocking ? "BLOCK1":" ", */ +/* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */ +/* rs_get_blocking(&mainboard.rs)); */ +/* } */ + + c = cmdline_getchar(); + if (c == -1) { + wait_ms(10); + continue; + } + cmd = vt100_parser(&vt100, c); + if (cmd == -2) { + wait_ms(10); + continue; + } + + if (cmd == -1) { + switch(c) { + case '1': print ^= PRINT_POS; break; + case '2': print ^= PRINT_PID; break; + case '3': print ^= PRINT_CS; break; + case '4': print ^= PRINT_SENSORS; break; + case '5': print ^= PRINT_TIME; break; + case '6': print ^= PRINT_BLOCKING; break; + + case 'q': + if (mainboard.flags & DO_CS) + strat_hardstop(); + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + return; + case ' ': + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + break; + default: + break; + } + } + else { + switch(cmd) { + case KEY_UP_ARR: + pwm_set_and_save(LEFT_PWM, 1200); + pwm_set_and_save(RIGHT_PWM, 1200); + break; + case KEY_LEFT_ARR: + pwm_set_and_save(LEFT_PWM, -1200); + pwm_set_and_save(RIGHT_PWM, 1200); + break; + case KEY_DOWN_ARR: + pwm_set_and_save(LEFT_PWM, -1200); + pwm_set_and_save(RIGHT_PWM, -1200); + break; + case KEY_RIGHT_ARR: + pwm_set_and_save(LEFT_PWM, 1200); + pwm_set_and_save(RIGHT_PWM, -1200); + break; + } + } + wait_ms(10); + } +} + +prog_char str_interact_arg0[] = "interact"; +parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0); + +prog_char help_interact[] = "Interactive mode"; +parse_pgm_inst_t cmd_interact = { + .f = cmd_interact_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_interact, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_interact_arg0, + 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, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; + if (!strcmp_P(res->color, PSTR("red"))) { + mainboard.our_color = I2C_COLOR_RED; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + } + else if (!strcmp_P(res->color, PSTR("green"))) { + mainboard.our_color = I2C_COLOR_GREEN; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + } + printf_P(PSTR("Done\r\n")); +#endif +} + +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, + }, +}; + + +/**********************************************************/ +/* Rs tests */ + +/* this structure is filled when cmd_rs is parsed successfully */ +struct cmd_rs_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_rs is parsed successfully */ +static void cmd_rs_parsed(void *parsed_result, void *data) +{ + // struct cmd_rs_result *res = parsed_result; + do { + printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + cs_get_consign(&mainboard.angle.cs), + cs_get_filtered_feedback(&mainboard.angle.cs), + cs_get_out(&mainboard.angle.cs)); + printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + cs_get_consign(&mainboard.distance.cs), + cs_get_filtered_feedback(&mainboard.distance.cs), + cs_get_out(&mainboard.distance.cs)); + printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l, + mainboard.pwm_r); + wait_ms(100); + } while(!cmdline_keypressed()); +} + +prog_char str_rs_arg0[] = "rs"; +parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0); +prog_char str_rs_arg1[] = "show"; +parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1); + +prog_char help_rs[] = "Show rs (robot system) values"; +parse_pgm_inst_t cmd_rs = { + .f = cmd_rs_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_arg0, + (prog_void *)&cmd_rs_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* I2cdebug */ + +/* this structure is filled when cmd_i2cdebug is parsed successfully */ +struct cmd_i2cdebug_result { + fixed_string_t arg0; +}; + +/* function called when cmd_i2cdebug is parsed successfully */ +static void cmd_i2cdebug_parsed(void * parsed_result, void * data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + i2c_debug(); + i2c_protocol_debug(); +#endif +} + +prog_char str_i2cdebug_arg0[] = "i2cdebug"; +parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0); + +prog_char help_i2cdebug[] = "I2c debug infos"; +parse_pgm_inst_t cmd_i2cdebug = { + .f = cmd_i2cdebug_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_i2cdebug, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_i2cdebug_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Show */ + +/* this structure is filled when cmd_mechboard_show is parsed successfully */ +struct cmd_mechboard_show_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_mechboard_show is parsed successfully */ +static void cmd_mechboard_show_parsed(void * parsed_result, void * data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + printf_P(PSTR("mode = %x\r\n"), mechboard.mode); + printf_P(PSTR("status = %x\r\n"), mechboard.status); + printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count); + + printf_P(PSTR("column_count = %d\r\n"), get_column_count()); + printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"), + pump_left1_is_full(), pump_left2_is_full(), + pump_right1_is_full(), pump_right2_is_full()); + + printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1); + printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2); + printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1); + printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2); + + printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left); + printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right); + +#endif +} + +prog_char str_mechboard_show_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0); +prog_char str_mechboard_show_arg1[] = "show"; +parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1); + +prog_char help_mechboard_show[] = "show mechboard status"; +parse_pgm_inst_t cmd_mechboard_show = { + .f = cmd_mechboard_show_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_show_arg0, + (prog_void *)&cmd_mechboard_show_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode1 */ + +/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */ +struct cmd_mechboard_setmode1_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_mechboard_setmode1 is parsed successfully */ +static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_mechboard_setmode1_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("init"))) + i2c_mechboard_mode_init(); + else if (!strcmp_P(res->arg1, PSTR("manual"))) + i2c_mechboard_mode_manual(); + else if (!strcmp_P(res->arg1, PSTR("pickup"))) + i2c_mechboard_mode_pickup(); + else if (!strcmp_P(res->arg1, PSTR("lazy_harvest"))) + i2c_mechboard_mode_lazy_harvest(); + else if (!strcmp_P(res->arg1, PSTR("harvest"))) + i2c_mechboard_mode_harvest(); + else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel"))) + i2c_mechboard_mode_prepare_get_lintel(); + else if (!strcmp_P(res->arg1, PSTR("get_lintel"))) + i2c_mechboard_mode_get_lintel(); + else if (!strcmp_P(res->arg1, PSTR("put_lintel"))) + i2c_mechboard_mode_put_lintel(); + else if (!strcmp_P(res->arg1, PSTR("init"))) + i2c_mechboard_mode_init(); + else if (!strcmp_P(res->arg1, PSTR("eject"))) + i2c_mechboard_mode_init(); + else if (!strcmp_P(res->arg1, PSTR("clear"))) + i2c_mechboard_mode_clear(); + else if (!strcmp_P(res->arg1, PSTR("loaded"))) + i2c_mechboard_mode_loaded(); + else if (!strcmp_P(res->arg1, PSTR("store"))) + i2c_mechboard_mode_store(); + else if (!strcmp_P(res->arg1, PSTR("manivelle"))) + i2c_mechboard_mode_manivelle(); + else if (!strcmp_P(res->arg1, PSTR("lazy_pickup"))) + i2c_mechboard_mode_lazy_pickup(); +#endif +} + +prog_char str_mechboard_setmode1_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0); +prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup"; +parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1); + +prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)"; +parse_pgm_inst_t cmd_mechboard_setmode1 = { + .f = cmd_mechboard_setmode1_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode1_arg0, + (prog_void *)&cmd_mechboard_setmode1_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode2 */ + +/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */ +struct cmd_mechboard_setmode2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_mechboard_setmode2 is parsed successfully */ +static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_mechboard_setmode2_result *res = parsed_result; + uint8_t side = I2C_LEFT_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 if (!strcmp_P(res->arg2, PSTR("auto"))) + side = I2C_AUTO_SIDE; + + if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) + i2c_mechboard_mode_prepare_pickup(side); + else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) + i2c_mechboard_mode_push_temple_disc(side); +#endif +} + +prog_char str_mechboard_setmode2_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0); +prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc"; +parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1); +prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center"; +parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2); + +prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)"; +parse_pgm_inst_t cmd_mechboard_setmode2 = { + .f = cmd_mechboard_setmode2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode2_arg0, + (prog_void *)&cmd_mechboard_setmode2_arg1, + (prog_void *)&cmd_mechboard_setmode2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode3 */ + +/* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */ +struct cmd_mechboard_setmode3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t level; +}; + +/* function called when cmd_mechboard_setmode3 is parsed successfully */ +static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_mechboard_setmode3_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("autobuild"))) + i2c_mechboard_mode_simple_autobuild(res->level); + else if (!strcmp_P(res->arg1, PSTR("prepare_build"))) + i2c_mechboard_mode_prepare_build_both(res->level); + else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) + i2c_mechboard_mode_prepare_inside_both(res->level); + else if (!strcmp_P(res->arg1, PSTR("push_temple"))) + i2c_mechboard_mode_push_temple(res->level); +#endif +} + +prog_char str_mechboard_setmode3_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0); +prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple"; +parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1); +parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8); + +prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)"; +parse_pgm_inst_t cmd_mechboard_setmode3 = { + .f = cmd_mechboard_setmode3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode3_arg0, + (prog_void *)&cmd_mechboard_setmode3_arg1, + (prog_void *)&cmd_mechboard_setmode3_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode4 */ + +/* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */ +struct cmd_mechboard_setmode4_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_mechboard_setmode4 is parsed successfully */ +static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_mechboard_setmode4_result *res = parsed_result; + i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l, + res->level_r, res->count_r, res->dist_r, + res->do_lintel); +#endif +} + +prog_char str_mechboard_setmode4_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0); +prog_char str_mechboard_setmode4_arg1[] = "autobuild"; +parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8); + +prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)"; +parse_pgm_inst_t cmd_mechboard_setmode4 = { + .f = cmd_mechboard_setmode4_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode4, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode4_arg0, + (prog_void *)&cmd_mechboard_setmode4_arg1, + (prog_void *)&cmd_mechboard_setmode4_arg2, + (prog_void *)&cmd_mechboard_setmode4_arg3, + (prog_void *)&cmd_mechboard_setmode4_arg4, + (prog_void *)&cmd_mechboard_setmode4_arg5, + (prog_void *)&cmd_mechboard_setmode4_arg6, + (prog_void *)&cmd_mechboard_setmode4_arg7, + (prog_void *)&cmd_mechboard_setmode4_arg8, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode5 */ + +/* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */ +struct cmd_mechboard_setmode5_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_mechboard_setmode5 is parsed successfully */ +static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_mechboard_setmode5_result *res = parsed_result; + uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + + 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 if (!strcmp_P(res->arg2, PSTR("auto"))) + side = I2C_AUTO_SIDE; + + if (!strcmp_P(res->arg3, PSTR("harvest"))) + next_mode = I2C_MECHBOARD_MODE_HARVEST; + else if (!strcmp_P(res->arg3, PSTR("lazy_harvest"))) + next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; + else if (!strcmp_P(res->arg3, PSTR("pickup"))) + next_mode = I2C_MECHBOARD_MODE_PICKUP; + else if (!strcmp_P(res->arg3, PSTR("clear"))) + next_mode = I2C_MECHBOARD_MODE_CLEAR; + else if (!strcmp_P(res->arg3, PSTR("store"))) + next_mode = I2C_MECHBOARD_MODE_STORE; + else if (!strcmp_P(res->arg3, PSTR("lazy_pickup"))) + next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; + + if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) + i2c_mechboard_mode_prepare_pickup_next(side, next_mode); +#endif +} + +prog_char str_mechboard_setmode5_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0); +prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1); +prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2); +prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3); + +prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)"; +parse_pgm_inst_t cmd_mechboard_setmode5 = { + .f = cmd_mechboard_setmode5_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode5, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode5_arg0, + (prog_void *)&cmd_mechboard_setmode5_arg1, + (prog_void *)&cmd_mechboard_setmode5_arg2, + (prog_void *)&cmd_mechboard_setmode5_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* pickup wheels */ + +/* this structure is filled when cmd_pickup_wheels is parsed successfully */ +struct cmd_pickup_wheels_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_pickup_wheels is parsed successfully */ +static void cmd_pickup_wheels_parsed(void *parsed_result, void *data) +{ + struct cmd_pickup_wheels_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("on"))) + pickup_wheels_on(); + else + pickup_wheels_off(); +} + +prog_char str_pickup_wheels_arg0[] = "pickup_wheels"; +parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0); +prog_char str_pickup_wheels_arg1[] = "on#off"; +parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1); + +prog_char help_pickup_wheels[] = "Enable/disable pickup wheels"; +parse_pgm_inst_t cmd_pickup_wheels = { + .f = cmd_pickup_wheels_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pickup_wheels, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pickup_wheels_arg0, + (prog_void *)&cmd_pickup_wheels_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Beacon_Start */ + +/* this structure is filled when cmd_beacon_start is parsed successfully */ +struct cmd_beacon_start_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_beacon_start is parsed successfully */ +static void cmd_beacon_start_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_beacon_start_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("start"))) + i2c_sensorboard_set_beacon(1); + else + i2c_sensorboard_set_beacon(0); +#endif +} + +prog_char str_beacon_start_arg0[] = "beacon"; +parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0); +prog_char str_beacon_start_arg1[] = "start#stop"; +parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1); + +prog_char help_beacon_start[] = "Beacon enabled/disable"; +parse_pgm_inst_t cmd_beacon_start = { + .f = cmd_beacon_start_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beacon_start, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_beacon_start_arg0, + (prog_void *)&cmd_beacon_start_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) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"), + sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4), + mechboard.pump_right1_current, mechboard.pump_right2_current); +#endif +} + +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, + }, +}; + +/**********************************************************/ +/* Build_Test */ + +/* this structure is filled when cmd_build_test is parsed successfully */ +struct cmd_build_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_build_test is parsed successfully */ +static void cmd_build_test_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + //struct cmd_build_test_result *res = parsed_result; + + printf_P(PSTR("lintel must be there\r\n")); + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + wait_ms(500); + + printf_P(PSTR("Insert 4 colums\r\n")); + while (get_column_count() != 4); + + i2c_mechboard_mode_prepare_build_both(0); + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + wait_ms(500); + + i2c_mechboard_mode_simple_autobuild(0); + wait_ms(100); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + + while (get_column_count() != 3); + + i2c_mechboard_mode_prepare_build_both(3); + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + wait_ms(500); + + i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST, + 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0); + i2cproto_wait_update(); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE); + wait_ms(500); + + i2c_mechboard_mode_harvest(); + while (get_column_count() != 3); + + i2c_mechboard_mode_prepare_build_both(5); + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + wait_ms(1000); + + i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST, + 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0); + i2cproto_wait_update(); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + trajectory_d_rel(&mainboard.traj, -200); +#endif +} + +prog_char str_build_test_arg0[] = "build_test"; +parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0); + +prog_char help_build_test[] = "Build_Test function"; +parse_pgm_inst_t cmd_build_test = { + .f = cmd_build_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_build_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_build_test_arg0, + NULL, + }, +}; + + +/**********************************************************/ +/* Column_Test */ + +/* this structure is filled when cmd_column_test is parsed successfully */ +struct cmd_column_test_result { + fixed_string_t arg0; + uint8_t level; + int16_t dist; + int8_t a1; + int8_t a2; + int8_t a3; + int16_t arm_dist; + int8_t nb_col; +}; + +/* function called when cmd_column_test is parsed successfully */ +static void cmd_column_test_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_column_test_result *res = parsed_result; + uint8_t level = res->level, debug = 0; + uint8_t c, push = 0; + + /* default conf */ + if (data) { + res->dist = 70; + res->a1 = -20; + res->a2 = 40; + res->a3 = -20; + res->arm_dist = 220; + res->nb_col = 2; + } + + if (!strcmp_P(res->arg0, PSTR("column_test_debug"))) + debug = 1; + if (!strcmp_P(res->arg0, PSTR("column_test_push"))) + push = 1; + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + + /* Go to disc */ + + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + /* go back, insert colums */ + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + printf_P(PSTR("Insert 4 colums\r\n")); + while (get_column_count() != 4); + + /* build with left arm */ + + i2c_mechboard_mode_prepare_inside_both(level); + trajectory_d_rel(&mainboard.traj, 200-(res->dist)); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + + trajectory_a_rel(&mainboard.traj, res->a1); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + + i2c_mechboard_mode_prepare_build_select(level, -1); + time_wait_ms(200); + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist, + 0, 0, res->arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1); + + if (debug) + c = cmdline_getchar_wait(); + /* build with right arm */ + + trajectory_a_rel(&mainboard.traj, res->a2); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + /* only ok for nb_col == 2 */ + if ((level + res->nb_col) >= 7) + i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1); + else + i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col); + time_wait_ms(200); + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_autobuild(0, 0, res->arm_dist, + level + res->nb_col, res->nb_col, + res->arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + + if (push) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE); + time_wait_ms(500); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + else if (level == 1 || level == 0) { + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2); + + if (debug) + c = cmdline_getchar_wait(); + + trajectory_a_rel(&mainboard.traj, res->a3); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + /* go back, insert colums */ + + trajectory_d_rel(&mainboard.traj, -100); + + return; +#endif +} + +prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push"; +parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0); +parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8); + +prog_char help_column_test[] = "Column_Test function (level)"; +parse_pgm_inst_t cmd_column_test = { + .f = cmd_column_test_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_column_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_column_test_arg0, + (prog_void *)&cmd_column_test_arg1, + NULL, + }, +}; + +parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16); +parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8); +parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8); +parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8); +parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16); +parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8); + +prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)"; +parse_pgm_inst_t cmd_column_test2 = { + .f = cmd_column_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_column_test2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_column_test_arg0, + (prog_void *)&cmd_column_test_arg1, + (prog_void *)&cmd_column_test_arg2, + (prog_void *)&cmd_column_test_arg3, + (prog_void *)&cmd_column_test_arg4, + (prog_void *)&cmd_column_test_arg5, + (prog_void *)&cmd_column_test_arg6, + (prog_void *)&cmd_column_test_arg7, + NULL, + }, +}; + + +/**********************************************************/ +/* Pickup_Test */ + +/* this structure is filled when cmd_pickup_test is parsed successfully */ +struct cmd_pickup_test_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t dist; +}; + +/* return red or green sensor */ +#define COLOR_IR_SENSOR() \ + ({ \ + uint8_t __ret = 0; \ + if (side == I2C_RIGHT_SIDE) \ + __ret = sensor_get(S_DISP_RIGHT); \ + else \ + __ret = sensor_get(S_DISP_LEFT); \ + __ret; \ + }) \ +/* column dispensers */ +#define COL_SCAN_MARGIN 200 +/* distance between the wheel axis and the IR sensor */ + +/* function called when cmd_pickup_test is parsed successfully */ +static void cmd_pickup_test_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + uint8_t err, side, first_try = 1; + int8_t cols_count_before, cols_count_after, cols; + struct cmd_pickup_test_result *res = parsed_result; + int16_t pos1, pos2, pos; + microseconds us; + int16_t dist = res->dist; + uint8_t timeout = 0; + + if (!strcmp_P(res->arg1, PSTR("left"))) + side = I2C_LEFT_SIDE; + else + side = I2C_RIGHT_SIDE; + + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + cols_count_before = get_column_count(); + position_set(&mainboard.pos, 0, 0, 0); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -1000); + err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); + if (err) /* we should not reach end */ + goto fail; + pos1 = position_get_x_s16(&mainboard.pos); + printf_P(PSTR("pos1 = %d\r\n"), pos1); + + err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); + if (err) + goto fail; + pos2 = position_get_x_s16(&mainboard.pos); + printf_P(PSTR("pos2 = %d\r\n"), pos2); + + pos = ABS(pos1 - pos2); + printf_P(PSTR("pos = %d\r\n"), pos); + + trajectory_d_rel(&mainboard.traj, -dist + pos/2); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + if (side == I2C_LEFT_SIDE) + trajectory_a_rel(&mainboard.traj, 90); + else + trajectory_a_rel(&mainboard.traj, -90); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + pickup_wheels_on(); + retry: + if (first_try) + i2c_mechboard_mode_lazy_harvest(); + else + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + first_try = 0; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, 300); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST); + + trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + goto fail; + + position_set(&mainboard.pos, 0, 0, 0); + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(90, 0); + goto retry; + } + + /* start to pickup with finger / arms */ + + printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__); + i2c_mechboard_mode_pickup(); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == + I2C_MECHBOARD_MODE_PICKUP, 100); + us = time_get_us2(); + cols = get_column_count(); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) { + if (get_column_count() != cols) { + cols = get_column_count(); + us = time_get_us2(); + } + if ((get_column_count() - cols_count_before) >= 4) { + printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__); + break; + } + /* 1 second timeout */ + if (time_get_us2() - us > 1500000L) { + printf_P(PSTR("%s timeout\r\n"), __FUNCTION__); + timeout = 1; + break; + } + } + + /* eject if we found a bad color column */ + + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(90, 0); + goto retry; + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -250); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + + cols_count_after = get_column_count(); + cols = cols_count_after - cols_count_before; + DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols); + + pickup_wheels_off(); + i2c_mechboard_mode_clear(); + + wait_ms(1000); + return; + fail: + printf_P(PSTR("failed\r\n")); + strat_hardstop(); +#endif +} + +prog_char str_pickup_test_arg0[] = "pickup_test"; +parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0); +prog_char str_pickup_test_arg1[] = "left#right"; +parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1); +parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16); + +prog_char help_pickup_test[] = "Pickup_Test function"; +parse_pgm_inst_t cmd_pickup_test = { + .f = cmd_pickup_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pickup_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pickup_test_arg0, + (prog_void *)&cmd_pickup_test_arg1, + (prog_void *)&cmd_pickup_test_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Lintel_Test */ + +/* this structure is filled when cmd_lintel_test is parsed successfully */ +struct cmd_lintel_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_lintel_test is parsed successfully */ +static void cmd_lintel_test_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + uint8_t err, first_try = 1, right_ok, left_ok; + int16_t left_cur, right_cur; + + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(500); + retry: + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, 500); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err) && err != END_BLOCKING) + goto fail; + + i2c_mechboard_mode_get_lintel(); + time_wait_ms(500); + + left_cur = sensor_get_adc(ADC_CSENSE3); + left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN); + right_cur = mechboard.pump_right1_current; + right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN); + + printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"), + left_ok, left_cur, right_ok, right_cur); + if (first_try) { + if (!right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + else if (right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -150, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, -140, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + else if (!right_ok && left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -150, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, -140, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + /* else, lintel is ok */ + else { + i2c_mechboard_mode_put_lintel(); + } + } + else { + if (right_ok && left_ok) { + /* lintel is ok */ + i2c_mechboard_mode_put_lintel(); + } + else { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -250); + err = wait_traj_end(TRAJ_FLAGS_STD); + return; + +fail: + printf_P(PSTR("fail\r\n")); + return; +#endif +} + +prog_char str_lintel_test_arg0[] = "lintel_test"; +parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0); + +prog_char help_lintel_test[] = "Lintel_Test function"; +parse_pgm_inst_t cmd_lintel_test = { + .f = cmd_lintel_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_lintel_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_lintel_test_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Scan_Test */ + +/* this structure is filled when cmd_scan_test is parsed successfully */ +struct cmd_scan_test_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t start_dist; + int16_t scan_dist; + int16_t scan_speed; + int16_t center_x; + int16_t center_y; + uint8_t level; +}; + +#define SCAN_MODE_CHECK_TEMPLE 0 +#define SCAN_MODE_SCAN_COL 1 +#define SCAN_MODE_SCAN_TEMPLE 2 +#define SCAN_MODE_TRAJ_ONLY 3 + +/* function called when cmd_scan_test is parsed successfully */ +static void cmd_scan_test_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + uint8_t err, mode=0, c; + int16_t pos1x, pos1y, dist; + struct cmd_scan_test_result *res = parsed_result; + int16_t back_mm = 0; + + int16_t ckpt_rel_x = 0, ckpt_rel_y = 0; + + double center_abs_x, center_abs_y; + double ckpt_rel_d, ckpt_rel_a; + double ckpt_abs_x, ckpt_abs_y; + + if (!strcmp_P(res->arg1, PSTR("traj_only"))) + mode = SCAN_MODE_TRAJ_ONLY; + else if (!strcmp_P(res->arg1, PSTR("check_temple"))) + mode = SCAN_MODE_CHECK_TEMPLE; + else if (!strcmp_P(res->arg1, PSTR("scan_col"))) + mode = SCAN_MODE_SCAN_COL; + else if (!strcmp_P(res->arg1, PSTR("scan_temple"))) + mode = SCAN_MODE_SCAN_TEMPLE; + + /* go to disc */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (err != END_BLOCKING) + return; + + /* save absolute position of disc */ + rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y); + + /* go back and prepare to scan */ + strat_set_speed(1000, 1000); + trajectory_d_a_rel(&mainboard.traj, -140, 130); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return; + + /* prepare scanner arm */ + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_prepare(); + time_wait_ms(250); + + strat_set_speed(res->scan_speed, 1000); + + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + trajectory_d_rel(&mainboard.traj, -res->scan_dist); + + while (1) { + err = test_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (err != 0) + break; + + dist = distance_from_robot(pos1x, pos1y); + + if (dist > res->start_dist) + break; + + if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) { + err = END_ERROR; + break; + } + } + + if (err) { + if (TRAJ_SUCCESS(err)) + err = END_ERROR; /* should not reach end */ + strat_hardstop(); + trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_stop(); + return; + } + + /* start the scanner */ + + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_start(); + + err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN, + TRAJ_FLAGS_NO_NEAR); + if (err == 0) + err = END_ERROR; + if (!TRAJ_SUCCESS(err)) { + strat_hardstop(); + trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_stop(); + return; + } + + if (mode == SCAN_MODE_TRAJ_ONLY) + return; + + wait_scan_done(10000); + + i2c_sensorboard_scanner_stop(); + + if (mode == SCAN_MODE_CHECK_TEMPLE) { + i2c_sensorboard_scanner_algo_check(res->level, + res->center_x, res->center_y); + i2cproto_wait_update(); + wait_scan_done(10000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1) { + printf_P(PSTR("-- try to build a temple\r\n")); + res->center_x = 15; + res->center_y = 13; + mode = SCAN_MODE_SCAN_TEMPLE; + } + } + + if (mode == SCAN_MODE_SCAN_TEMPLE) { + i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC, + res->center_x, + res->center_y); + i2cproto_wait_update(); + wait_scan_done(10000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1 || + strat_scan_get_checkpoint(mode, &ckpt_rel_x, + &ckpt_rel_y, &back_mm)) { + printf_P(PSTR("-- try to build a column\r\n")); + mode = SCAN_MODE_SCAN_COL; + } + } + + if (mode == SCAN_MODE_SCAN_COL) { + i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, + res->center_x, res->center_y); + i2cproto_wait_update(); + wait_scan_done(10000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1 || + strat_scan_get_checkpoint(mode, &ckpt_rel_x, + &ckpt_rel_y, &back_mm)) { + return; + } + } + + if (sensorboard.dropzone_h == -1) + return; + + if (mode == SCAN_MODE_CHECK_TEMPLE) { + ckpt_rel_x = 220; + ckpt_rel_y = 100; + } + + + printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y); + + rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y); + abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a); + + printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y); + + printf_P(PSTR("ok ? (y/n)\r\n")); + + c = cmdline_getchar_wait(); + + if (c != 'y') + return; + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + /* intermediate checkpoint for some positions */ + if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) { + trajectory_goto_xy_rel(&mainboard.traj, 200, 100); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + return; + } + + trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + return; + + trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + return; + + c = cmdline_getchar_wait(); + + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 200); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200, + TRAJ_FLAGS_SMALL_DIST); + if (err == 0) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + if (err != END_BLOCKING) + return; + + if (back_mm) { + trajectory_d_rel(&mainboard.traj, -back_mm); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } +#endif +} + +prog_char str_scan_test_arg0[] = "scan_test"; +parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0); +prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple"; +parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1); +parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16); +parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16); +parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16); +parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16); +parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16); + +prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)"; +parse_pgm_inst_t cmd_scan_test = { + .f = cmd_scan_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scan_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_test_arg0, + (prog_void *)&cmd_scan_test_arg1, + (prog_void *)&cmd_scan_test_arg2, + (prog_void *)&cmd_scan_test_arg3, + (prog_void *)&cmd_scan_test_arg4, + (prog_void *)&cmd_scan_test_arg5, + (prog_void *)&cmd_scan_test_arg6, + NULL, + }, +}; + +prog_char str_scan_test_arg1b[] = "check_temple"; +parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b); +parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8); + +prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)"; +parse_pgm_inst_t cmd_scan_test2 = { + .f = cmd_scan_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scan_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_test_arg0, + (prog_void *)&cmd_scan_test_arg1b, + (prog_void *)&cmd_scan_test_arg2, + (prog_void *)&cmd_scan_test_arg3, + (prog_void *)&cmd_scan_test_arg4, + (prog_void *)&cmd_scan_test_arg5, + (prog_void *)&cmd_scan_test_arg6, + (prog_void *)&cmd_scan_test_arg7, + NULL, + }, +}; + +/**********************************************************/ +/* Time_Monitor */ + +/* this structure is filled when cmd_time_monitor is parsed successfully */ +struct cmd_time_monitor_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_time_monitor is parsed successfully */ +static void cmd_time_monitor_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_time_monitor_result *res = parsed_result; + uint16_t seconds; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + eeprom_write_word(EEPROM_TIME_ADDRESS, 0); + } + seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); + printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); +#endif +} + +prog_char str_time_monitor_arg0[] = "time_monitor"; +parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0); +prog_char str_time_monitor_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1); + +prog_char help_time_monitor[] = "Show since how long we are running"; +parse_pgm_inst_t cmd_time_monitor = { + .f = cmd_time_monitor_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_time_monitor, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_time_monitor_arg0, + (prog_void *)&cmd_time_monitor_arg1, + NULL, + }, +}; + + +/**********************************************************/ +/* Scanner */ + +/* this structure is filled when cmd_scanner is parsed successfully */ +struct cmd_scanner_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scanner is parsed successfully */ +static void cmd_scanner_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_scanner_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("prepare"))) { + i2c_sensorboard_scanner_prepare(); + } + else if (!strcmp_P(res->arg1, PSTR("stop"))) { + i2c_sensorboard_scanner_stop(); + } + else if (!strcmp_P(res->arg1, PSTR("start"))) { + i2c_sensorboard_scanner_start(); + } + else if (!strcmp_P(res->arg1, PSTR("algo_col"))) { + i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, + 15, 15); + } + else if (!strcmp_P(res->arg1, PSTR("algo_check"))) { + i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX + } + else if (!strcmp_P(res->arg1, PSTR("calib"))) { + i2c_sensorboard_scanner_calib(); + } + else if (!strcmp_P(res->arg1, PSTR("show"))) { + scanner_dump_state(); + } +#endif +} + +prog_char str_scanner_arg0[] = "scanner"; +parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0); +prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib"; +parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1); + +prog_char help_scanner[] = "send commands to scanner"; +parse_pgm_inst_t cmd_scanner = { + .f = cmd_scanner_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scanner, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scanner_arg0, + (prog_void *)&cmd_scanner_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Build_Z1 */ + +/* this structure is filled when cmd_build_z1 is parsed successfully */ +struct cmd_build_z1_result { + fixed_string_t arg0; + uint8_t level; + int16_t d1; + int16_t d2; + int16_t d3; +}; + +/* function called when cmd_build_z1 is parsed successfully */ +static void cmd_build_z1_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_build_z1_result *res = parsed_result; + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + + while (get_column_count() != 4); + + i2c_mechboard_mode_prepare_build_both(res->level); + time_wait_ms(500); + + trajectory_d_rel(&mainboard.traj, 400); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -res->d1); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, + res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, + 1); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == + I2C_MECHBOARD_MODE_AUTOBUILD, 100); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != + I2C_MECHBOARD_MODE_AUTOBUILD, 10000); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -res->d2); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(1); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, res->d3); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); +#endif +} + +prog_char str_build_z1_arg0[] = "build_z1"; +parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0); +parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8); +parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16); +parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16); +parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16); + +prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)"; +parse_pgm_inst_t cmd_build_z1 = { + .f = cmd_build_z1_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_build_z1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_build_z1_arg0, + (prog_void *)&cmd_build_z1_arg1, + (prog_void *)&cmd_build_z1_arg2, + (prog_void *)&cmd_build_z1_arg3, + (prog_void *)&cmd_build_z1_arg4, + NULL, + }, +}; + +#ifdef TEST_BEACON +/**********************************************************/ +/* Beacon_Opp_Dump */ + +/* this structure is filled when cmd_beacon_opp_dump is parsed successfully */ +struct cmd_beacon_opp_dump_result { + fixed_string_t arg0; +}; + +void beacon_dump_samples(void); + +/* function called when cmd_beacon_opp_dump is parsed successfully */ +static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + beacon_dump_samples(); +#endif +} + +prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump"; +parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0); + +prog_char help_beacon_opp_dump[] = "Dump beacon samples"; +parse_pgm_inst_t cmd_beacon_opp_dump = { + .f = cmd_beacon_opp_dump_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beacon_opp_dump, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_beacon_opp_dump_arg0, + NULL, + }, +}; +#endif + +/**********************************************************/ +/* Test */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; + int32_t radius; +}; +void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a); +/* function called when cmd_test is parsed successfully */ +static void cmd_test_parsed(void *parsed_result, void *data) +{ + struct cmd_test_result *res = parsed_result; + double d,a; + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a); + printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a); +} + +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); +parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32); + +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, + (prog_void *)&cmd_test_arg1, + NULL, + }, +}; diff --git a/projects/microb2010/tests/hostsim/commands_traj.c b/projects/microb2010/tests/hostsim/commands_traj.c new file mode 100644 index 0000000..55bfe5f --- /dev/null +++ b/projects/microb2010/tests/hostsim/commands_traj.c @@ -0,0 +1,1213 @@ +/* + * 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_traj.c,v 1.8 2009-11-08 17:24:33 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 +#include + +#include "i2c_commands.h" +#include "main.h" +#include "cs.h" +#include "cmdline.h" +#include "strat.h" +#include "strat_utils.h" +#include "strat_base.h" +#include "strat_avoid.h" + +/**********************************************************/ +/* Traj_Speeds for trajectory_manager */ + +/* this structure is filled when cmd_traj_speed is parsed successfully */ +struct cmd_traj_speed_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t s; +}; + +/* function called when cmd_traj_speed is parsed successfully */ +static void cmd_traj_speed_parsed(void *parsed_result, void *data) +{ + struct cmd_traj_speed_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("angle"))) { + trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s); + } + else if (!strcmp_P(res->arg1, PSTR("distance"))) { + trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed); + } + /* else it is a "show" */ + + printf_P(PSTR("angle %u, distance %u\r\n"), + mainboard.traj.a_speed, + mainboard.traj.d_speed); +} + +prog_char str_traj_speed_arg0[] = "traj_speed"; +parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0); +prog_char str_traj_speed_arg1[] = "angle#distance"; +parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1); +parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16); + +prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg1, + (prog_void *)&cmd_traj_speed_s, + NULL, + }, +}; + +/* show */ + +prog_char str_traj_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg); + +prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed_show = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* circle coef configuration */ + +/* this structure is filled when cmd_circle_coef is parsed successfully */ +struct cmd_circle_coef_result { + fixed_string_t arg0; + fixed_string_t arg1; + float circle_coef; +}; + + +/* function called when cmd_circle_coef is parsed successfully */ +static void cmd_circle_coef_parsed(void *parsed_result, void *data) +{ + struct cmd_circle_coef_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + trajectory_set_circle_coef(&mainboard.traj, res->circle_coef); + } + + printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef); +} + +prog_char str_circle_coef_arg0[] = "circle_coef"; +parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0); +prog_char str_circle_coef_arg1[] = "set"; +parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1); +parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT); + +prog_char help_circle_coef[] = "Set circle coef"; +parse_pgm_inst_t cmd_circle_coef = { + .f = cmd_circle_coef_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_circle_coef, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_arg1, + (prog_void *)&cmd_circle_coef_val, + NULL, + }, +}; + +/* show */ + +prog_char str_circle_coef_show_arg[] = "show"; +parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg); + +prog_char help_circle_coef_show[] = "Show circle coef"; +parse_pgm_inst_t cmd_circle_coef_show = { + .f = cmd_circle_coef_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_circle_coef_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* trajectory window configuration */ + +/* this structure is filled when cmd_trajectory is parsed successfully */ +struct cmd_trajectory_result { + fixed_string_t arg0; + fixed_string_t arg1; + float d_win; + float a_win; + float a_start; +}; + + +/* function called when cmd_trajectory is parsed successfully */ +static void cmd_trajectory_parsed(void * parsed_result, void * data) +{ + struct cmd_trajectory_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + trajectory_set_windows(&mainboard.traj, res->d_win, + res->a_win, res->a_start); + } + + printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win, + DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad)); +} + +prog_char str_trajectory_arg0[] = "trajectory"; +parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0); +prog_char str_trajectory_arg1[] = "set"; +parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1); +parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT); + +prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)"; +parse_pgm_inst_t cmd_trajectory = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg1, + (prog_void *)&cmd_trajectory_d, + (prog_void *)&cmd_trajectory_a, + (prog_void *)&cmd_trajectory_as, + NULL, + }, +}; + +/* show */ + +prog_char str_trajectory_show_arg[] = "show"; +parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg); + +prog_char help_trajectory_show[] = "Show trajectory window configuration"; +parse_pgm_inst_t cmd_trajectory_show = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* rs_gains configuration */ + +/* this structure is filled when cmd_rs_gains is parsed successfully */ +struct cmd_rs_gains_result { + fixed_string_t arg0; + fixed_string_t arg1; + float left; + float right; +}; + +/* function called when cmd_rs_gains is parsed successfully */ +static void cmd_rs_gains_parsed(void * parsed_result, void * data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_rs_gains_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, + LEFT_ENCODER, res->left); // en augmentant on tourne à gauche + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + RIGHT_ENCODER, res->right); //en augmentant on tourne à droite + } + printf_P(PSTR("rs_gains set ")); + f64_print(mainboard.rs.left_ext_gain); + printf_P(PSTR(" ")); + f64_print(mainboard.rs.right_ext_gain); + printf_P(PSTR("\r\n")); +#endif +} + +prog_char str_rs_gains_arg0[] = "rs_gains"; +parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0); +prog_char str_rs_gains_arg1[] = "set"; +parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1); +parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT); +parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT); + +prog_char help_rs_gains[] = "Set rs_gains (left, right)"; +parse_pgm_inst_t cmd_rs_gains = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg1, + (prog_void *)&cmd_rs_gains_l, + (prog_void *)&cmd_rs_gains_r, + NULL, + }, +}; + +/* show */ + +prog_char str_rs_gains_show_arg[] = "show"; +parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg); + +prog_char help_rs_gains_show[] = "Show rs_gains"; +parse_pgm_inst_t cmd_rs_gains_show = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* track configuration */ + +/* this structure is filled when cmd_track is parsed successfully */ +struct cmd_track_result { + fixed_string_t arg0; + fixed_string_t arg1; + float val; +}; + +/* function called when cmd_track is parsed successfully */ +static void cmd_track_parsed(void * parsed_result, void * data) +{ + struct cmd_track_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM); + } + printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm); +} + +prog_char str_track_arg0[] = "track"; +parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0); +prog_char str_track_arg1[] = "set"; +parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1); +parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT); + +prog_char help_track[] = "Set track in mm"; +parse_pgm_inst_t cmd_track = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg1, + (prog_void *)&cmd_track_val, + NULL, + }, +}; + +/* show */ + +prog_char str_track_show_arg[] = "show"; +parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg); + +prog_char help_track_show[] = "Show track"; +parse_pgm_inst_t cmd_track_show = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Pt_Lists for testing traj */ + +#define PT_LIST_SIZE 10 +static struct xy_point pt_list[PT_LIST_SIZE]; +static uint16_t pt_list_len = 0; + +/* this structure is filled when cmd_pt_list is parsed successfully */ +struct cmd_pt_list_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t arg2; + int16_t arg3; + int16_t arg4; +}; + +/* function called when cmd_pt_list is parsed successfully */ +static void cmd_pt_list_parsed(void * parsed_result, void * data) +{ + struct cmd_pt_list_result * res = parsed_result; + uint8_t i, why=0; + + if (!strcmp_P(res->arg1, PSTR("append"))) { + res->arg2 = pt_list_len; + } + + if (!strcmp_P(res->arg1, PSTR("insert")) || + !strcmp_P(res->arg1, PSTR("append"))) { + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + if (pt_list_len >= PT_LIST_SIZE) { + printf_P(PSTR("List is too large\r\n")); + return; + } + memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], + PT_LIST_SIZE-1-res->arg2); + pt_list[res->arg2].x = res->arg3; + pt_list[res->arg2].y = res->arg4; + pt_list_len++; + } + else if (!strcmp_P(res->arg1, PSTR("del"))) { + if (pt_list_len <= 0) { + printf_P(PSTR("Error: list empty\r\n")); + return; + } + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], + (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point)); + pt_list_len--; + } + else if (!strcmp_P(res->arg1, PSTR("reset"))) { + pt_list_len = 0; + } + + /* else it is a "show" or a "start" */ + if (pt_list_len == 0) { + printf_P(PSTR("List empty\r\n")); + return; + } + for (i=0 ; iarg1, PSTR("start"))) { + trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); + why = wait_traj_end(0xFF); /* all */ + } + else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { + while (1) { + why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF); + printf("next point\r\n"); + if (why != END_OBSTACLE) + break; + } + } + if (why & (~(END_TRAJ | END_NEAR))) + trajectory_stop(&mainboard.traj); + } +} + +prog_char str_pt_list_arg0[] = "pt_list"; +parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0); +prog_char str_pt_list_arg1[] = "insert"; +parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1); +parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16); +parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16); +parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16); + +prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)"; +parse_pgm_inst_t cmd_pt_list = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1, + (prog_void *)&cmd_pt_list_arg2, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* append */ + +prog_char str_pt_list_arg1_append[] = "append"; +parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append); + +prog_char help_pt_list_append[] = "Append point in pt_list (x,y)"; +parse_pgm_inst_t cmd_pt_list_append = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_append, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1_append, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* del */ + +prog_char str_pt_list_del_arg[] = "del"; +parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg); + +prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)"; +parse_pgm_inst_t cmd_pt_list_del = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_del, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_del_arg, + (prog_void *)&cmd_pt_list_arg2, + NULL, + }, +}; +/* show */ + +prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start"; +parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg); + +prog_char help_pt_list_show[] = "Show, start or reset pt_list"; +parse_pgm_inst_t cmd_pt_list_show = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Goto function */ + +/* this structure is filled when cmd_goto is parsed successfully */ +struct cmd_goto_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +/* function called when cmd_goto is parsed successfully */ +static void cmd_goto_parsed(void * parsed_result, void * data) +{ + struct cmd_goto_result * res = parsed_result; + uint8_t err; + microseconds t1, t2; + + interrupt_traj_reset(); + if (!strcmp_P(res->arg1, PSTR("a_rel"))) { + trajectory_a_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("d_rel"))) { + trajectory_d_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_abs"))) { + trajectory_a_abs(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) { + trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) { + trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) { + trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) { + trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("avoid"))) { + err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF); + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) { + err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF); + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) { + trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) { + trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("da_rel"))) { + trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3); + } + t1 = time_get_us2(); + while ((err = test_traj_end(0xFF)) == 0) { + t2 = time_get_us2(); + if (t2 - t1 > 200000) { + dump_cs_debug("angle", &mainboard.angle.cs); + dump_cs_debug("distance", &mainboard.distance.cs); + t1 = t2; + } + } + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + printf_P(PSTR("returned %s\r\n"), get_err(err)); +} + +prog_char str_goto_arg0[] = "goto"; +parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0); +prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs"; +parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a); +parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32); + +/* 1 params */ +prog_char help_goto1[] = "Change orientation of the mainboard"; +parse_pgm_inst_t cmd_goto1 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_a, + (prog_void *)&cmd_goto_arg2, + NULL, + }, +}; + +prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy"; +parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b); +parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32); + +/* 2 params */ +prog_char help_goto2[] = "Go to a (x,y) or (d,a) position"; +parse_pgm_inst_t cmd_goto2 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_b, + (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* Position tests */ + +/* this structure is filled when cmd_position is parsed successfully */ +struct cmd_position_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +#define AUTOPOS_SPEED_FAST 200 +static void auto_position(void) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + + interrupt_traj_reset(); + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(ROBOT_LENGTH/2, 0, 0); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_a_rel(&mainboard.traj, COLOR_A(90)); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2), + COLOR_A(90)); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + trajectory_a_rel(&mainboard.traj, COLOR_A(-40)); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + strat_set_speed(old_spdd, old_spda); + return; + +intr: + strat_hardstop(); + strat_set_speed(old_spdd, old_spda); +} + +/* function called when cmd_position is parsed successfully */ +static void cmd_position_parsed(void * parsed_result, void * data) +{ + struct cmd_position_result * res = parsed_result; + + /* display raw position values */ + if (!strcmp_P(res->arg1, PSTR("reset"))) { + position_set(&mainboard.pos, 0, 0, 0); + } + else if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) { + mainboard.our_color = I2C_COLOR_GREEN; +#ifndef HOST_VERSION + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); +#endif + auto_position(); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { + mainboard.our_color = I2C_COLOR_RED; +#ifndef HOST_VERSION + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); +#endif + auto_position(); + } + + /* else it's just a "show" */ + printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), + position_get_x_double(&mainboard.pos), + position_get_y_double(&mainboard.pos), + DEG(position_get_a_rad_double(&mainboard.pos))); +} + +prog_char str_position_arg0[] = "position"; +parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); +prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red"; +parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); + +prog_char help_position[] = "Show/reset (x,y,a) position"; +parse_pgm_inst_t cmd_position = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1, + NULL, + }, +}; + + +prog_char str_position_arg1_set[] = "set"; +parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set); +parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32); +parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32); +parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32); + +prog_char help_position_set[] = "Set (x,y,a) position"; +parse_pgm_inst_t cmd_position_set = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position_set, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1_set, + (prog_void *)&cmd_position_arg2, + (prog_void *)&cmd_position_arg3, + (prog_void *)&cmd_position_arg4, + NULL, + }, +}; + + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_infos is parsed successfully */ +struct cmd_strat_infos_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_strat_infos is parsed successfully */ +static void cmd_strat_infos_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_infos_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + strat_reset_infos(); + } + strat_infos.dump_enabled = 1; + strat_dump_infos(__FUNCTION__); +} + +prog_char str_strat_infos_arg0[] = "strat_infos"; +parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0); +prog_char str_strat_infos_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1); + +prog_char help_strat_infos[] = "reset/show strat_infos"; +parse_pgm_inst_t cmd_strat_infos = { + .f = cmd_strat_infos_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_infos, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_infos_arg0, + (prog_void *)&cmd_strat_infos_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf is parsed successfully */ +struct cmd_strat_conf_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_strat_conf is parsed successfully */ +static void cmd_strat_conf_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("base"))) { + strat_infos.conf.flags = 0; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("big3"))) { + strat_infos.conf.flags = + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_BIG_3_TEMPLE; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("base_check"))) { + strat_infos.conf.flags = 0; + strat_infos.conf.scan_our_min_time = 35; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("big3_check"))) { + strat_infos.conf.flags = + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_BIG_3_TEMPLE; + strat_infos.conf.scan_our_min_time = 35; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) { + strat_infos.conf.flags = + STRAT_CONF_TAKE_ONE_LINTEL | + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_EARLY_SCAN | + STRAT_CONF_PUSH_OPP_COLS; + strat_infos.conf.scan_our_min_time = 50; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 15; + strat_infos.conf.delay_between_opp_scan = 90; + strat_infos.conf.wait_opponent = 5; + } + else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) { + strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 30; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) { + strat_infos.conf.flags = + STRAT_CONF_ONLY_ONE_ON_DISC; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0); +prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc"; +parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1); + +prog_char help_strat_conf[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf = { + .f = cmd_strat_conf_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf_arg0, + (prog_void *)&cmd_strat_conf_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf2 is parsed successfully */ +struct cmd_strat_conf2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_strat_conf2 is parsed successfully */ +static void cmd_strat_conf2_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf2_result *res = parsed_result; + uint8_t on, bit = 0; + + if (!strcmp_P(res->arg2, PSTR("on"))) + on = 1; + else + on = 0; + + if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc"))) + bit = STRAT_CONF_ONLY_ONE_ON_DISC; + else if (!strcmp_P(res->arg1, PSTR("bypass_static2"))) + bit = STRAT_CONF_BYPASS_STATIC2; + else if (!strcmp_P(res->arg1, PSTR("take_one_lintel"))) + bit = STRAT_CONF_TAKE_ONE_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail"))) + bit = STRAT_CONF_TAKE_ONE_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("store_static2"))) + bit = STRAT_CONF_STORE_STATIC2; + else if (!strcmp_P(res->arg1, PSTR("big3_temple"))) + bit = STRAT_CONF_BIG_3_TEMPLE; + else if (!strcmp_P(res->arg1, PSTR("early_opp_scan"))) + bit = STRAT_CONF_EARLY_SCAN; + else if (!strcmp_P(res->arg1, PSTR("push_opp_cols"))) + bit = STRAT_CONF_PUSH_OPP_COLS; + + if (on) + strat_infos.conf.flags |= bit; + else + strat_infos.conf.flags &= (~bit); + + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf2_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0); +prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan"; +parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1); +prog_char str_strat_conf2_arg2[] = "on#off"; +parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2); + + +prog_char help_strat_conf2[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf2 = { + .f = cmd_strat_conf2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf2_arg0, + (prog_void *)&cmd_strat_conf2_arg1, + (prog_void *)&cmd_strat_conf2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf3 is parsed successfully */ +struct cmd_strat_conf3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t arg2; +}; + +/* function called when cmd_strat_conf3 is parsed successfully */ +static void cmd_strat_conf3_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf3_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.scan_opp_min_time = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.delay_between_opp_scan = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.scan_our_min_time = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.delay_between_our_scan = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { + strat_infos.conf.wait_opponent = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { + strat_infos.conf.lintel_min_time = res->arg2; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf3_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0); +prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent"; +parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1); +parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16); + +prog_char help_strat_conf3[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf3 = { + .f = cmd_strat_conf3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf3_arg0, + (prog_void *)&cmd_strat_conf3_arg1, + (prog_void *)&cmd_strat_conf3_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf4 is parsed successfully */ +struct cmd_strat_conf4_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_strat_conf4 is parsed successfully */ +static void cmd_strat_conf4_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf4_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) { + strat_infos.conf.scan_opp_angle = res->arg2; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf4_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0); +prog_char str_strat_conf4_arg1[] = "scan_opponent_angle"; +parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1); +parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16); + +prog_char help_strat_conf4[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf4 = { + .f = cmd_strat_conf4_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf4, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf4_arg0, + (prog_void *)&cmd_strat_conf4_arg1, + (prog_void *)&cmd_strat_conf4_arg2, + NULL, + }, +}; + + +/**********************************************************/ +/* Subtraj */ + +/* this structure is filled when cmd_subtraj is parsed successfully */ +struct cmd_subtraj_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; + int32_t arg5; +}; + +/* function called when cmd_subtraj is parsed successfully */ +static void cmd_subtraj_parsed(void *parsed_result, void *data) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + struct cmd_subtraj_result *res = parsed_result; + uint8_t err = 0; + struct column_dispenser *disp; + + if (strcmp_P(res->arg1, PSTR("static")) == 0) { + err = strat_static_columns(res->arg2); + } + else if (strcmp_P(res->arg1, PSTR("static2")) == 0) { + strat_infos.s_cols.configuration = res->arg2; + switch (res->arg2) { + case 1: + position_set(&mainboard.pos, 1398, + COLOR_Y(1297), COLOR_A(-66)); + break; + case 2: + position_set(&mainboard.pos, 1232, + COLOR_Y(1051), COLOR_A(4)); + break; + case 3: + position_set(&mainboard.pos, 1232, + COLOR_Y(1043), COLOR_A(5)); + break; + case 4: + position_set(&mainboard.pos, 1346, + COLOR_Y(852), COLOR_A(57)); + break; + default: + return; + } + if (res->arg2 == 1 && res->arg3 == 1) { + strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE; + } + if (res->arg2 == 1 && res->arg3 == 2) { + strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE; + } + err = strat_static_columns_pass2(); + } + else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) { + err = strat_goto_lintel_disp(&strat_infos.l1); + } + else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) { + err = strat_goto_lintel_disp(&strat_infos.l2); + } + else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) { + disp = &strat_infos.c1; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) { + disp = &strat_infos.c2; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) { + disp = &strat_infos.c3; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("disc")) == 0) { + if (res->arg2 == 0) { + printf_P(PSTR("bad level\r\n")); + return; + } + err = strat_goto_disc(res->arg2); + } + + printf_P(PSTR("substrat returned %s\r\n"), get_err(err)); + trajectory_hardstop(&mainboard.traj); +#endif +} + +prog_char str_subtraj_arg0[] = "subtraj"; +parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0); +prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2"; +parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1); +parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32); +parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32); +parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32); +parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32); + +prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)"; +parse_pgm_inst_t cmd_subtraj = { + .f = cmd_subtraj_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_subtraj, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_subtraj_arg0, + (prog_void *)&cmd_subtraj_arg1, + (prog_void *)&cmd_subtraj_arg2, + (prog_void *)&cmd_subtraj_arg3, + (prog_void *)&cmd_subtraj_arg4, + (prog_void *)&cmd_subtraj_arg5, + NULL, + }, +}; diff --git a/projects/microb2010/tests/hostsim/cs.c b/projects/microb2010/tests/hostsim/cs.c index cbb65d6..c5ecf27 100644 --- a/projects/microb2010/tests/hostsim/cs.c +++ b/projects/microb2010/tests/hostsim/cs.c @@ -43,6 +43,8 @@ #include #include "robotsim.h" +#include "strat.h" +#include "actuator.h" #include "main.h" /* called every 5 ms */ @@ -88,11 +90,11 @@ static void do_cs(void *dummy) * compensation) */ position_manage(&mainboard.pos); } -#if 0 if (mainboard.flags & DO_BD) { bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); } +#if 0 if (mainboard.flags & DO_TIMER) { uint8_t second; /* the robot should stop correctly in the strat, but @@ -152,8 +154,8 @@ void microb_cs_init(void) { /* ROBOT_SYSTEM */ rs_init(&mainboard.rs); - rs_set_left_pwm(&mainboard.rs, robotsim_pwm, LEFT_PWM); - rs_set_right_pwm(&mainboard.rs, robotsim_pwm, RIGHT_PWM); + rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM); + rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM); /* increase gain to decrease dist, increase left and it will turn more left */ rs_set_left_ext_encoder(&mainboard.rs, robotsim_encoder_get, LEFT_ENCODER, IMP_COEF); diff --git a/projects/microb2010/tests/hostsim/i2c_commands.h b/projects/microb2010/tests/hostsim/i2c_commands.h new file mode 100644 index 0000000..fdb5d49 --- /dev/null +++ b/projects/microb2010/tests/hostsim/i2c_commands.h @@ -0,0 +1,295 @@ +/* + * 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_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $ + * + */ + +#ifndef _I2C_COMMANDS_H_ +#define _I2C_COMMANDS_H_ + +#define I2C_MAINBOARD_ADDR 1 +#define I2C_MECHBOARD_ADDR 2 +#define I2C_SENSORBOARD_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_COLOR_RED 0 +#define I2C_COLOR_GREEN 1 + +#define I2C_AUTOBUILD_DEFAULT_DIST 210 + +struct i2c_cmd_hdr { + uint8_t cmd; +}; + +/****/ +/* commands that do not need and answer */ +/****/ + +#define I2C_CMD_GENERIC_LED_CONTROL 0x00 + +struct i2c_cmd_led_control { + struct i2c_cmd_hdr hdr; + uint8_t led_num:7; + uint8_t state:1; +}; + +/****/ + +#define I2C_CMD_GENERIC_SET_COLOR 0x01 + +struct i2c_cmd_generic_color { + struct i2c_cmd_hdr hdr; + uint8_t color; +}; + +/****/ + +#define I2C_CMD_MECHBOARD_SET_MODE 0x02 + +struct i2c_cmd_mechboard_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 + 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; + }; +}; + +/****/ +/* requests and their answers */ +/****/ + + +#define I2C_REQ_MECHBOARD_STATUS 0x80 + +struct i2c_req_mechboard_status { + struct i2c_cmd_hdr hdr; + + int16_t pump_left1_current; + int16_t pump_left2_current; +}; + +#define I2C_ANS_MECHBOARD_STATUS 0x81 + +struct i2c_ans_mechboard_status { + struct i2c_cmd_hdr hdr; + /* mode type are defined above: I2C_MECHBOARD_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 + 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; +}; + +#define I2C_REQ_SENSORBOARD_STATUS 0x82 + +struct i2c_req_sensorboard_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 + +struct i2c_ans_sensorboard_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_COLUMN_NO_DROPZONE -1 + int8_t dropzone_h; + int16_t dropzone_x; + int16_t dropzone_y; +}; + +#endif /* _I2C_PROTOCOL_H_ */ diff --git a/projects/microb2010/tests/hostsim/main.c b/projects/microb2010/tests/hostsim/main.c index ec70479..85a31a2 100644 --- a/projects/microb2010/tests/hostsim/main.c +++ b/projects/microb2010/tests/hostsim/main.c @@ -46,6 +46,7 @@ #include #include "cs.h" +#include "cmdline.h" #include "main.h" struct genboard gen; @@ -53,14 +54,23 @@ struct mainboard mainboard; int main(void) { -#ifndef HOST_VERSION uart_init(); + uart_register_rx_event(CMDLINE_UART, emergency); +#ifndef HOST_VERSION fdevopen(uart0_dev_send, uart0_dev_recv); sei(); -#else - int i; #endif + memset(&gen, 0, sizeof(gen)); + memset(&mainboard, 0, sizeof(mainboard)); + + /* LOGS */ + error_register_emerg(mylog); + error_register_error(mylog); + error_register_warning(mylog); + error_register_notice(mylog); + error_register_debug(mylog); + #ifdef CONFIG_MODULE_TIMER timer_init(); #endif @@ -71,21 +81,24 @@ int main(void) hostsim_init(); robotsim_init(); #endif - time_init(TIME_PRIO); - - microb_cs_init(); - gen.logs[0] = E_USER_CS; - gen.log_level = 5; + time_init(TIME_PRIO); + + printf_P(PSTR("\r\n")); + printf_P(PSTR("Respect et robustesse.\r\n")); mainboard.flags = DO_ENCODERS | DO_RS | DO_POS | DO_POWER | DO_BD | DO_CS; + strat_reset_pos(1000, 1000, 0); + + cmdline_interact(); - trajectory_d_rel(&mainboard.traj, 1000); - time_wait_ms(2000); - trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000); - time_wait_ms(2000); +/* trajectory_d_rel(&mainboard.traj, 1000); */ +/* time_wait_ms(2000); */ +/* trajectory_circle_rel(&mainboard.traj, 1500, 1000, */ +/* 250, 360, 0); */ +/* time_wait_ms(15000); */ return 0; } diff --git a/projects/microb2010/tests/hostsim/main.h b/projects/microb2010/tests/hostsim/main.h index 5f07ab4..73e3da7 100644 --- a/projects/microb2010/tests/hostsim/main.h +++ b/projects/microb2010/tests/hostsim/main.h @@ -143,13 +143,6 @@ struct mainboard { extern struct genboard gen; extern struct mainboard mainboard; -#define SPEED_DIST_FAST 2500 -#define SPEED_ANGLE_FAST 2000 -#define SPEED_DIST_SLOW 1000 -#define SPEED_ANGLE_SLOW 1000 -#define SPEED_DIST_VERY_SLOW 400 -#define SPEED_ANGLE_VERY_SLOW 400 - #define WAIT_COND_OR_TIMEOUT(cond, timeout) \ ({ \ microseconds __us = time_get_us2(); \ diff --git a/projects/microb2010/tests/hostsim/obstacle_avoidance_config.h b/projects/microb2010/tests/hostsim/obstacle_avoidance_config.h new file mode 100644 index 0000000..dbf2cab --- /dev/null +++ b/projects/microb2010/tests/hostsim/obstacle_avoidance_config.h @@ -0,0 +1,26 @@ +/* + * Copyright Droids Corporation, Microb Technology (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: obstacle_avoidance_config.h,v 1.1.2.1 2009-05-02 10:00:35 zer0 Exp $ + * + */ + +#define MAX_POLY 3 +#define MAX_PTS 10 +#define MAX_RAYS 100 +#define MAX_CHKPOINTS 5 +#define OA_COEF 30000 diff --git a/projects/microb2010/tests/hostsim/robotsim.c b/projects/microb2010/tests/hostsim/robotsim.c index 7ed49f4..990104b 100644 --- a/projects/microb2010/tests/hostsim/robotsim.c +++ b/projects/microb2010/tests/hostsim/robotsim.c @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -44,6 +45,8 @@ #include #include +#include "strat.h" +#include "strat_utils.h" #include "main.h" static int32_t l_pwm, r_pwm; @@ -59,11 +62,47 @@ static int fd; void robotsim_update(void) { static int32_t l_speed, r_speed; + double x, y, a, a2, d; + + /* corners of the robot */ + double xfl, yfl; /* front left */ + double xrl, yrl; /* rear left */ + double xrr, yrr; /* rear right */ + double xfr, yfr; /* front right */ + + x = position_get_x_double(&mainboard.pos); + y = position_get_y_double(&mainboard.pos); + a = position_get_a_rad_double(&mainboard.pos); - /* XXX should lock */ l_speed = ((l_speed * FILTER) / 100) + ((l_pwm * 1000 * FILTER2)/1000); - l_enc += (l_speed / 1000); r_speed = ((r_speed * FILTER) / 100) + ((r_pwm * 1000 * FILTER2)/1000); + + /* basic collision detection */ + a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2); + d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2); + + xfl = x + cos(a+a2) * d; + yfl = y + sin(a+a2) * d; + if (!is_in_area(xfl, yfl, 0) && l_speed > 0) + l_speed = 0; + + xrl = x + cos(a+M_PI-a2) * d; + yrl = y + sin(a+M_PI-a2) * d; + if (!is_in_area(xrl, yrl, 0) && l_speed < 0) + l_speed = 0; + + xrr = x + cos(a+M_PI+a2) * d; + yrr = y + sin(a+M_PI+a2) * d; + if (!is_in_area(xrr, yrr, 0) && r_speed < 0) + r_speed = 0; + + xfr = x + cos(a-a2) * d; + yfr = y + sin(a-a2) * d; + if (!is_in_area(xfr, yfr, 0) && r_speed > 0) + r_speed = 0; + + /* XXX should lock */ + l_enc += (l_speed / 1000); r_enc += (r_speed / 1000); } diff --git a/projects/microb2010/tests/hostsim/strat.c b/projects/microb2010/tests/hostsim/strat.c new file mode 100644 index 0000000..ab6de1d --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat.c @@ -0,0 +1,820 @@ +/* + * Copyright Droids, Microb Technology (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: strat.c,v 1.6 2009-11-08 17:24:33 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 +#include + +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "i2c_commands.h" + + +#define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ +#define COL_SCAN_PRE_MARGIN 250 + + +#ifdef TEST_BEACON + +#define BEACON_MAX_SAMPLES 100 +struct beacon_sample { + int16_t posx; + int16_t posy; + int16_t posa; + int16_t oppx; + int16_t oppy; + uint8_t time; +}; + +static struct beacon_sample beacon_sample[BEACON_MAX_SAMPLES]; +static uint8_t beacon_prev_time = 0; +static uint8_t beacon_cur_idx = 0; + +static void beacon_update_samples(void) +{ + int16_t opp_a, opp_d, opp_x, opp_y; + int8_t err; + uint8_t time; + + time = time_get_s(); + + /* one sample per second max */ + if (time <= beacon_prev_time) + return; + /* limit max number of samples */ + if (beacon_cur_idx >= BEACON_MAX_SAMPLES) + return; + + memset(&beacon_sample[beacon_cur_idx], 0, sizeof(beacon_sample[beacon_cur_idx])); + beacon_prev_time = time; + beacon_sample[beacon_cur_idx].time = time; + + /* get opponent pos; if not found, just set struct to 0 */ + err = get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a); + if (err == -1) + return; + + beacon_sample[beacon_cur_idx].posx = position_get_x_s16(&mainboard.pos); + beacon_sample[beacon_cur_idx].posy = position_get_y_s16(&mainboard.pos); + beacon_sample[beacon_cur_idx].posa = position_get_a_deg_s16(&mainboard.pos); + beacon_sample[beacon_cur_idx].oppx = opp_x; + beacon_sample[beacon_cur_idx].oppy = opp_y; + beacon_cur_idx++; +} + +void beacon_dump_samples(void) +{ + uint16_t i; + + for (i=0; izone->name); + + if (temple->flags & TEMPLE_F_MONOCOL) + printf_P(PSTR("MONOCOL ")); + else + printf_P(PSTR("BICOL ")); + + if (temple->flags & TEMPLE_F_ON_DISC) + printf_P(PSTR("ON_DISC ")); + else + printf_P(PSTR("ON_ZONE_0_1 ")); + + if (temple->flags & TEMPLE_F_OPPONENT) + printf_P(PSTR("OPPONENT ")); + else + printf_P(PSTR("OURS ")); + + if (temple->flags & TEMPLE_F_LINTEL) + printf_P(PSTR("LIN_ON_TOP ")); + else + printf_P(PSTR("COL_ON_TOP ")); + + printf_P(PSTR("\r\n")); + + printf_P(PSTR(" pos=(%d,%d,%d) ckpt=(%d,%d) ltime=%d\r\n"), + temple->x, temple->y, temple->a, + temple->checkpoint_x, temple->checkpoint_y, + temple->last_try_time); + printf_P(PSTR(" L: lev=%d da=%d,%d\r\n"), + temple->level_l, temple->dist_l, temple->angle_l); + printf_P(PSTR(" R: lev=%d da=%d,%d\r\n"), + temple->level_l, temple->dist_l, temple->angle_l); +} + +void strat_dump_zone(struct build_zone *zone) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR(" zone %s: "), zone->name); + + if (zone->flags & ZONE_F_DISC) + printf_P(PSTR("DISC ")); + else if (zone->flags & ZONE_F_ZONE1) + printf_P(PSTR("ZONE1 ")); + else if (zone->flags & ZONE_F_ZONE0) + printf_P(PSTR("ZONE0 ")); + + if (zone->flags & ZONE_F_BUSY) + printf_P(PSTR("BUSY ")); + else + printf_P(PSTR("FREE ")); + + printf_P(PSTR("\r\n")); + + printf_P(PSTR(" lev=%d ckpt=(%d,%d) ltime=%d\r\n"), + zone->level, + zone->checkpoint_x, zone->checkpoint_y, + zone->last_try_time); +} + +void strat_dump_static_cols(void) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR(" static cols: l0=%d l1=%d l2=%d\r\n"), + strat_infos.s_cols.flags & STATIC_COL_LINE0_DONE, + strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE, + strat_infos.s_cols.flags & STATIC_COL_LINE2_DONE); +} + +void strat_dump_col_disp(void) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR(" c1 cnt=%d ltt=%d\r\n"), + strat_infos.c1.count, strat_infos.c1.last_try_time); + printf_P(PSTR(" c2 cnt=%d ltt=%d\r\n"), + strat_infos.c2.count, strat_infos.c2.last_try_time); + printf_P(PSTR(" c3 cnt=%d ltt=%d\r\n"), + strat_infos.c3.count, strat_infos.c3.last_try_time); +} + +void strat_dump_lin_disp(void) +{ + if (!strat_infos.dump_enabled) + return; + printf_P(PSTR(" l1 cnt=%d ltt=%d\r\n"), + strat_infos.l1.count, strat_infos.l1.last_try_time); + printf_P(PSTR(" l2 cnt=%d ltt=%d\r\n"), + strat_infos.l2.count, strat_infos.l2.last_try_time); + +} + +void strat_dump_all_temples(void) +{ + struct temple *temple; + uint8_t i; + + if (!strat_infos.dump_enabled) + return; + + for (i=0; iflags & TEMPLE_F_VALID)) + continue; + strat_dump_temple(temple); + } +} + +void strat_dump_all_zones(void) +{ + struct build_zone *zone; + uint8_t i; + + if (!strat_infos.dump_enabled) + return; + + for (i=0; iflags & ZONE_F_VALID)) + continue; + strat_dump_zone(zone); + } +} + +/* display current information about the state of the game */ +void strat_dump_infos(const char *caller) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR("%s() dump strat infos:\r\n"), caller); + strat_dump_static_cols(); + strat_dump_col_disp(); + strat_dump_lin_disp(); + strat_dump_all_temples(); + strat_dump_all_zones(); +} + +/* init current area state before a match. Dump update user conf + * here */ +void strat_reset_infos(void) +{ + uint8_t i; + + /* /!\ don't do a big memset() as there is static data */ + strat_infos.s_cols.flags = 0; + strat_infos.c1.count = 5; + strat_infos.c1.last_try_time = 0; + strat_infos.c2.count = 5; + strat_infos.c2.last_try_time = 0; + strat_infos.c3.count = 5; + strat_infos.c3.last_try_time = 0; + strat_infos.l1.count = 1; + strat_infos.l1.last_try_time = 0; + strat_infos.l2.count = 1; + strat_infos.l2.last_try_time = 0; + + strat_infos.taken_lintel = 0; + strat_infos.col_in_boobs = 0; + strat_infos.lazy_pickup_done = 0; + strat_infos.i2c_loaded_skipped = 0; + + memset(strat_infos.temple_list, 0, sizeof(strat_infos.temple_list)); + + for (i=0; i= 1) && + (get_column_count_right() >= 1)) + return 0; + } + else { + if (get_column_count()) + return 0; + } +#endif + return 1; +} + +/* dump state (every 5 s max) */ +#define DUMP_RATE_LIMIT(dump, last_print) \ + do { \ + if (time_get_s() - last_print > 5) { \ + dump(); \ + last_print = time_get_s(); \ + } \ + } while (0) + + +uint8_t strat_main(void) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); + return END_ERROR; +#else + uint8_t err; + struct temple *temple = NULL; + struct build_zone *zone = NULL; + + uint8_t last_print_cols = 0; + uint8_t last_print_lin = 0; + uint8_t last_print_temple = 0; + uint8_t last_print_zone = 0; + + /* do static cols + first temple */ + err = strat_beginning(); + + /* skip error code */ + + while (1) { + + if (err == END_TIMER) { + DEBUG(E_USER_STRAT, "End of time"); + strat_exit(); + break; + } + + /* we have at least one col on each arm, build now */ + if (need_more_elements() == 0) { + + /* try to build on opponent, will return + * END_TRAJ without doing anything if + * disabled */ + err = strat_build_on_opponent_temple(); + if (!TRAJ_SUCCESS(err)) + continue; + if (need_more_elements()) + continue; + + /* try to scan and build on our temple, will + * return END_TRAJ without doing anything if + * disabled */ + err = strat_check_temple_and_build(); + if (!TRAJ_SUCCESS(err)) + continue; + if (need_more_elements()) + continue; + + /* Else, do a simple build, as before */ + + temple = strat_get_best_temple(); + + /* one valid temple found */ + if (temple) { + DUMP_RATE_LIMIT(strat_dump_all_temples, last_print_temple); + + err = strat_goto_temple(temple); + if (!TRAJ_SUCCESS(err)) + continue; + + /* can return END_ERROR or END_TIMER, + * should not happen here */ + err = strat_grow_temple(temple); + if (!TRAJ_SUCCESS(err)) + continue; + + err = strat_escape(temple->zone, TRAJ_FLAGS_STD); + if (!TRAJ_SUCCESS(err)) + continue; + + continue; + } + + zone = strat_get_best_zone(); + if (zone) { + DUMP_RATE_LIMIT(strat_dump_all_zones, last_print_zone); + + DEBUG(E_USER_STRAT, "goto zone %s", zone->name); + err = strat_goto_build_zone(zone, zone->level); + if (!TRAJ_SUCCESS(err)) + continue; + DEBUG(E_USER_STRAT, "zone reached"); + + /* no error code except END_ERROR, should not happen */ + err = strat_build_new_temple(zone); + + err = strat_escape(zone, TRAJ_FLAGS_STD); + if (!TRAJ_SUCCESS(err)) + continue; + + continue; + } + + /* XXX hey what can we do here... :'( */ + DEBUG(E_USER_STRAT, "panic :)"); + time_wait_ms(1000); + continue; + } + + /* else we need some elements (lintels, then columns) */ + else { + if (strat_infos.l1.count != 0 && strat_infos.l2.count != 0) + DUMP_RATE_LIMIT(strat_dump_lin_disp, last_print_lin); + + err = strat_pickup_lintels(); + /* can return an error code, but we have + * nothing to do because pickup_column() + * starts with a goto_and_avoid() */ + if (!TRAJ_SUCCESS(err)) + nop(); + + DUMP_RATE_LIMIT(strat_dump_col_disp, last_print_cols); + + err = strat_pickup_columns(); + if (!TRAJ_SUCCESS(err)) + nop(); /* nothing to do */ + + /* XXX check here that we have elements, or do + * something else */ + /* if we cannot take elements, try to build */ + } + } + return END_TRAJ; +#endif +} diff --git a/projects/microb2010/tests/hostsim/strat.h b/projects/microb2010/tests/hostsim/strat.h new file mode 100644 index 0000000..2b2b96a --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat.h @@ -0,0 +1,302 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (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: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#ifndef _STRAT_H_ +#define _STRAT_H_ + +/* convert coords according to our color */ +#define COLOR_Y(y) ((mainboard.our_color==I2C_COLOR_RED)? (y) : (AREA_Y-(y))) +#define COLOR_A(a) ((mainboard.our_color==I2C_COLOR_RED)? (a) : (-a)) +#define COLOR_SIGN(x) ((mainboard.our_color==I2C_COLOR_RED)? (x) : (-x)) +#define COLOR_INVERT(x) ((mainboard.our_color==I2C_COLOR_RED)? (x) : (!x)) + +/* area */ +#define AREA_X 3000 +#define AREA_Y 2100 + +#define START_X 200 +#define START_Y COLOR_Y(200) +#define START_A COLOR_A(45) + +#define CENTER_X 1500 +#define CENTER_Y 1050 + +#define CORNER_X 3000 +#define CORNER_Y COLOR_Y(2100) + +/* + * /- line 0 + * | /- line 1 + * | | /- line 2 + * | | | + * +---C1--------------------------C1---+ + * | z0a z1 z0b | + * | . | + * | . | + * C3 0 1 2 ^ C3 + * y | 3 4 5 < > | + * C2 6 7 8 v C2 + * |------ 9 10 11 . ------| + * | | . | | + * | red | . |green| + * +-----+--L1--L2-------L2--L1---+-----+ + * x + */ + +/* static columns */ +#define LINE0_X 600 +#define LINE1_X 850 +#define LINE2_X 1100 + +#define COL0_X 600 +#define COL0_Y COLOR_Y(1175) +#define COL1_X 850 +#define COL1_Y COLOR_Y(1175) +#define COL2_X 1100 +#define COL2_Y COLOR_Y(1175) + +#define COL3_X 600 +#define COL3_Y COLOR_Y(975) +#define COL4_X 850 +#define COL4_Y COLOR_Y(975) +#define COL5_X 1100 +#define COL5_Y COLOR_Y(975) + +#define COL6_X 600 +#define COL6_Y COLOR_Y(775) +#define COL7_X 850 +#define COL7_Y COLOR_Y(775) +#define COL8_X 1100 +#define COL8_Y COLOR_Y(775) + +#define COL9_X 600 +#define COL9_Y COLOR_Y(575) +#define COL10_X 850 +#define COL10_Y COLOR_Y(575) +#define COL11_X 1100 +#define COL11_Y COLOR_Y(575) + +/* distance to go backward before pickup in dispenser */ +#define DIST_BACK_DISPENSER 35 + +/* diag of the pentagon (pentacle ?) */ +#define DISC_PENTA_DIAG 530 + +#define COL_DISP_MAX_TRIES 5 +#define LIN_DISP_MAX_TRIES 3 + +/* useful traj flags */ +#define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR)) +#define TRAJ_FLAGS_STD (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR|END_TIMER) +#define TRAJ_FLAGS_NO_TIMER (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR) +#define TRAJ_FLAGS_NO_NEAR (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR|END_TIMER) +#define TRAJ_FLAGS_NO_NEAR_NO_TIMER (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR) +#define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR) + +/* default speeds */ +#define SPEED_DIST_FAST 2500 +#define SPEED_ANGLE_FAST 2000 +#define SPEED_DIST_SLOW 1000 +#define SPEED_ANGLE_SLOW 1000 +#define SPEED_DIST_VERY_SLOW 400 +#define SPEED_ANGLE_VERY_SLOW 400 + +/* strat infos structures */ + +struct bbox { + int32_t x1; + int32_t y1; + int32_t x2; + int32_t y2; +}; + +struct conf { +#define STRAT_CONF_ONLY_ONE_ON_DISC 0x01 +#define STRAT_CONF_BYPASS_STATIC2 0x02 +#define STRAT_CONF_TAKE_ONE_LINTEL 0x04 +#define STRAT_CONF_SKIP_WHEN_CHECK_FAILS 0x08 +#define STRAT_CONF_STORE_STATIC2 0x10 +#define STRAT_CONF_BIG_3_TEMPLE 0x20 +#define STRAT_CONF_EARLY_SCAN 0x40 +#define STRAT_CONF_PUSH_OPP_COLS 0x80 + uint8_t flags; + uint8_t scan_opp_min_time; + uint8_t delay_between_opp_scan; + uint8_t scan_our_min_time; + uint8_t delay_between_our_scan; + uint8_t wait_opponent; + uint8_t lintel_min_time; + int16_t scan_opp_angle; +}; + +struct static_columns { +#define STATIC_COL_LINE0_DONE 0x01 +#define STATIC_COL_LINE1_DONE 0x02 +#define STATIC_COL_LINE2_DONE 0x04 + uint8_t flags; + uint8_t configuration; +}; + +struct column_dispenser { + int8_t count; + uint8_t last_try_time; + uint8_t scan_left; + int16_t checkpoint_x; + int16_t checkpoint_y; + int16_t scan_a; + int16_t eject_a; + int16_t pickup_a; + int16_t recalib_x; + int16_t recalib_y; + char *name; +}; + +struct lintel_dispenser { + int8_t count; + uint8_t last_try_time; + int16_t x; + char *name; +}; + +struct temple { +#define TEMPLE_F_VALID 0x01 /* structure is valid */ +#define TEMPLE_F_MONOCOL 0x02 /* temple has only one col */ +#define TEMPLE_F_ON_DISC 0x04 /* temple is on disc (else it's on other zone) */ +#define TEMPLE_F_OPPONENT 0x08 /* temple was originally built by opponent */ +#define TEMPLE_F_LINTEL 0x10 /* lintel on top (don't put another lintel) */ + + uint8_t flags; + /* position of the robot when we built it */ + int16_t x; + int16_t y; + int16_t a; + + /* position of the robot checkpoint */ + int16_t checkpoint_x; + int16_t checkpoint_y; + + /* position and level of each col */ + uint8_t level_l; + uint8_t dist_l; + uint8_t angle_l; + + uint8_t level_r; + uint8_t dist_r; + uint8_t angle_r; + +#define TEMPLE_DISABLE_TIME 5 + uint8_t last_try_time; + + struct build_zone *zone; +}; + +struct build_zone { +#define ZONE_F_VALID 0x01 /* zone is valid */ +#define ZONE_F_DISC 0x02 /* specific disc zone */ +#define ZONE_F_ZONE1 0x04 /* specific zone 1 */ +#define ZONE_F_ZONE0 0x08 /* specific zone 0 */ +#define ZONE_F_BUSY 0x10 /* this zone is busy */ + uint8_t flags; + uint8_t level; + int16_t checkpoint_x; + int16_t checkpoint_y; + +#define ZONE_DISABLE_TIME 5 + uint8_t last_try_time; + char *name; +}; + +#define MAX_TEMPLE 5 +#define MAX_ZONE 5 + +/* all infos related to strat */ +struct strat_infos { + uint8_t dump_enabled; + struct conf conf; + struct bbox area_bbox; + uint8_t taken_lintel; + uint8_t col_in_boobs; + uint8_t lazy_pickup_done; + uint8_t i2c_loaded_skipped; + struct static_columns s_cols; + struct column_dispenser c1; + struct column_dispenser c2; + struct column_dispenser c3; + struct lintel_dispenser l1; + struct lintel_dispenser l2; + struct build_zone zone_list[MAX_ZONE]; + struct temple temple_list[MAX_TEMPLE]; +}; +extern struct strat_infos strat_infos; + +/* in strat.c */ +void strat_dump_infos(const char *caller); /* show current known state + of area */ +void strat_dump_temple(struct temple *temple); +void strat_dump_conf(void); +void strat_reset_infos(void); /* reset current known state */ +void strat_preinit(void); +void strat_init(void); +void strat_exit(void); +void strat_dump_flags(void); +void strat_goto_near(int16_t x, int16_t y, uint16_t dist); +uint8_t strat_main(void); +void strat_event(void *dummy); + +/* in strat_static_columns.c */ +uint8_t strat_static_columns(uint8_t configuration); +uint8_t strat_static_columns_pass2(void); + +/* in strat_lintel.c */ +uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp); +uint8_t strat_pickup_lintels(void); + +/* in strat_column_disp.c */ +uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a); +uint8_t strat_pickup_columns(void); +uint8_t strat_goto_col_disp(struct column_dispenser **disp); + +/* in strat_building.c */ +uint8_t strat_goto_disc(int8_t level); +uint8_t strat_goto_build_zone(struct build_zone *build_zone, uint8_t level); +uint8_t strat_build_new_temple(struct build_zone *build_zone); +uint8_t strat_goto_temple(struct temple *temple); +uint8_t strat_grow_temple(struct temple *temple); +uint8_t strat_grow_temple_column(struct temple *temple); +struct temple *strat_get_best_temple(void); +struct temple *strat_get_our_temple_on_disc(uint8_t valid); +struct build_zone *strat_get_best_zone(void); +struct temple *strat_get_free_temple(void); + +/* in strat_scan.c */ +struct scan_disc_result; +void scanner_dump_state(void); +int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x, + int16_t *ckpt_rel_y, int16_t *back_mm); +uint8_t strat_scan_disc(int16_t angle, uint8_t mode, + struct scan_disc_result *result); +uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level); +int16_t strat_get_temple_angle(struct temple *temple); +int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle); +uint8_t strat_build_on_opponent_temple(void); +uint8_t strat_check_temple_and_build(void); + +#endif diff --git a/projects/microb2010/tests/hostsim/strat_avoid.c b/projects/microb2010/tests/hostsim/strat_avoid.c new file mode 100644 index 0000000..6a337ed --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat_avoid.c @@ -0,0 +1,531 @@ +/* + * Copyright Droids Corporation, Microb Technology (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: strat_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#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 "strat.h" +#include "strat_base.h" +#include "strat_utils.h" + +#define EDGE_NUMBER 5 +void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt, + int16_t radius, int16_t x, int16_t y) +{ + + double c_a, s_a; + uint8_t i; + double px1, py1, px2, py2; + double a_rad; + + a_rad = atan2(y - robot_pt->y, x - robot_pt->x); + + /* generate pentagon */ + c_a = cos(-2*M_PI/EDGE_NUMBER); + s_a = sin(-2*M_PI/EDGE_NUMBER); + + /* + px1 = radius; + py1 = 0; + */ + px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER)); + py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER)); + + + for (i = 0; i < EDGE_NUMBER; i++){ + oa_poly_set_point(pol, x + px1, y + py1, i); + + px2 = px1*c_a + py1*s_a; + py2 = -px1*s_a + py1*c_a; + + px1 = px2; + py1 = py2; + } +} + +void set_rotated_poly(poly_t *pol, const point_t *robot_pt, + int16_t w, int16_t l, int16_t x, int16_t y) +{ + double tmp_x, tmp_y; + double a_rad; + + a_rad = atan2(y - robot_pt->y, x - robot_pt->x); + + DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f", + __FUNCTION__, x, y, a_rad); + + /* point 1 */ + tmp_x = w; + tmp_y = l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 0); + + /* point 2 */ + tmp_x = -w; + tmp_y = l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 1); + + /* point 3 */ + tmp_x = -w; + tmp_y = -l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 2); + + /* point 4 */ + tmp_x = w; + tmp_y = -l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 3); +} + +#define DISC_X CENTER_X +#define DISC_Y CENTER_Y + +void set_central_disc_poly(poly_t *pol, const point_t *robot_pt) +{ + set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG, + DISC_X, DISC_Y); +} + +#ifdef HOMOLOGATION +/* /!\ half size */ +#define O_WIDTH 400 +#define O_LENGTH 550 +#else +/* /!\ half size */ +#define O_WIDTH 360 +#define O_LENGTH 500 +#endif + +void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l) +{ + int16_t x, y; + get_opponent_xy(&x, &y); + DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y); + + /* place poly even if invalid, because it's -100 */ + set_rotated_poly(pol, robot_pt, w, l, x, y); +} + +/* don't care about polygons further than this distance for escape */ +#define ESCAPE_POLY_THRES 1000 + +/* don't reduce opp if opp is too far */ +#define REDUCE_POLY_THRES 600 + +/* has to be longer than any poly */ +#define ESCAPE_VECT_LEN 3000 + +/* + * Go in playground, loop until out of poly. The argument robot_pt is + * the pointer to the current position of the robot. + * Return 0 if there was nothing to do. + * Return 1 if we had to move. In this case, update the theorical + * position of the robot in robot_pt. + */ +static int8_t go_in_area(point_t *robot_pt) +{ + point_t poly_pts_area[4]; + poly_t poly_area; + point_t disc_pt, dst_pt; + + disc_pt.x = DISC_X; + disc_pt.y = DISC_Y; + + /* Go in playground */ + if (!is_in_boundingbox(robot_pt)){ + NOTICE(E_USER_STRAT, "not in playground %"PRIi32", %"PRIi32"", + robot_pt->x, robot_pt->y); + + poly_area.l = 4; + poly_area.pts = poly_pts_area; + poly_pts_area[0].x = strat_infos.area_bbox.x1; + poly_pts_area[0].y = strat_infos.area_bbox.y1; + + poly_pts_area[1].x = strat_infos.area_bbox.x2; + poly_pts_area[1].y = strat_infos.area_bbox.y1; + + poly_pts_area[2].x = strat_infos.area_bbox.x2; + poly_pts_area[2].y = strat_infos.area_bbox.y2; + + poly_pts_area[3].x = strat_infos.area_bbox.x1; + poly_pts_area[3].y = strat_infos.area_bbox.y2; + + is_crossing_poly(*robot_pt, disc_pt, &dst_pt, &poly_area); + NOTICE(E_USER_STRAT, "pt dst %"PRIi32", %"PRIi32"", dst_pt.x, dst_pt.y); + + strat_goto_xy_force(dst_pt.x, dst_pt.y); + + robot_pt->x = dst_pt.x; + robot_pt->y = dst_pt.y; + + NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", + dst_pt.x, dst_pt.y); + + return 1; + } + + return 0; +} + + +/* + * Escape from polygons if needed. + * robot_pt is the current position of the robot, it will be + * updated. + */ +static int8_t escape_from_poly(point_t *robot_pt, + poly_t *pol_disc, + int16_t opp_x, int16_t opp_y, + int16_t opp_w, int16_t opp_l, + poly_t *pol_opp) +{ + uint8_t in_disc = 0, in_opp = 0; + double escape_dx = 0, escape_dy = 0; + double disc_dx = 0, disc_dy = 0; + double opp_dx = 0, opp_dy = 0; + double len; + point_t opp_pt, disc_pt, dst_pt; + point_t intersect_disc_pt, intersect_opp_pt; + + opp_pt.x = opp_x; + opp_pt.y = opp_y; + disc_pt.x = DISC_X; + disc_pt.y = DISC_Y; + + /* escape from other poly if necessary */ + if (is_in_poly(robot_pt, pol_disc) == 1) + in_disc = 1; + if (is_in_poly(robot_pt, pol_opp) == 1) + in_opp = 1; + + if (in_disc == 0 && in_opp == 0) { + NOTICE(E_USER_STRAT, "no need to escape"); + return 0; + } + + NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp); + + /* process escape vector */ + + if (distance_between(robot_pt->x, robot_pt->y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) { + disc_dx = robot_pt->x - DISC_X; + disc_dy = robot_pt->y - DISC_Y; + NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f", + disc_dx, disc_dy); + len = norm(disc_dx, disc_dy); + if (len != 0) { + disc_dx /= len; + disc_dy /= len; + } + else { + disc_dx = 1.0; + disc_dy = 0.0; + } + escape_dx += disc_dx; + escape_dy += disc_dy; + } + + if (distance_between(robot_pt->x, robot_pt->y, opp_x, opp_y) < ESCAPE_POLY_THRES) { + opp_dx = robot_pt->x - opp_x; + opp_dy = robot_pt->y - opp_y; + NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f", + opp_dx, opp_dy); + len = norm(opp_dx, opp_dy); + if (len != 0) { + opp_dx /= len; + opp_dy /= len; + } + else { + opp_dx = 1.0; + opp_dy = 0.0; + } + escape_dx += opp_dx; + escape_dy += opp_dy; + } + + /* normalize escape vector */ + len = norm(escape_dx, escape_dy); + if (len != 0) { + escape_dx /= len; + escape_dy /= len; + } + else { + if (pol_disc != NULL) { + /* rotate 90° */ + escape_dx = disc_dy; + escape_dy = disc_dx; + } + else if (pol_opp != NULL) { + /* rotate 90° */ + escape_dx = opp_dy; + escape_dy = opp_dx; + } + else { /* should not happen */ + opp_dx = 1.0; + opp_dy = 0.0; + } + } + + NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f", + escape_dx, escape_dy); + + /* process the correct len of escape vector */ + + dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN; + dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN; + + NOTICE(E_USER_STRAT, "robot pt %"PRIi32" %"PRIi32, + robot_pt->x, robot_pt->y); + NOTICE(E_USER_STRAT, "dst point %"PRIi32",%"PRIi32, + dst_pt.x, dst_pt.y); + + if (in_disc) { + if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt, + pol_disc) == 1) { + /* we add 2 mm to be sure we are out of th polygon */ + dst_pt.x = intersect_disc_pt.x + escape_dx * 2; + dst_pt.y = intersect_disc_pt.y + escape_dy * 2; + if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) { + + if (!is_in_boundingbox(&dst_pt)) + return -1; + + NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", + dst_pt.x, dst_pt.y); + + strat_goto_xy_force(dst_pt.x, dst_pt.y); + + robot_pt->x = dst_pt.x; + robot_pt->y = dst_pt.y; + + return 0; + } + } + } + + if (in_opp) { + if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt, + pol_opp) == 1) { + /* we add 2 cm to be sure we are out of th polygon */ + dst_pt.x = intersect_opp_pt.x + escape_dx * 2; + dst_pt.y = intersect_opp_pt.y + escape_dy * 2; + + if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) { + + if (!is_in_boundingbox(&dst_pt)) + return -1; + + NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", + dst_pt.x, dst_pt.y); + + strat_goto_xy_force(dst_pt.x, dst_pt.y); + + robot_pt->x = dst_pt.x; + robot_pt->y = dst_pt.y; + + return 0; + } + } + } + + /* should not happen */ + return -1; +} + + +static int8_t __goto_and_avoid(int16_t x, int16_t y, + uint8_t flags_intermediate, + uint8_t flags_final, + uint8_t forward) +{ + int8_t len = -1, i; + point_t *p; + poly_t *pol_disc, *pol_opp; + int8_t ret; + int16_t opp_w, opp_l, opp_x, opp_y; + point_t p_dst, robot_pt; + + DEBUG(E_USER_STRAT, "%s(%d,%d) flags_i=%x flags_f=%x forw=%d", + __FUNCTION__, x, y, flags_intermediate, flags_final, forward); + + retry: + get_opponent_xy(&opp_x, &opp_y); + opp_w = O_WIDTH; + opp_l = O_LENGTH; + + robot_pt.x = position_get_x_s16(&mainboard.pos); + robot_pt.y = position_get_y_s16(&mainboard.pos); + + oa_init(); + pol_disc = oa_new_poly(5); + set_central_disc_poly(pol_disc, &robot_pt); + pol_opp = oa_new_poly(4); + set_opponent_poly(pol_opp, &robot_pt, O_WIDTH, O_LENGTH); + + /* If we are not in the limited area, try to go in it. */ + ret = go_in_area(&robot_pt); + + /* check that destination is valid */ + p_dst.x = x; + p_dst.y = y; + if (!is_in_boundingbox(&p_dst)) { + NOTICE(E_USER_STRAT, " dst is not in playground"); + return END_ERROR; + } + if (is_point_in_poly(pol_disc, x, y)) { + NOTICE(E_USER_STRAT, " dst is in disc"); + return END_ERROR; + } + if (is_point_in_poly(pol_opp, x, y)) { + NOTICE(E_USER_STRAT, " dst is in opp"); + return END_ERROR; + } + + /* now start to avoid */ + while (opp_w && opp_l) { + + /* robot_pt is not updated if it fails */ + ret = escape_from_poly(&robot_pt, + pol_disc, opp_x, opp_y, + opp_w, opp_l, pol_opp); + if (ret == 0) { + oa_reset(); + oa_start_end_points(robot_pt.x, robot_pt.y, x, y); + /* oa_dump(); */ + + len = oa_process(); + if (len >= 0) + break; + } + if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) { + if (opp_w == 0) + opp_l /= 2; + opp_w /= 2; + } + else { + NOTICE(E_USER_STRAT, "oa_process() returned %d", len); + return END_ERROR; + } + + NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l); + set_opponent_poly(pol_opp, &robot_pt, opp_w, opp_l); + } + + p = oa_get_path(); + for (i=0 ; ix, p->y); + + if (forward) + trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y); + else + trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y); + + /* no END_NEAR for the last point */ + if (i == len - 1) + ret = wait_traj_end(flags_final); + else + ret = wait_traj_end(flags_intermediate); + + if (ret == END_BLOCKING) { + DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", + __FUNCTION__, x, y); + goto retry; + } + else if (ret == END_OBSTACLE) { + /* brake and wait the speed to be slow */ + DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", + __FUNCTION__, x, y); + goto retry; + } + /* else if it is not END_TRAJ or END_NEAR, return */ + else if (!TRAJ_SUCCESS(ret)) { + return ret; + } + p++; + } + + return END_TRAJ; +} + +/* go forward to a x,y point. use current speed for that */ +uint8_t goto_and_avoid_forward(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final) +{ + return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1); +} + +/* go backward to a x,y point. use current speed for that */ +uint8_t goto_and_avoid_backward(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final) +{ + return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0); +} + +/* go to a x,y point. prefer backward but go forward if the point is + * near and in front of us */ +uint8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final) +{ + double d,a; + abs_xy_to_rel_da(x, y, &d, &a); + + if (d < 300 && a < RAD(90) && a > RAD(-90)) + return __goto_and_avoid(x, y, flags_intermediate, + flags_final, 1); + else + return __goto_and_avoid(x, y, flags_intermediate, + flags_final, 0); +} diff --git a/projects/microb2010/tests/hostsim/strat_avoid.h b/projects/microb2010/tests/hostsim/strat_avoid.h new file mode 100644 index 0000000..0978fd7 --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat_avoid.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology (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: strat_avoid.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +void set_opponent_poly(poly_t *pol, int16_t w, int16_t l); +int8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final); +int8_t goto_and_avoid_backward(int16_t x, int16_t y, + uint8_t flags_intermediate, + uint8_t flags_final); +int8_t goto_and_avoid_forward(int16_t x, int16_t y, + uint8_t flags_intermediate, + uint8_t flags_final); diff --git a/projects/microb2010/tests/hostsim/strat_base.c b/projects/microb2010/tests/hostsim/strat_base.c new file mode 100644 index 0000000..0dd1a8b --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat_base.c @@ -0,0 +1,515 @@ +/* + * Copyright Droids Corporation, Microb Technology (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: strat_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#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 "strat_utils.h" +#include "strat_base.h" +#include "strat.h" + +/* true if we want to interrupt a trajectory */ +static uint8_t traj_intr=0; + +/* filled when a END_OBSTACLE is returned */ +struct opponent_obstacle opponent_obstacle; + +/* asked speed */ +static volatile int16_t strat_speed_a = SPEED_DIST_FAST; +static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST; +static volatile uint16_t strat_limit_speed_a = 0; /* no limit */ +static volatile uint16_t strat_limit_speed_d = 0; + +static volatile uint8_t strat_limit_speed_enabled = 1; + + +/* Strings that match the end traj cause */ +/* /!\ keep it sync with stat_base.h */ +const char *err_tab []= { + "END_TRAJ", + "END_BLOCKING", + "END_NEAR", + "END_OBSTACLE", + "END_ERROR", + "END_INTR", + "END_TIMER", + "END_RESERVED", +}; + +/* return string from end traj type num */ +const char *get_err(uint8_t err) +{ + uint8_t i; + if (err == 0) + return "SUCCESS"; + for (i=0 ; i<8; i++) { + if (err & (1 < 200) || + (ABS(mainboard.speed_a) > 200)) + + trajectory_hardstop(&mainboard.traj); + pid_reset(&mainboard.angle.pid); + pid_reset(&mainboard.distance.pid); + bd_reset(&mainboard.angle.bd); + bd_reset(&mainboard.distance.bd); +} + +/* go to an x,y point without checking for obstacle or blocking. It + * should be used for very small dist only. Return END_TRAJ if we + * reach destination, or END_BLOCKING if the robot blocked more than 3 + * times. */ +uint8_t strat_goto_xy_force(int16_t x, int16_t y) +{ + uint8_t i, err; + +#ifdef HOMOLOGATION + int8_t serr; + uint8_t hardstop = 0; + microseconds us = time_get_us2(); + int16_t opp_a, opp_d, opp_x, opp_y; + + while (1) { + serr = get_opponent_xyda(&opp_x, &opp_y, + &opp_d, &opp_a); + if (serr == -1) + break; + if (opp_d < 600) + break; + if (hardstop == 0) { + strat_hardstop(); + hardstop = 1; + } + if ((time_get_us2() - us) > 3000000L) + break; + } +#endif + for (i=0; i<3; i++) { + trajectory_goto_xy_abs(&mainboard.traj, x, y); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (TRAJ_SUCCESS(err)) + return END_TRAJ; + if (err == END_BLOCKING) { + time_wait_ms(500); + strat_hardstop(); + } + } + return END_BLOCKING; +} + +/* reset position */ +void strat_reset_pos(int16_t x, int16_t y, int16_t a) +{ + int16_t posx = position_get_x_s16(&mainboard.pos); + int16_t posy = position_get_y_s16(&mainboard.pos); + int16_t posa = position_get_a_deg_s16(&mainboard.pos); + + if (x == DO_NOT_SET_POS) + x = posx; + if (y == DO_NOT_SET_POS) + y = posy; + if (a == DO_NOT_SET_POS) + a = posa; + + DEBUG(E_USER_STRAT, "reset pos (%s%s%s)", + x == DO_NOT_SET_POS ? "" : "x", + y == DO_NOT_SET_POS ? "" : "y", + a == DO_NOT_SET_POS ? "" : "a"); + position_set(&mainboard.pos, x, y, a); + DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__); +} + +/* + * decrease gain on angle PID, and go forward until we reach the + * border. + */ +uint8_t strat_calib(int16_t dist, uint8_t flags) +{ + int32_t p = pid_get_gain_P(&mainboard.angle.pid); + int32_t i = pid_get_gain_I(&mainboard.angle.pid); + int32_t d = pid_get_gain_D(&mainboard.angle.pid); + uint8_t err; + + pid_set_gains(&mainboard.angle.pid, 150, 0, 2000); + trajectory_d_rel(&mainboard.traj, dist); + err = wait_traj_end(flags); + pid_set_gains(&mainboard.angle.pid, p, i, d); + return err; +} + +/* escape from zone, and don't brake, so we can continue with another + * traj */ +uint8_t strat_escape(struct build_zone *zone, uint8_t flags) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + + strat_get_speed(&old_spdd, &old_spda); + + err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000); + if (err == 0) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -150); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + strat_set_speed(old_spdd, old_spda); + return err; + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + if (zone->flags & ZONE_F_DISC) { + trajectory_d_rel(&mainboard.traj, -350); + err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags); + } + else { + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(flags); + } + + strat_set_speed(old_spdd, old_spda); + if (err == 0) + return END_NEAR; + return err; +} + +static void strat_update_traj_speed(void) +{ + uint16_t d, a; + + d = strat_speed_d; + if (strat_limit_speed_d && d > strat_limit_speed_d) + d = strat_limit_speed_d; + a = strat_speed_a; + if (strat_limit_speed_a && a > strat_limit_speed_a) + a = strat_limit_speed_a; + + trajectory_set_speed(&mainboard.traj, d, a); +} + +void strat_set_speed(uint16_t d, uint16_t a) +{ + uint8_t flags; + IRQ_LOCK(flags); + strat_speed_d = d; + strat_speed_a = a; + strat_update_traj_speed(); + IRQ_UNLOCK(flags); +} + +void strat_get_speed(uint16_t *d, uint16_t *a) +{ + uint8_t flags; + IRQ_LOCK(flags); + *d = strat_speed_d; + *a = strat_speed_a; + IRQ_UNLOCK(flags); +} + +void strat_limit_speed_enable(void) +{ + strat_limit_speed_enabled = 1; +} + +void strat_limit_speed_disable(void) +{ + strat_limit_speed_enabled = 0; +} + +/* called periodically */ +void strat_limit_speed(void) +{ + uint16_t lim_d = 0, lim_a = 0; + int16_t opp_d, opp_a; + + if (strat_limit_speed_enabled == 0) + goto update; + + if (get_opponent_da(&opp_d, &opp_a) != 0) + goto update; + +#ifdef HOMOLOGATION + if (opp_d < 600) { + lim_d = 150; + lim_a = 200; + } +#else + if (opp_d < 500) { + if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { + lim_d = SPEED_DIST_VERY_SLOW; + lim_a = SPEED_ANGLE_VERY_SLOW; + } + else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) { + lim_d = SPEED_DIST_VERY_SLOW; + lim_a = SPEED_ANGLE_VERY_SLOW; + } + else { + lim_d = SPEED_DIST_SLOW; + lim_a = SPEED_ANGLE_VERY_SLOW; + } + } +#endif + else if (opp_d < 800) { + if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { + lim_d = SPEED_DIST_SLOW; + lim_a = SPEED_ANGLE_SLOW; + } + else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) { + lim_d = SPEED_DIST_SLOW; + lim_a = SPEED_ANGLE_SLOW; + } + } + + update: + if (lim_d != strat_limit_speed_d || + lim_a != strat_limit_speed_a) { + strat_limit_speed_d = lim_d; + strat_limit_speed_a = lim_a; + DEBUG(E_USER_STRAT, "new speed limit da=%d,%d", lim_d, lim_a); + strat_update_traj_speed(); + } +} + +/* start the strat */ +void strat_start(void) +{ +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + int8_t i; + uint8_t err; + + strat_preinit(); + + /* if start sw not plugged */ + if (sensor_get(S_START_SWITCH)) { + printf_P(PSTR("No start switch, press a key or plug it\r\n")); + + /* while start sw not plugged */ + while (sensor_get(S_START_SWITCH)) { + if (! cmdline_keypressed()) + continue; + + for (i=3; i>0; i--) { + printf_P(PSTR("%d\r\n"), i); + time_wait_ms(1000); + } + break; + } + } + + /* if start sw plugged */ + if (!sensor_get(S_START_SWITCH)) { + printf_P(PSTR("Ready, unplug start switch to start\r\n")); + /* while start sw plugged */ + while (!sensor_get(S_START_SWITCH)); + } + + strat_init(); + err = strat_main(); + NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err)); + strat_exit(); +#endif +} + +/* return true if we have to brake due to an obstacle */ +uint8_t strat_obstacle(void) +{ + int16_t x_rel, y_rel; + int16_t opp_x, opp_y, opp_d, opp_a; + + /* too slow */ + if (ABS(mainboard.speed_d) < 150) + return 0; + + /* no opponent detected */ + if (get_opponent_xyda(&opp_x, &opp_y, + &opp_d, &opp_a)) + return 0; + + /* save obstacle position */ + opponent_obstacle.x = opp_x; + opponent_obstacle.y = opp_y; + opponent_obstacle.d = opp_d; + opponent_obstacle.a = opp_a; + +#ifndef HOST_VERSION + /* sensor are temporarily disabled */ + if (sensor_obstacle_is_disabled()) + return 0; +#endif + + /* relative position */ + x_rel = cos(RAD(opp_a)) * (double)opp_d; + y_rel = sin(RAD(opp_a)) * (double)opp_d; + + /* opponent too far */ + if (opp_d > 600) + return 0; + + /* opponent is in front of us */ + if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) { + DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d " + "xrel=%d yrel=%d (speed_d=%d)", + opp_d, opp_a, x_rel, y_rel, mainboard.speed_d); +#ifndef HOST_VERSION + sensor_obstacle_disable(); +#endif + return 1; + } + /* opponent is behind us */ + if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) { + DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", + opp_d, opp_a, x_rel, y_rel); +#ifndef HOST_VERSION + sensor_obstacle_disable(); +#endif + return 1; + } + + return 0; +} + +void interrupt_traj(void) +{ + traj_intr = 1; +} + +void interrupt_traj_reset(void) +{ + traj_intr = 0; +} + +uint8_t test_traj_end(uint8_t why) +{ + uint16_t cur_timer; + point_t robot_pt; + + robot_pt.x = position_get_x_s16(&mainboard.pos); + robot_pt.y = position_get_y_s16(&mainboard.pos); + + /* trigger an event at 3 sec before the end of the match if we + * have balls in the barrel */ + cur_timer = time_get_s(); + + if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) { + /* end of match */ + if (cur_timer >= MATCH_TIME) + return END_TIMER; + } + + if ((why & END_INTR) && traj_intr) { + interrupt_traj_reset(); + return END_INTR; + } + + if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj)) + return END_TRAJ; + + /* we are near the destination point (depends on current + * speed) AND the robot is in the area bounding box. */ + if (why & END_NEAR) { + int16_t d_near = 100; + + if (mainboard.speed_d > 2000) + d_near = 150; + + if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && + is_in_boundingbox(&robot_pt)) + return END_NEAR; + } + + if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) { + strat_hardstop(); + return END_BLOCKING; + } + + if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) { + strat_hardstop(); + return END_BLOCKING; + } + + if ((why & END_OBSTACLE) && strat_obstacle()) { + strat_hardstop(); + return END_OBSTACLE; + } + + return 0; +} + +uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line) +{ + uint8_t ret = 0; + int16_t opp_x, opp_y, opp_d, opp_a; + + while (ret == 0) + ret = test_traj_end(why); + + if (ret == END_OBSTACLE) { + if (get_opponent_xyda(&opp_x, &opp_y, + &opp_d, &opp_a) == 0) + DEBUG(E_USER_STRAT, "Got %s at line %d" + " xy=(%d,%d) da=(%d,%d)", get_err(ret), + line, opp_x, opp_y, opp_d, opp_a); + } + else { + DEBUG(E_USER_STRAT, "Got %s at line %d", + get_err(ret), line); + } + return ret; +} diff --git a/projects/microb2010/tests/hostsim/strat_base.h b/projects/microb2010/tests/hostsim/strat_base.h new file mode 100644 index 0000000..27e1497 --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat_base.h @@ -0,0 +1,90 @@ +/* + * Copyright Droids Corporation, Microb Technology (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: strat_base.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +/* return values for strats and sub trajs */ +#define END_TRAJ 1 /* traj successful */ +#define END_BLOCKING 2 /* blocking during traj */ +#define END_NEAR 4 /* we are near destination */ +#define END_OBSTACLE 8 /* There is an obstacle in front of us */ +#define END_ERROR 16 /* Cannot do the command */ +#define END_INTR 32 /* interrupted by user */ +#define END_TIMER 64 /* we don't a lot of time */ +#define END_RESERVED 128 /* reserved */ + +/* only valid after a END_OBSTACLE */ +struct opponent_obstacle { + int16_t x; + int16_t y; + int16_t a; + int16_t d; +}; +extern struct opponent_obstacle opponent_obstacle; + +/* stop as fast as possible, without ramp */ +void strat_hardstop(void); + +#define DO_NOT_SET_POS -1000 +/* Reset position. If arg == DO_NOT_SET_POS, don't update value for + * it. */ +void strat_reset_pos(int16_t x, int16_t y, int16_t a); + +/* decrease gain on angle PID, and go forward until we reach the + * border. */ +uint8_t strat_calib(int16_t d, uint8_t flags); + +/* launch start procedure */ +void strat_start(void); + +/* go to an x,y point without checking for obstacle or blocking. It + * should be used for very small dist only. Return END_TRAJ if we + * reach destination, or END_BLOCKING if the robot blocked more than 3 + * times. */ +uint8_t strat_goto_xy_force(int16_t x, int16_t y); + +/* escape from disc polygon or another zone */ +struct build_zone; +uint8_t strat_escape(struct build_zone *zone, uint8_t flags); + +/* return true if we have to brake due to an obstacle */ +uint8_t strat_obstacle(void); + +/* set/get user strat speed */ +void strat_set_speed(uint16_t d, uint16_t a); +void strat_get_speed(uint16_t *d, uint16_t *a); + +/* when user type ctrl-c we can interrupt traj */ +void interrupt_traj(void); +void interrupt_traj_reset(void); + +/* limit speed when we are close of opponent */ +void strat_limit_speed_enable(void); +void strat_limit_speed_disable(void); +void strat_limit_speed(void); + +/* get name of traj error with its number */ +const char *get_err(uint8_t err); + +/* test trajectory end condition */ +uint8_t test_traj_end(uint8_t why); + +/* loop until test_traj_end() is true */ +#define wait_traj_end(why) __wait_traj_end_debug(why, __LINE__) +uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line); diff --git a/projects/microb2010/tests/hostsim/strat_utils.c b/projects/microb2010/tests/hostsim/strat_utils.c new file mode 100644 index 0000000..8fcecb2 --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat_utils.c @@ -0,0 +1,411 @@ +/* + * Copyright Droids Corporation, Microb Technology (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: strat_utils.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $ + * + */ +#include +#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 "i2c_commands.h" +#include "strat_utils.h" +#include "strat.h" + +/* return the distance between two points */ +int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2) +{ + int32_t x,y; + x = (x2-x1); + x = x*x; + y = (y2-y1); + y = y*y; + return sqrt(x+y); +} + +/* return the distance to a point in the area */ +int16_t distance_from_robot(int16_t x, int16_t y) +{ + return distance_between(position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), x, y); +} + +/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */ +int16_t simple_modulo_360(int16_t a) +{ + if (a < -180) { + a += 360; + } + else if (a > 180) { + a -= 360; + } + return a; +} + +/* return the distance to a point in the area */ +int16_t angle_abs_to_rel(int16_t a_abs) +{ + return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos)); +} + +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, + double *x_abs, double *y_abs) +{ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + *x_abs = x + d_rel*cos(a+a_rel_rad); + *y_abs = y + d_rel*sin(a+a_rel_rad); +} + +double norm(double x, double y) +{ + return sqrt(x*x + y*y); +} + +void rel_xy_to_abs_xy(double x_rel, double y_rel, + double *x_abs, double *y_abs) +{ + double d_rel, a_rel; + d_rel = norm(x_rel, y_rel); + a_rel = atan2(y_rel, x_rel); + rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs); +} + +/* return an angle between -pi and pi */ +void abs_xy_to_rel_da(double x_abs, double y_abs, + double *d_rel, double *a_rel_rad) +{ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + *a_rel_rad = atan2(y_abs - y, x_abs - x) - a; + if (*a_rel_rad < -M_PI) { + *a_rel_rad += M_2PI; + } + else if (*a_rel_rad > M_PI) { + *a_rel_rad -= M_2PI; + } + *d_rel = norm(x_abs-x, y_abs-y); +} + +void rotate(double *x, double *y, double rot) +{ + double l, a; + + l = norm(*x, *y); + a = atan2(*y, *x); + + a += rot; + *x = l * cos(a); + *y = l * sin(a); +} + +/* return true if the point is in area */ +uint8_t is_in_area(int16_t x, int16_t y, int16_t margin) +{ + if (x < margin) + return 0; + if (x > (AREA_X - margin)) + return 0; + if (y < margin) + return 0; + if (y > (AREA_Y - margin)) + return 0; + return 1; +} + + +/* return true if the point is in area */ +uint8_t robot_is_in_area(int16_t margin) +{ + return is_in_area(position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + margin); +} + +/* return true if we are near the disc */ +uint8_t robot_is_near_disc(void) +{ + if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) + return 1; + return 0; +} + +/* return 1 or 0 depending on which side of a line (y=cste) is the + * robot. works in red or green color. */ +uint8_t y_is_more_than(int16_t y) +{ + int16_t posy; + + posy = position_get_y_s16(&mainboard.pos); + if (mainboard.our_color == I2C_COLOR_RED) { + if (posy > y) + return 1; + else + return 0; + } + else { + if (posy < (AREA_Y-y)) + return 1; + else + return 0; + } +} + +/* return 1 or 0 depending on which side of a line (x=cste) is the + * robot. works in red or green color. */ +uint8_t x_is_more_than(int16_t x) +{ + int16_t posx; + + posx = position_get_x_s16(&mainboard.pos); + if (posx > x) + return 1; + else + return 0; +} + +int16_t sin_table[] = { + 0, + 3211, + 6392, + 9512, + 12539, + 15446, + 18204, + 20787, + 23170, + 25330, + 27245, + 28898, + 30273, + 31357, + 32138, + 32610, + 32767, +}; + +int16_t fast_sin(int16_t deg) +{ + deg %= 360; + + if (deg < 0) + deg += 360; + + if (deg < 90) + return sin_table[(deg*16)/90]; + else if (deg < 180) + return sin_table[((180-deg)*16)/90]; + else if (deg < 270) + return -sin_table[((deg-180)*16)/90]; + else + return -sin_table[((360-deg)*16)/90]; +} + +int16_t fast_cos(int16_t deg) +{ + return fast_sin(90+deg); +} + + +/* get the color of our robot */ +uint8_t get_color(void) +{ + return mainboard.our_color; +} + +/* get the color of the opponent robot */ +uint8_t get_opponent_color(void) +{ + if (mainboard.our_color == I2C_COLOR_RED) + return I2C_COLOR_GREEN; + else + return I2C_COLOR_RED; +} + +/* get the xy pos of the opponent robot */ +int8_t get_opponent_xy(int16_t *x, int16_t *y) +{ +#ifdef HOST_VERSION + *x = I2C_OPPONENT_NOT_THERE; + return -1; +#else + uint8_t flags; + IRQ_LOCK(flags); + *x = sensorboard.opponent_x; + *y = sensorboard.opponent_y; + IRQ_UNLOCK(flags); + if (*x == I2C_OPPONENT_NOT_THERE) + return -1; + return 0; +#endif +} + +/* get the da pos of the opponent robot */ +int8_t get_opponent_da(int16_t *d, int16_t *a) +{ +#ifdef HOST_VERSION + return -1; +#else + uint8_t flags; + int16_t x_tmp; + IRQ_LOCK(flags); + x_tmp = sensorboard.opponent_x; + *d = sensorboard.opponent_d; + *a = sensorboard.opponent_a; + IRQ_UNLOCK(flags); + if (x_tmp == I2C_OPPONENT_NOT_THERE) + return -1; + return 0; +#endif +} + +/* get the da pos of the opponent robot */ +int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a) +{ +#ifdef HOST_VERSION + *x = I2C_OPPONENT_NOT_THERE; + return -1; +#else + uint8_t flags; + IRQ_LOCK(flags); + *x = sensorboard.opponent_x; + *y = sensorboard.opponent_y; + *d = sensorboard.opponent_d; + *a = sensorboard.opponent_a; + IRQ_UNLOCK(flags); + if (*x == I2C_OPPONENT_NOT_THERE) + return -1; + return 0; +#endif +} + +#ifndef HOST_VERSION +uint8_t pump_left1_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && + (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +uint8_t pump_left2_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && + (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +uint8_t pump_right1_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && + (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +uint8_t pump_right2_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && + (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +/* number of column owned by the robot */ +uint8_t get_column_count_left(void) +{ + uint8_t ret = 0; + ret += pump_left1_is_full(); + ret += pump_left2_is_full(); + return ret; +} + +/* number of column owned by the robot */ +uint8_t get_column_count_right(void) +{ + uint8_t ret = 0; + ret += pump_right1_is_full(); + ret += pump_right2_is_full(); + return ret; +} + +/* number of column owned by the robot */ +uint8_t get_column_count(void) +{ + uint8_t ret = 0; + ret += pump_left1_is_full(); + ret += pump_left2_is_full(); + ret += pump_right1_is_full(); + ret += pump_right2_is_full(); + return ret; +} + +uint8_t get_lintel_count(void) +{ + return mechboard.lintel_count; +} + +uint8_t get_mechboard_mode(void) +{ + return mechboard.mode; +} + +uint8_t get_scanner_status(void) +{ + return sensorboard.scan_status; +} + +/* return 0 if timeout, or 1 if cond is true */ +uint8_t wait_scan_done(uint16_t timeout) +{ + uint8_t err; + err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); + return err; +} +#endif + +uint8_t opponent_is_behind(void) +{ + int8_t opp_there; + int16_t opp_d, opp_a; + + opp_there = get_opponent_da(&opp_d, &opp_a); + if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) + return 1; + return 0; +} diff --git a/projects/microb2010/tests/hostsim/strat_utils.h b/projects/microb2010/tests/hostsim/strat_utils.h new file mode 100644 index 0000000..c4687bb --- /dev/null +++ b/projects/microb2010/tests/hostsim/strat_utils.h @@ -0,0 +1,75 @@ +/* + * Copyright Droids Corporation, Microb Technology (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: strat_utils.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + + +#define DEG(x) (((double)(x)) * (180.0 / M_PI)) +#define RAD(x) (((double)(x)) * (M_PI / 180.0)) +#define M_2PI (2*M_PI) + +struct xy_point { + int16_t x; + int16_t y; +}; + +/* wait traj end flag or cond. return 0 if cond become true, else + * return the traj flag */ +#define WAIT_COND_OR_TRAJ_END(cond, mask) \ + ({ \ + uint8_t __err = 0; \ + while ( (! (cond)) && (__err == 0)) { \ + __err = test_traj_end(TRAJ_FLAGS_NO_NEAR); \ + } \ + __err; \ + }) \ + +int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2); +int16_t distance_from_robot(int16_t x, int16_t y); +int16_t simple_modulo_360(int16_t a); +int16_t angle_abs_to_rel(int16_t a_abs); +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs); +double norm(double x, double y); +void rel_xy_to_abs_xy(double x_rel, double y_rel, double *x_abs, double *y_abs); +void abs_xy_to_rel_da(double x_abs, double y_abs, double *d_rel, double *a_rel_rad); +void rotate(double *x, double *y, double rot); +uint8_t is_in_area(int16_t x, int16_t y, int16_t margin); +uint8_t robot_is_in_area(int16_t margin); +uint8_t robot_is_near_disc(void); +uint8_t y_is_more_than(int16_t y); +uint8_t x_is_more_than(int16_t x); +int16_t fast_sin(int16_t deg); +int16_t fast_cos(int16_t deg); +uint8_t get_color(void); +uint8_t get_opponent_color(void); +int8_t get_opponent_xy(int16_t *x, int16_t *y); +int8_t get_opponent_da(int16_t *d, int16_t *a); +int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a); +uint8_t get_column_count(void); +uint8_t get_column_count_right(void); +uint8_t get_column_count_left(void); +uint8_t get_lintel_count(void); +uint8_t get_mechboard_mode(void); +uint8_t pump_left1_is_full(void); +uint8_t pump_left2_is_full(void); +uint8_t pump_right1_is_full(void); +uint8_t pump_right2_is_full(void); +uint8_t get_scanner_status(void); +uint8_t wait_scan_done(uint16_t timeout); +uint8_t opponent_is_behind(void);