hostsim enhancements, some bugs remaining (freeze sometimes)
authorzer0 <zer0@carbon.local>
Mon, 25 Jan 2010 00:07:11 +0000 (01:07 +0100)
committerzer0 <zer0@carbon.local>
Mon, 25 Jan 2010 00:07:11 +0000 (01:07 +0100)
15 files changed:
1  2 
modules/base/hostsim/hostsim.c
modules/base/hostsim/hostsim.h
modules/base/time/clock_time.h
modules/devices/robot/obstacle_avoidance/obstacle_avoidance.c
projects/microb2010/tests/hostsim/.config
projects/microb2010/tests/hostsim/Makefile
projects/microb2010/tests/hostsim/cmdline.c
projects/microb2010/tests/hostsim/commands_mainboard.c
projects/microb2010/tests/hostsim/commands_traj.c
projects/microb2010/tests/hostsim/cs.c
projects/microb2010/tests/hostsim/main.c
projects/microb2010/tests/hostsim/robotsim.c
projects/microb2010/tests/hostsim/strat.c
projects/microb2010/tests/hostsim/strat_base.c
projects/microb2010/tests/hostsim/strat_utils.c

  #ifdef CONFIG_MODULE_SCHEDULER
  #include <scheduler.h>
  #endif
 +#ifdef CONFIG_MODULE_UART
 +#include <uart_host.h>
 +#endif
  
pthread_mutex_t mut;
static pthread_mutex_t mut = PTHREAD_MUTEX_INITIALIZER;
  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,
@@@ -242,11 -138,9 +262,9 @@@ int hostsim_uart_init(void
  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);
        parent_id = pthread_self();
  
        pthread_mutex_lock(&mut);
  
        return 0;
  }
- void hostsim_exit(void)
 +
++int hostsim_exit(void)
 +{
 +#ifdef CONFIG_MODULE_UART
 +      tcsetattr(0, TCSANOW, &oldterm);
 +#endif
++      return 0;
 +}
  #endif /* HOST_VERSION */
Simple merge
index 0000000,4e1fcec..02e8fe6
mode 000000,100644..100644
--- /dev/null
@@@ -1,0 -1,96 +1,95 @@@
 -#ifndef _TIME_H_
 -#define _TIME_H_
+ /*  
+  *  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: time.h,v 1.3.4.2 2007-05-23 17:18:11 zer0 Exp $
+  *
+  */
+ /* Droids-corp, Eirbot, Microb Technology 2005 - Zer0
+  * Interface of the time module
+  */
+ /**
+  *  This module can be used to get a human readable time. It uses the
+  *  scheduler module. Its goal is not to be very precise, but just
+  *  simple to use.  provides two timers: one in s and us, and one in
+  *  us which doesn't overflow on seconds (better to substract two
+  *  times)
+  */
 -
++#ifndef _CLOCK_TIME_H_
++#define _CLOCK_TIME_H_
+ #include <aversive.h>
+ /* a 16 bit variable cannot cover one day */
+ typedef int32_t seconds; 
+ typedef int32_t microseconds;
+ /** the time structure */
+ typedef struct 
+ {
+   microseconds us;
+   seconds s;
+ } time_h;
+ /**********************************************************/
+ /** init time module : schedule the event with the givent priority */
+ void time_init(uint8_t priority);
+ /**********************************************************/
+ /** get time in second since last init/reset */
+ seconds time_get_s(void);
+ /**********************************************************/
+ /** get time in microsecond since last init/reset */
+ microseconds time_get_us(void);
+ /**********************************************************/
+ /** get the complete time struct since last init/reset */
+ time_h time_get_time(void); 
+ /**********************************************************/
+ /** reset time counter */
+ void time_reset(void);
+ /**********************************************************/
+ /** set time */
+ void time_set(seconds s, microseconds us);
+ /**********************************************************/
+ /** This is an equivalent of 'wait_ms(x)', but uses time value, so it
+  *  is independant of CPU load. Warning, you should not use this
+  *  function in a irq locked context, or in a scheduled function with
+  *  higher priority than time module */
+ void time_wait_ms(uint16_t ms);
+ /**********************************************************/
+ /** get a microsecond timer that overflows naturally */
+ microseconds time_get_us2(void);
+ #endif
@@@ -164,7 -164,7 +164,7 @@@ void oa_dump(void
                printf_P(PSTR("poly #%d\r\n"), i);
                for (j=0; j<poly->l; j++) {
                        pt = &poly->pts[j];
--                      printf_P(PSTR("  pt #%d (%"PRIi32",%"PRIi32")\r\n"), j, pt->x, pt->y);
++                      printf_P(PSTR("  pt #%d (%2.2f,%2.2f)\r\n"), j, pt->x, pt->y);
                }
        }
  }
@@@ -1,5 -1,5 +1,5 @@@
  #
--# Automatically generated by make menuconfig: don't edit
++# Automatically generated make config: don't edit
  #
  
  #
@@@ -74,6 -74,6 +74,10 @@@ CONFIG_FORMAT_IHEX=
  #
  # Base modules
  #
++
++#
++# Enable math library in generation options to see all modules
++#
  CONFIG_MODULE_CIRBUF=y
  # CONFIG_MODULE_CIRBUF_LARGE is not set
  # CONFIG_MODULE_FIXED_POINT is not set
@@@ -94,6 -94,6 +98,10 @@@ CONFIG_MODULE_TIME_CREATE_CONFIG=
  #
  # Communication modules
  #
++
++#
++# uart needs circular buffer, mf2 client may need scheduler
++#
  CONFIG_MODULE_UART=y
  # CONFIG_MODULE_UART_9BITS is not set
  # CONFIG_MODULE_UART_CREATE_CONFIG is not set
@@@ -178,6 -178,6 +186,10 @@@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE
  # Control system modules
  #
  CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
++
++#
++# Filters
++#
  CONFIG_MODULE_PID=y
  CONFIG_MODULE_PID_CREATE_CONFIG=y
  # CONFIG_MODULE_RAMP is not set
@@@ -188,12 -188,12 +200,20 @@@ CONFIG_MODULE_QUADRAMP=
  #
  # Radio devices
  #
++
++#
++# Some radio devices require SPI to be activated
++#
  # CONFIG_MODULE_CC2420 is not set
  # CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
  
  #
  # Crypto modules
  #
++
++#
++# Crypto modules depend on utils module
++#
  # CONFIG_MODULE_AES is not set
  # CONFIG_MODULE_AES_CTR is not set
  # CONFIG_MODULE_MD5 is not set
  #
  # Encodings modules
  #
++
++#
++# Encoding modules depend on utils module
++#
  # CONFIG_MODULE_BASE64 is not set
  # CONFIG_MODULE_HAMMING is not set
  
  #
  # Debug modules
  #
++
++#
++# Debug modules depend on utils module
++#
  # CONFIG_MODULE_DIAGNOSTIC is not set
  # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
  CONFIG_MODULE_ERROR=y
@@@ -18,6 -15,6 +18,8 @@@ SRC += actuator.c strat_avoid.
  # care about how the name is spelled on its command-line.
  ASRC = 
  
++CFLAGS += -Wall -Werror
++
  -include .aversive_conf
  include $(AVERSIVE_DIR)/mk/aversive_project.mk
  
index 3038036,0000000..77392ec
mode 100644,000000..100644
--- /dev/null
@@@ -1,172 -1,0 +1,172 @@@
- #include <time.h>
 +/*  
 + *  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 <clock_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;
 +}
index 0de0b2d,0000000..b07983b
mode 100644,000000..100644
--- /dev/null
@@@ -1,2236 -1,0 +1,2236 @@@
- #include <time.h>
 +/*
 + *  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 <clock_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,
 +      },
 +};
index 55bfe5f,0000000..0e23798
mode 100644,000000..100644
--- /dev/null
@@@ -1,1213 -1,0 +1,1213 @@@
- #include <time.h>
 +/*
 + *  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 <clock_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,
 +      },
 +};
  #include <hostsim.h>\r
  \r
  #include "cs.h"\r
 +#include "cmdline.h"\r
++#include "robotsim.h"\r
++#include "strat.h"\r
++#include "strat_base.h"\r
  #include "main.h"\r
  \r
  struct genboard gen;\r
@@@ -54,51 -53,29 +57,47 @@@ struct mainboard mainboard
  \r
  int main(void)\r
  {\r
-       scheduler_init();\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
 +#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
++\r
        printf("init\n");\r
  \r
- #ifdef HOST_VERSION\r
+       scheduler_init();\r
 -      time_init(TIME_PRIO);\r
\r
        hostsim_init();\r
        robotsim_init();\r
- #endif\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_circle_rel(&mainboard.traj, 1500, 1000, */\r
- /*                          250, 360, 0); */\r
- /*    time_wait_ms(15000); */\r
 -      time_wait_ms(1000);\r
 -      printf("init\n");\r
 -      trajectory_d_rel(&mainboard.traj, 1000);\r
 -      time_wait_ms(2000);\r
 -      printf("init\n");\r
 -      trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000);\r
 -      time_wait_ms(2000);\r
        return 0;\r
  }\r
  \r
  #include <stdio.h>
  #include <string.h>
  #include <stdint.h>
- #include <sys/types.h>
+ #include <unistd.h>
  #include <sys/stat.h>
+ #include <sys/types.h>
  #include <fcntl.h>
 +#include <unistd.h>
  
  #include <aversive.h>
  #include <aversive/error.h>
index ab6de1d,0000000..4cc17fb
mode 100644,000000..100644
--- /dev/null
@@@ -1,820 -1,0 +1,820 @@@
- #include <time.h>
 +/*  
 + *  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 <clock_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
 +}
index 0dd1a8b,0000000..19e9cf6
mode 100644,000000..100644
--- /dev/null
@@@ -1,515 -1,0 +1,515 @@@
- #include <time.h>
 +/*  
 + *  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 <clock_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;
 +}
index 8fcecb2,0000000..57c5a0b
mode 100644,000000..100644
--- /dev/null
@@@ -1,411 -1,0 +1,411 @@@
-       int16_t opp_d, opp_a;
 +/*  
 + *  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 = 0, opp_a = 0;
 +
 +      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;
 +}