hostsim enhancements
authorzer0 <zer0@carbon.local>
Sun, 24 Jan 2010 23:47:45 +0000 (00:47 +0100)
committerzer0 <zer0@carbon.local>
Sun, 24 Jan 2010 23:47:45 +0000 (00:47 +0100)
33 files changed:
include/aversive.h
modules/base/hostsim/hostsim.c
modules/base/hostsim/hostsim.h
modules/base/scheduler/Makefile
modules/base/scheduler/scheduler.c
modules/base/scheduler/scheduler_stats.c [new file with mode: 0644]
modules/comm/uart/uart_host.c
modules/comm/uart/uart_host.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/.config
projects/microb2010/tests/hostsim/Makefile
projects/microb2010/tests/hostsim/actuator.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/actuator.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/cmdline.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/cmdline.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/commands.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/commands_cs.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/commands_gen.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/commands_mainboard.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/commands_traj.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/cs.c
projects/microb2010/tests/hostsim/i2c_commands.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/main.c
projects/microb2010/tests/hostsim/main.h
projects/microb2010/tests/hostsim/obstacle_avoidance_config.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/robotsim.c
projects/microb2010/tests/hostsim/strat.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/strat.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/strat_avoid.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/strat_avoid.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/strat_base.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/strat_base.h [new file with mode: 0644]
projects/microb2010/tests/hostsim/strat_utils.c [new file with mode: 0644]
projects/microb2010/tests/hostsim/strat_utils.h [new file with mode: 0644]

index c61c409..af91bf1 100644 (file)
@@ -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 */
 
 /**
index 850d23a..87bbfc6 100644 (file)
 #include <stdlib.h>
 #include <signal.h>
 #include <unistd.h>
+#include <fcntl.h>
 #include <string.h>
 #include <pthread.h>
 #include <sys/time.h>
 #include <sys/types.h>
 #include <sys/wait.h>
+#include <termios.h>
 
 #ifdef CONFIG_MODULE_SCHEDULER
 #include <scheduler.h>
 #endif
+#ifdef CONFIG_MODULE_UART
+#include <uart_host.h>
+#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 */
index b61d9e3..cc41183 100644 (file)
@@ -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);
index 1da307b..4db7b40 100644 (file)
@@ -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
index 8d0f730..32e0a79 100644 (file)
@@ -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<SCHEDULER_NB_MAX_EVENT; i++) {
-               printf_P(PSTR("task_delayed[%d]: %"PRIu32"\r\n"), i,
-                        sched_stats.task_delayed[i]);
-       }
-#endif /* CONFIG_MODULE_SCHEDULER_STATS */
-}
diff --git a/modules/base/scheduler/scheduler_stats.c b/modules/base/scheduler/scheduler_stats.c
new file mode 100644 (file)
index 0000000..3e2fe7b
--- /dev/null
@@ -0,0 +1,47 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  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: scheduler.c,v 1.9.4.6 2009-11-08 17:33:14 zer0 Exp $
+ *
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include <inttypes.h>
+
+#include <aversive/parts.h>
+#include <aversive/pgmspace.h>
+#include <aversive.h>
+
+#include <scheduler.h>
+#include <scheduler_stats.h>
+
+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<SCHEDULER_NB_MAX_EVENT; i++) {
+               printf_P(PSTR("task_delayed[%d]: %"PRIu32"\r\n"), i,
+                        sched_stats.task_delayed[i]);
+       }
+#endif /* CONFIG_MODULE_SCHEDULER_STATS */
+}
index 429c4db..bd6aeed 100644 (file)
@@ -34,13 +34,29 @@ void uart_init(void)
 event *rx_event[UART_HW_NUM];
 event *tx_event[UART_HW_NUM];
 
+void uart_host_rx_event(char c)
+{
+       /* only one uart */
+       if (rx_event[0])
+               rx_event[0](c);
+}
+
+void uart_host_tx_event(char c)
+{
+       /* only one uart */
+       if (tx_event[0])
+               tx_event[0](c);
+}
+
 int8_t uart_setconf(uint8_t num, struct uart_config *u)
 {
+       /* XXX todo */
        return 0;
 }
 
 void uart_getconf(uint8_t num, struct uart_config *u)
 {
+       return;
 }
 
 int uart_recv(uint8_t num)
@@ -48,6 +64,12 @@ int uart_recv(uint8_t num)
        return getchar();
 }
 
+/* XXX should not wait */
+int uart_recv_nowait(uint8_t num)
+{
+       return getchar();
+}
+
 int uart_send_nowait(uint8_t num, char c)
 {
        return putchar(c);
@@ -55,6 +77,5 @@ int uart_send_nowait(uint8_t num, char c)
 
 int uart_send(uint8_t num, char c)
 {
-       return put_char(c);
+       return putchar(c);
 }
-
diff --git a/modules/comm/uart/uart_host.h b/modules/comm/uart/uart_host.h
new file mode 100644 (file)
index 0000000..fec9e5d
--- /dev/null
@@ -0,0 +1,25 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  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: uart_host.c,v 1.3.4.3 2008-12-27 16:29:08 zer0 Exp $
+ *
+ */
+
+/* Olivier MATZ, Droids-corp 2004 - 2010 */
+
+void uart_host_rx_event(char c);
+void uart_host_tx_event(char c);
index a5d281a..47c7158 100644 (file)
@@ -78,7 +78,7 @@ CONFIG_MODULE_CIRBUF=y
 # CONFIG_MODULE_CIRBUF_LARGE is not set
 # CONFIG_MODULE_FIXED_POINT is not set
 CONFIG_MODULE_VECT2=y
-# CONFIG_MODULE_GEOMETRY is not set
+CONFIG_MODULE_GEOMETRY=y
 CONFIG_MODULE_HOSTSIM=y
 CONFIG_MODULE_SCHEDULER=y
 # CONFIG_MODULE_SCHEDULER_STATS is not set
@@ -171,8 +171,8 @@ CONFIG_MODULE_POSITION_MANAGER=y
 CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
 CONFIG_MODULE_TRAJECTORY_MANAGER=y
 CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
-# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
-# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+CONFIG_MODULE_OBSTACLE_AVOIDANCE=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y
 
 #
 # Control system modules
index 9fe84d3..ffc971e 100644 (file)
@@ -4,7 +4,10 @@ TARGET = main
 AVERSIVE_DIR = ../../../..
 
 # List C source files here. (C dependencies are automatically generated.)
-SRC = $(TARGET).c cs.c robotsim.c
+SRC := $(TARGET).c cs.c robotsim.c cmdline.c commands.c
+SRC += commands_cs.c commands_gen.c commands_mainboard.c
+SRC += commands_traj.c strat.c strat_base.c strat_utils.c
+SRC += actuator.c strat_avoid.c
 
 # List Assembler source files here.
 # Make them always end in a capital .S.  Files ending in a lowercase .s
diff --git a/projects/microb2010/tests/hostsim/actuator.c b/projects/microb2010/tests/hostsim/actuator.c
new file mode 100644 (file)
index 0000000..9c13288
--- /dev/null
@@ -0,0 +1,78 @@
+/*  
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: actuator.c,v 1.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+
+#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 (file)
index 0000000..9c7174a
--- /dev/null
@@ -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 (file)
index 0000000..3038036
--- /dev/null
@@ -0,0 +1,172 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  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 <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <hostsim.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#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; i<NB_LOGS+1; i++)
+                       if (gen.logs[i] == e->err_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 (file)
index 0000000..7d4521d
--- /dev/null
@@ -0,0 +1,44 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  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 (file)
index 0000000..111de4a
--- /dev/null
@@ -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 <zer0@droids-corp.org> 
+ */
+
+#include <stdlib.h>
+#include <aversive/pgmspace.h>
+#include <parse.h>
+
+/* 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 (file)
index 0000000..c9e9729
--- /dev/null
@@ -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 <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <hostsim.h>
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#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 (file)
index 0000000..92f8969
--- /dev/null
@@ -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 <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <hostsim.h>
+#include <uart.h>
+#include <time.h>
+
+#include <scheduler.h>
+#include <scheduler_stats.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#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; i<ADC_MAX; i++) {
+                       printf_P(PSTR("%.4d "), sensor_get_adc(i));
+               }
+               printf_P(PSTR("\r\n"));
+               wait_ms(100);
+       } while (loop && !cmdline_keypressed());
+#endif
+}
+
+prog_char str_adc_arg0[] = "adc";
+parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
+prog_char str_adc_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
+
+prog_char help_adc[] = "Show adc values";
+parse_pgm_inst_t cmd_adc = {
+       .f = cmd_adc_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_adc,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_adc_arg0, 
+               (prog_void *)&cmd_adc_arg1, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Sensors tests */
+
+/* this structure is filled when cmd_sensor is parsed successfully */
+struct cmd_sensor_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_sensor is parsed successfully */
+static void cmd_sensor_parsed(void *parsed_result, void *data)
+{
+#ifdef HOST_VERSION
+       printf("not implemented\n");
+#else
+       struct cmd_sensor_result *res = parsed_result;
+       uint8_t i, loop = 0;
+
+       if (!strcmp_P(res->arg1, PSTR("loop_show")))
+               loop = 1;
+       
+       do {
+               printf_P(PSTR("SENSOR values: "));
+               for (i=0; i<SENSOR_MAX; i++) {
+                       printf_P(PSTR("%d "), !!sensor_get(i));
+               }
+               printf_P(PSTR("\r\n"));
+               wait_ms(100);
+       } while (loop && !cmdline_keypressed());
+#endif
+}
+
+prog_char str_sensor_arg0[] = "sensor";
+parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
+prog_char str_sensor_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
+
+prog_char help_sensor[] = "Show sensor values";
+parse_pgm_inst_t cmd_sensor = {
+       .f = cmd_sensor_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_sensor,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_sensor_arg0, 
+               (prog_void *)&cmd_sensor_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint8_t arg2;
+       fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const prog_char uart_log[] = "uart";
+static const prog_char rs_log[] = "rs";
+static const prog_char traj_log[] = "traj";
+static const prog_char i2c_log[] = "i2c";
+static const prog_char oa_log[] = "oa";
+static const prog_char strat_log[] = "strat";
+static const prog_char i2cproto_log[] = "i2cproto";
+static const prog_char sensor_log[] = "sensor";
+static const prog_char block_log[] = "bd";
+static const prog_char cs_log[] = "cs";
+
+struct log_name_and_num {
+       const prog_char * name;
+       uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+       { uart_log, E_UART },
+       { rs_log, E_ROBOT_SYSTEM },
+       { traj_log, E_TRAJECTORY },
+       { i2c_log, E_I2C },
+       { oa_log, E_OA },
+       { strat_log, E_USER_STRAT },
+       { i2cproto_log, E_USER_I2C_PROTO },
+       { sensor_log, E_USER_SENSOR },
+       { block_log, E_BLOCKING_DETECTION_MANAGER },
+       { cs_log, E_USER_CS },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+       uint8_t i;
+       
+       for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+               if (!strcmp_P(s, log_name_and_num[i].name)) {
+                       return log_name_and_num[i].num;
+               }
+       }
+       return 0;
+}
+
+const prog_char *
+log_num2name(uint8_t num)
+{
+       uint8_t i;
+       
+       for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+               if (num ==  log_name_and_num[i].num) {
+                       return log_name_and_num[i].name;
+               }
+       }
+       return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+       uint8_t i, empty=1;
+       const prog_char * name;
+
+       printf_P(PSTR("log level is %d\r\n"), gen.log_level);
+       for (i=0; i<NB_LOGS; i++) {
+               name = log_num2name(gen.logs[i]);
+               if (name) {
+#ifdef HOST_VERSION
+                       printf_P(PSTR("log type %s is on\r\n"), name);
+#else
+                       printf_P(PSTR("log type %S is on\r\n"), name);
+#endif
+                       empty = 0;
+               }
+       }
+       if (empty)
+               printf_P(PSTR("no log configured\r\n"));
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, void * data)
+{
+       struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+       if (!strcmp_P(res->arg1, 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; i<NB_LOGS; i++) {
+                       if (gen.logs[i] == lognum) {
+                               printf_P(PSTR("Already on\r\n"));
+                               return;
+                       }
+               }
+               for (i=0; i<NB_LOGS; i++) {
+                       if (gen.logs[i] == 0) {
+                               gen.logs[i] = lognum;
+                               break;
+                       }
+               }
+               if (i==NB_LOGS) {
+                       printf_P(PSTR("no more room\r\n"));
+               }
+       }
+       else if (!strcmp_P(res->arg3, PSTR("off"))) {
+               for (i=0; i<NB_LOGS; i++) {
+                       if (gen.logs[i] == lognum) {
+                               gen.logs[i] = 0;
+                               break;
+                       }
+               }
+               if (i==NB_LOGS) {
+                       printf_P(PSTR("already off\r\n"));
+               }
+       }
+       cmd_log_do_show();
+}
+
+prog_char str_log_arg1_type[] = "type";
+parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+prog_char str_log_arg2_type[] = "uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor#bd#cs";
+parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+prog_char str_log_arg3[] = "on#off";
+parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+prog_char help_log_type[] = "Set log type";
+parse_pgm_inst_t cmd_log_type = {
+       .f = cmd_log_type_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_log_type,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_log_arg0,
+               (prog_void *)&cmd_log_arg1_type,
+               (prog_void *)&cmd_log_arg2_type,
+               (prog_void *)&cmd_log_arg3,
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(void *parsed_result, void *data)
+{
+#ifdef HOST_VERSION
+       printf("not implemented\n");
+#else
+       printf("res stack: %d\r\n", min_stack_space_available());
+#endif
+}
+
+prog_char str_stack_space_arg0[] = "stack_space";
+parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+prog_char help_stack_space[] = "Display remaining stack space";
+parse_pgm_inst_t cmd_stack_space = {
+       .f = cmd_stack_space_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_stack_space,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_stack_space_arg0, 
+               NULL,
+       },
+};
diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c
new file mode 100644 (file)
index 0000000..0de0b2d
--- /dev/null
@@ -0,0 +1,2236 @@
+/*
+ *  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_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <hostsim.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <aversive/eeprom.h>
+
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#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, &center_abs_x, &center_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 (file)
index 0000000..55bfe5f
--- /dev/null
@@ -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 <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <hostsim.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#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 ; i<pt_list_len ; i++) {
+               printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
+               if (!strcmp_P(res->arg1, 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,
+       },
+};
index cbb65d6..c5ecf27 100644 (file)
@@ -43,6 +43,8 @@
 #include <rdline.h>
 
 #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 (file)
index 0000000..fdb5d49
--- /dev/null
@@ -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_ */
index ec70479..85a31a2 100644 (file)
@@ -46,6 +46,7 @@
 #include <hostsim.h>\r
 \r
 #include "cs.h"\r
+#include "cmdline.h"\r
 #include "main.h"\r
 \r
 struct genboard gen;\r
@@ -53,14 +54,23 @@ struct mainboard mainboard;
 \r
 int main(void)\r
 {\r
-#ifndef HOST_VERSION\r
        uart_init();\r
+       uart_register_rx_event(CMDLINE_UART, emergency);\r
+#ifndef HOST_VERSION\r
        fdevopen(uart0_dev_send, uart0_dev_recv);\r
        sei();\r
-#else\r
-       int i;\r
 #endif\r
 \r
+       memset(&gen, 0, sizeof(gen));\r
+       memset(&mainboard, 0, sizeof(mainboard));\r
+\r
+       /* LOGS */\r
+       error_register_emerg(mylog);\r
+       error_register_error(mylog);\r
+       error_register_warning(mylog);\r
+       error_register_notice(mylog);\r
+       error_register_debug(mylog);\r
+\r
 #ifdef CONFIG_MODULE_TIMER\r
        timer_init();\r
 #endif\r
@@ -71,21 +81,24 @@ int main(void)
        hostsim_init();\r
        robotsim_init();\r
 #endif\r
-       time_init(TIME_PRIO);\r
-\r
-\r
        microb_cs_init();\r
 \r
-       gen.logs[0] = E_USER_CS;\r
-       gen.log_level = 5;\r
+       time_init(TIME_PRIO);\r
+\r
+       printf_P(PSTR("\r\n"));\r
+       printf_P(PSTR("Respect et robustesse.\r\n"));\r
 \r
        mainboard.flags = DO_ENCODERS | DO_RS |\r
                DO_POS | DO_POWER | DO_BD | DO_CS;\r
+       strat_reset_pos(1000, 1000, 0);\r
+\r
+       cmdline_interact();\r
 \r
-       trajectory_d_rel(&mainboard.traj, 1000);\r
-       time_wait_ms(2000);\r
-       trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000);\r
-       time_wait_ms(2000);\r
+/*     trajectory_d_rel(&mainboard.traj, 1000); */\r
+/*     time_wait_ms(2000); */\r
+/*     trajectory_circle_rel(&mainboard.traj, 1500, 1000, */\r
+/*                           250, 360, 0); */\r
+/*     time_wait_ms(15000); */\r
        return 0;\r
 }\r
 \r
index 5f07ab4..73e3da7 100644 (file)
@@ -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 (file)
index 0000000..dbf2cab
--- /dev/null
@@ -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
index 7ed49f4..990104b 100644 (file)
@@ -25,6 +25,7 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
+#include <unistd.h>
 
 #include <aversive.h>
 #include <aversive/error.h>
@@ -44,6 +45,8 @@
 #include <parse.h>
 #include <rdline.h>
 
+#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 (file)
index 0000000..ab6de1d
--- /dev/null
@@ -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 <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#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; i<BEACON_MAX_SAMPLES; i++) {
+               printf_P(PSTR("%d: pos=(%d,%d,%d) opp=(%d,%d) time=%d\r\n"),
+                        i,
+                        beacon_sample[i].posx,
+                        beacon_sample[i].posy,
+                        beacon_sample[i].posa,
+                        beacon_sample[i].oppx,
+                        beacon_sample[i].oppy,
+                        beacon_sample[i].time);
+       }
+}
+#endif
+
+struct strat_infos strat_infos = {
+       /* conf */
+       .conf = {
+               .flags = 0,
+               /* scanner disabled by default */
+               .scan_opp_min_time = 90,
+               .delay_between_opp_scan = 90,
+               .scan_our_min_time = 90,
+               .delay_between_our_scan = 90,
+               .wait_opponent = 0,
+               .lintel_min_time = 0,
+               .scan_opp_angle = -1,
+       },
+
+       /* static columns */
+       .s_cols = { 
+               .flags = 0, 
+               .configuration = 0,
+       },
+
+       /* column dispensers ; be carreful, positions are
+        * color-dependent, so COLOR_Y() and COLOR_A() should be
+        * used. All angles here are _absolute_ */
+       .c1 = {
+               .checkpoint_x = 2711 - COL_SCAN_PRE_MARGIN,
+               .checkpoint_y = AREA_Y - COL_DISP_MARGIN,
+               .scan_left = 0,
+               .scan_a = 180,
+               .eject_a = 180,
+               .recalib_x = 2711,
+               .recalib_y = AREA_Y - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
+               .pickup_a = 90,
+               .name = "col_disp1",
+       },
+       .c2 = {
+               .checkpoint_x = AREA_X - COL_DISP_MARGIN,
+               .checkpoint_y = 800 - COL_SCAN_PRE_MARGIN,
+               .scan_left = 1,
+               .scan_a = -90,
+               .eject_a = -90,
+               .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
+               .recalib_y = 800,
+               .pickup_a = 0,
+               .name = "col_disp2",
+       },
+       .c3 = {
+               .checkpoint_x = AREA_X-COL_DISP_MARGIN,
+               .checkpoint_y = 1300 + COL_SCAN_PRE_MARGIN,
+               .scan_a = 90,
+               .scan_left = 0,
+               .eject_a = -90,
+               .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
+               .recalib_y = 1300,
+               .pickup_a = 0,
+               .name = "col_disp3",
+       },
+       
+       /* lintel dispensers */
+       .l1 = {
+               .x = 912, /* XXX for red only */
+               .name = "lin_disp1",
+       },
+       .l2 = {
+               .x = 1312,  /* XXX for red only */
+               .name = "lin_disp2",
+       },
+
+       /* build zones */
+       .zone_list = {
+#define ZONE_DISC_NUM 0
+               {
+                       .flags = ZONE_F_VALID | ZONE_F_DISC,
+                       .level = 2,
+                       .checkpoint_x = 0,
+                       .checkpoint_x = 0,
+                       .name = "disc",
+               },
+#define ZONE_1A_NUM 1
+               {
+                       .flags = ZONE_F_VALID,
+                       .level = 1,
+                       .checkpoint_x = 1385,
+                       .checkpoint_y = 1700,
+                       .name = "z1a",
+               },
+#define ZONE_1B_NUM 2
+               {
+                       .flags = ZONE_F_VALID,
+                       .level = 1,
+                       .checkpoint_x = 1615,
+                       .checkpoint_y = 1700,
+                       .name = "z1b",
+               },
+#define ZONE_0B_NUM 3
+               {
+                       .flags = ZONE_F_VALID,
+                       .level = 0,
+                       .checkpoint_x = 2100,
+                       .checkpoint_y = 1700,
+                       .name = "z0b",
+               },
+#define ZONE_0A_NUM 4
+               {
+                       .flags = ZONE_F_VALID,
+                       .level = 0,
+                       .checkpoint_x = 900,
+                       .checkpoint_y = 1700,
+                       .name = "z0a",
+               },
+       }
+};
+
+/*************************************************************/
+
+/*                  INIT                                     */
+
+/*************************************************************/
+
+void strat_set_bounding_box(void)
+{
+       if (get_color() == I2C_COLOR_RED) {
+               strat_infos.area_bbox.x1 = 300;
+               strat_infos.area_bbox.y1 = 200;
+               strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
+               strat_infos.area_bbox.y2 = 1800;
+       }
+       else {
+               strat_infos.area_bbox.x1 = 200;
+               strat_infos.area_bbox.y1 = 300;
+               strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
+               strat_infos.area_bbox.y2 = 1900;
+       }
+
+       polygon_set_boundingbox(strat_infos.area_bbox.x1,
+                               strat_infos.area_bbox.y1,
+                               strat_infos.area_bbox.x2,
+                               strat_infos.area_bbox.y2);
+}
+
+/* called before each strat, and before the start switch */
+void strat_preinit(void)
+{
+       time_reset();
+       interrupt_traj_reset();
+       mainboard.flags =  DO_ENCODERS | DO_CS | DO_RS |
+               DO_POS | DO_BD | DO_POWER;
+
+#ifndef HOST_VERSION
+       i2c_mechboard_mode_init();
+       if (get_color() == I2C_COLOR_RED)
+               i2c_mechboard_mode_prepare_pickup(I2C_LEFT_SIDE);
+       else
+               i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
+#endif
+       strat_dump_conf();
+       strat_dump_infos(__FUNCTION__);
+}
+
+void strat_dump_conf(void)
+{
+       if (!strat_infos.dump_enabled)
+               return;
+
+       printf_P(PSTR("-- conf --\r\n"));
+
+       printf_P(PSTR("  one build on disc: "));
+       if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  bypass static2: "));
+       if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  take one lintel: "));
+       if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  skip this temple when temple check fails: "));
+       if (strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  store static2: "));
+       if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  (big3) try to build a temple with 3 lintels: "));
+       if (strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  early opponent scan: "));
+       if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  push opponent columns: "));
+       if (strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS)
+               printf_P(PSTR("on\r\n"));
+       else
+               printf_P(PSTR("off\r\n"));
+
+       printf_P(PSTR("  scan opponent min time: %d\r\n"),
+                strat_infos.conf.scan_opp_min_time);
+       printf_P(PSTR("  delay between oppnent scan: %d\r\n"),
+                strat_infos.conf.delay_between_opp_scan);
+       printf_P(PSTR("  scan our min time: %d\r\n"),
+                strat_infos.conf.scan_our_min_time);
+       printf_P(PSTR("  delay between our scan: %d\r\n"),
+                strat_infos.conf.delay_between_our_scan);
+       printf_P(PSTR("  wait opponent gone before scan: %d\r\n"),
+                strat_infos.conf.wait_opponent);
+       printf_P(PSTR("  lintel min time: %d\r\n"),
+                strat_infos.conf.lintel_min_time);
+       printf_P(PSTR("  scan_opp_angle: %d\r\n"),
+                strat_infos.conf.scan_opp_angle);
+}
+
+void strat_dump_temple(struct temple *temple)
+{
+       if (!strat_infos.dump_enabled)
+               return;
+
+       printf_P(PSTR("  temple %p (%s): "), temple, temple->zone->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; i<MAX_TEMPLE; i++) {
+               temple = &strat_infos.temple_list[i];
+               if (!(temple->flags & 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; i<MAX_ZONE; i++) {
+               zone = &strat_infos.zone_list[i];
+               if (!(zone->flags & 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<MAX_ZONE; i++)
+               strat_infos.zone_list[i].flags = ZONE_F_VALID;
+       strat_infos.zone_list[ZONE_DISC_NUM].flags |= ZONE_F_DISC;
+       strat_infos.zone_list[ZONE_1A_NUM].flags |= ZONE_F_ZONE1;
+       strat_infos.zone_list[ZONE_1B_NUM].flags |= ZONE_F_ZONE1;
+       strat_infos.zone_list[ZONE_0A_NUM].flags |= ZONE_F_ZONE0;
+       strat_infos.zone_list[ZONE_0B_NUM].flags |= ZONE_F_ZONE0;
+
+       strat_set_bounding_box();
+
+       /* set lintel position, depending on color */
+       if (mainboard.our_color == I2C_COLOR_RED) {
+               strat_infos.l1.x = 912;
+               strat_infos.l2.x = 1312;
+       }
+       else {
+               strat_infos.l1.x = 888;
+               strat_infos.l2.x = 1288;
+       }
+}
+
+/* call it just before launching the strat */
+void strat_init(void)
+{
+#ifdef HOST_VERSION
+       printf("not implemented\n");
+#else
+       pickup_wheels_on();
+       strat_reset_infos();
+
+       /* we consider that the color is correctly set */
+
+       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+       time_reset();
+       interrupt_traj_reset();
+
+       /* used in strat_base for END_TIMER */
+       mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | 
+               DO_POS | DO_BD | DO_TIMER | DO_POWER;
+
+#ifdef TEST_BEACON
+       beacon_prev_time = 0;
+       beacon_cur_idx = 0;
+#endif
+#endif
+}
+
+
+/* call it after each strat */
+void strat_exit(void)
+{
+#ifdef HOST_VERSION
+       printf("not implemented\n");
+#else
+       uint8_t flags;
+
+       pickup_wheels_off();
+       mainboard.flags &= ~(DO_TIMER);
+       strat_hardstop();
+       time_reset();
+       wait_ms(1000);
+       IRQ_LOCK(flags);
+       mainboard.flags &= ~(DO_CS);
+       pwm_ng_set(LEFT_PWM, 0);
+       pwm_ng_set(RIGHT_PWM, 0);
+       IRQ_UNLOCK(flags);
+#endif
+}
+
+/* called periodically */
+void strat_event(void *dummy)
+{
+       /* limit speed when opponent is close */
+       strat_limit_speed();
+
+#ifdef TEST_BEACON
+       beacon_update_samples();
+#endif
+}
+
+#ifndef HOST_VERSION
+/* do static cols + first temples */
+static uint8_t strat_beginning(void)
+{
+       uint8_t err;
+
+       /* don't limit the speed when opponent is near: it can change
+        * the radius of the curves */
+       strat_limit_speed_disable();
+
+       err = strat_static_columns(0);
+
+       strat_limit_speed_enable();
+
+       if (!TRAJ_SUCCESS(err))
+               return err;
+
+       /* go to disc to build the first temple */
+
+       /* XXX if opponent is near disc, go to zone1 */
+       err = strat_goto_disc(2);
+       if (!TRAJ_SUCCESS(err))
+               return err;
+       DEBUG(E_USER_STRAT, "disc reached");
+
+       /* can return END_ERROR or END_TIMER, should not happen
+        * here */
+       err = strat_build_new_temple(&strat_infos.zone_list[0]);
+       if (!TRAJ_SUCCESS(err))
+               return err;
+
+       /* bypass static2 if specified */
+       if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2) {
+               err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
+               return err;
+       }
+
+       /* get the last 2 columns, and build them on previous temple */
+       err = strat_static_columns_pass2();
+       if (!TRAJ_SUCCESS(err))
+               return err;
+
+       /* early opponent scan, for offensive strategy */
+       if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) {
+               err = strat_pickup_lintels();
+               /* ignore code */
+
+               /* try to build on opponent (scan must be enabled) */
+               err = strat_build_on_opponent_temple();
+               /* ignore code */
+       }
+
+       return err;
+}
+#endif
+
+/* return true if we need to grab some more elements (lintel/cols) */
+uint8_t need_more_elements(void)
+{
+#ifdef HOST_VERSION
+       printf("not implemented\n");
+#else
+       if (time_get_s() <= 75) {
+               /* we have at least one col on each arm, build now */
+               if ((get_column_count_left() >= 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 (file)
index 0000000..2b2b96a
--- /dev/null
@@ -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 (file)
index 0000000..6a337ed
--- /dev/null
@@ -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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#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 ; i<len ; i++) {
+               DEBUG(E_USER_STRAT, "With avoidance %d: x=%"PRIi32" y=%"PRIi32"", i, p->x, 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 (file)
index 0000000..0978fd7
--- /dev/null
@@ -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 (file)
index 0000000..0dd1a8b
--- /dev/null
@@ -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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#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 <<i))
+                       return err_tab[i];
+       }
+       return "END_UNKNOWN";
+}
+
+void strat_hardstop(void) 
+{
+       trajectory_hardstop(&mainboard.traj);
+       pid_reset(&mainboard.angle.pid);
+       pid_reset(&mainboard.distance.pid);
+       bd_reset(&mainboard.angle.bd);
+       bd_reset(&mainboard.distance.bd);
+
+       while ((ABS(mainboard.speed_d) > 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 (file)
index 0000000..27e1497
--- /dev/null
@@ -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 (file)
index 0000000..8fcecb2
--- /dev/null
@@ -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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#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 (file)
index 0000000..c4687bb
--- /dev/null
@@ -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&nbs