code 2010
authorOlivier Matz <zer0@droids-corp.org>
Sun, 21 Mar 2010 00:19:05 +0000 (01:19 +0100)
committerOlivier Matz <zer0@droids-corp.org>
Sun, 21 Mar 2010 00:19:05 +0000 (01:19 +0100)
56 files changed:
projects/microb2010/cobboard/Makefile
projects/microb2010/cobboard/actuator.c
projects/microb2010/cobboard/actuator.h
projects/microb2010/cobboard/commands.c
projects/microb2010/cobboard/commands_cobboard.c
projects/microb2010/cobboard/cs.c
projects/microb2010/cobboard/main.c
projects/microb2010/cobboard/main.h
projects/microb2010/cobboard/sensor.c
projects/microb2010/cobboard/sensor.h
projects/microb2010/cobboard/spickle.c [new file with mode: 0644]
projects/microb2010/cobboard/spickle.h [new file with mode: 0644]
projects/microb2010/common/i2c_commands.h
projects/microb2010/mainboard/.config
projects/microb2010/mainboard/Makefile
projects/microb2010/mainboard/actuator.c
projects/microb2010/mainboard/actuator.h
projects/microb2010/mainboard/ax12_user.c
projects/microb2010/mainboard/cmdline.c
projects/microb2010/mainboard/commands.c
projects/microb2010/mainboard/commands_ax12.c
projects/microb2010/mainboard/commands_cs.c
projects/microb2010/mainboard/commands_gen.c
projects/microb2010/mainboard/commands_mainboard.c
projects/microb2010/mainboard/commands_traj.c
projects/microb2010/mainboard/cs.c
projects/microb2010/mainboard/i2c_protocol.c
projects/microb2010/mainboard/i2c_protocol.h
projects/microb2010/mainboard/main.c
projects/microb2010/mainboard/main.h
projects/microb2010/mainboard/strat.c
projects/microb2010/mainboard/strat.h
projects/microb2010/mainboard/strat_avoid.c [deleted file]
projects/microb2010/mainboard/strat_avoid.h [deleted file]
projects/microb2010/mainboard/strat_base.c
projects/microb2010/mainboard/strat_base.h
projects/microb2010/mainboard/strat_building.c [deleted file]
projects/microb2010/mainboard/strat_column_disp.c [deleted file]
projects/microb2010/mainboard/strat_lintel.c [deleted file]
projects/microb2010/mainboard/strat_scan.c [deleted file]
projects/microb2010/mainboard/strat_static_columns.c [deleted file]
projects/microb2010/mainboard/strat_utils.c
projects/microb2010/mainboard/strat_utils.h
projects/microb2010/microb_cmd/microbcmd.py
projects/microb2010/tests/beacon_tsop/.config
projects/microb2010/tests/beacon_tsop/Makefile
projects/microb2010/tests/beacon_tsop/board2006.h [new file with mode: 0644]
projects/microb2010/tests/beacon_tsop/board2010.h [new file with mode: 0644]
projects/microb2010/tests/beacon_tsop/cmdline.h
projects/microb2010/tests/beacon_tsop/commands.c
projects/microb2010/tests/beacon_tsop/hfuse
projects/microb2010/tests/beacon_tsop/lfuse
projects/microb2010/tests/beacon_tsop/main.c
projects/microb2010/tests/beacon_tsop/uart_config.h
projects/microb2010/tests/beacon_tsop/uart_proto.c [new file with mode: 0644]
projects/microb2010/tests/beacon_tsop/uart_proto.h [new file with mode: 0644]

index 0c93238..8293a3f 100644 (file)
@@ -11,7 +11,7 @@ LDFLAGS = -T ../common/avr6.x
 SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c 
 SRC += commands_cs.c commands_cobboard.c commands.c
 SRC += i2c_protocol.c sensor.c actuator.c cs.c
-SRC += state.c ax12_user.c
+SRC += state.c ax12_user.c spickle.c
 
 # List Assembler source files here.
 # Make them always end in a capital .S.  Files ending in a lowercase .s
index 67598ea..8c15480 100644 (file)
@@ -40,9 +40,9 @@
 
 #include <rdline.h>
 
+#include "sensor.h"
 #include "../common/i2c_commands.h"
 #include "actuator.h"
-#include "ax12_user.h"
 #include "main.h"
 
 void servo_carry_open(void)
index cd2e28b..7ef1572 100644 (file)
  *
  */
 
+#ifndef _ACTUATOR_H_
+#define _ACTUATOR_H_
+
 void actuator_init(void);
 void servo_carry_open(void);
 void servo_carry_close(void);
 void servo_door_open(void);
 void servo_door_close(void);
 
+#endif
+
index 1723f2a..ca5ded6 100644 (file)
@@ -72,6 +72,9 @@ extern parse_pgm_inst_t cmd_state_debug;
 extern parse_pgm_inst_t cmd_state_machine;
 extern parse_pgm_inst_t cmd_servo_door;
 extern parse_pgm_inst_t cmd_servo_carry;
+extern parse_pgm_inst_t cmd_spickle;
+extern parse_pgm_inst_t cmd_spickle_params;
+extern parse_pgm_inst_t cmd_spickle_params_show;
 extern parse_pgm_inst_t cmd_test;
 
 
@@ -126,6 +129,9 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_state_machine,
        (parse_pgm_inst_t *)&cmd_servo_door,
        (parse_pgm_inst_t *)&cmd_servo_carry,
+       (parse_pgm_inst_t *)&cmd_spickle,
+       (parse_pgm_inst_t *)&cmd_spickle_params,
+       (parse_pgm_inst_t *)&cmd_spickle_params_show,
        (parse_pgm_inst_t *)&cmd_test,
 
        NULL,
index b822e4a..3c6e3e3 100644 (file)
@@ -50,7 +50,7 @@
 #include "state.h"
 #include "i2c_protocol.h"
 #include "actuator.h"
-#include "actuator.h"
+#include "spickle.h"
 
 extern uint16_t state_debug;
 
@@ -449,6 +449,122 @@ parse_pgm_inst_t cmd_servo_carry = {
 };
 
 /**********************************************************/
+/* Spickles tests */
+
+/* this structure is filled when cmd_spickle is parsed successfully */
+struct cmd_spickle_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_spickle is parsed successfully */
+static void cmd_spickle_parsed(void * parsed_result,
+                              __attribute__((unused)) void *data)
+{
+       struct cmd_spickle_result * res = parsed_result;
+       
+       if (!strcmp_P(res->arg1, PSTR("up"))) {
+               spickle_up();
+       }
+       else if (!strcmp_P(res->arg1, PSTR("down"))) {
+               spickle_down();
+       }
+       else if (!strcmp_P(res->arg1, PSTR("stop"))) {
+               spickle_stop();
+       }
+       else if (!strcmp_P(res->arg1, PSTR("auto"))) {
+               spickle_auto();
+       }
+
+       printf_P(PSTR("done\r\n"));
+}
+
+prog_char str_spickle_arg0[] = "spickle";
+parse_pgm_token_string_t cmd_spickle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg0, str_spickle_arg0);
+prog_char str_spickle_arg1[] = "auto#up#down#stop";
+parse_pgm_token_string_t cmd_spickle_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1);
+
+prog_char help_spickle[] = "spickle auto mode: spickle auto delay_up delay_down";
+parse_pgm_inst_t cmd_spickle = {
+       .f = cmd_spickle_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_spickle,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_spickle_arg0, 
+               (prog_void *)&cmd_spickle_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Set Spickle Params */
+
+/* this structure is filled when cmd_spickle_params is parsed successfully */
+struct cmd_spickle_params_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int32_t arg2;
+       int32_t arg3;
+};
+
+/* function called when cmd_spickle_params is parsed successfully */
+static void cmd_spickle_params_parsed(void *parsed_result,
+                                     __attribute__((unused)) void *data)
+{
+       struct cmd_spickle_params_result * res = parsed_result;
+       
+       
+       if (!strcmp_P(res->arg1, PSTR("delay"))) {
+               spickle_set_delays(res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("coef"))) {
+               spickle_set_coefs(res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("pos"))) {
+               spickle_set_pos(res->arg2, res->arg3);
+       }
+
+       /* else show */
+       spickle_dump_params();
+}
+
+prog_char str_spickle_params_arg0[] = "spickle_params";
+parse_pgm_token_string_t cmd_spickle_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg0, str_spickle_params_arg0);
+prog_char str_spickle_params_arg1[] = "delay#pos#coef";
+parse_pgm_token_string_t cmd_spickle_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1);
+parse_pgm_token_num_t cmd_spickle_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg2, INT32);
+parse_pgm_token_num_t cmd_spickle_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32);
+
+prog_char help_spickle_params[] = "Set spickle_params values";
+parse_pgm_inst_t cmd_spickle_params = {
+       .f = cmd_spickle_params_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_spickle_params,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_spickle_params_arg0, 
+               (prog_void *)&cmd_spickle_params_arg1, 
+               (prog_void *)&cmd_spickle_params_arg2, 
+               (prog_void *)&cmd_spickle_params_arg3, 
+               NULL,
+       },
+};
+
+prog_char str_spickle_params_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_spickle_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1_show);
+
+prog_char help_spickle_params_show[] = "show spickle params";
+parse_pgm_inst_t cmd_spickle_params_show = {
+       .f = cmd_spickle_params_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_spickle_params_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_spickle_params_arg0, 
+               (prog_void *)&cmd_spickle_params_arg1_show, 
+               NULL,
+       },
+};
+
+/**********************************************************/
 /* Test */
 
 /* this structure is filled when cmd_test is parsed successfully */
index 4d8ccb0..17d8e54 100644 (file)
@@ -45,6 +45,7 @@
 
 #include "main.h"
 #include "actuator.h"
+#include "spickle.h"
 
 /* called every 5 ms */
 static void do_cs(__attribute__((unused)) void *dummy) 
@@ -106,21 +107,15 @@ void microb_cs_init(void)
        /* ---- CS left_spickle */
        /* PID */
        pid_init(&cobboard.left_spickle.pid);
-       pid_set_gains(&cobboard.left_spickle.pid, 500, 40, 5000);
-       pid_set_maximums(&cobboard.left_spickle.pid, 0, 5000, 2400); /* max is 12 V */
+       pid_set_gains(&cobboard.left_spickle.pid, 300, 10, 1500);
+       pid_set_maximums(&cobboard.left_spickle.pid, 0, 10000, 2400); /* max is 12 V */
        pid_set_out_shift(&cobboard.left_spickle.pid, 10);
        pid_set_derivate_filter(&cobboard.left_spickle.pid, 4);
 
-       /* QUADRAMP */
-       quadramp_init(&cobboard.left_spickle.qr);
-       quadramp_set_1st_order_vars(&cobboard.left_spickle.qr, 2000, 2000); /* set speed */
-       quadramp_set_2nd_order_vars(&cobboard.left_spickle.qr, 20, 20); /* set accel */
-
        /* CS */
        cs_init(&cobboard.left_spickle.cs);
-       cs_set_consign_filter(&cobboard.left_spickle.cs, quadramp_do_filter, &cobboard.left_spickle.qr);
        cs_set_correct_filter(&cobboard.left_spickle.cs, pid_do_filter, &cobboard.left_spickle.pid);
-       cs_set_process_in(&cobboard.left_spickle.cs, pwm_ng_set, LEFT_SPICKLE_PWM);
+       cs_set_process_in(&cobboard.left_spickle.cs, spickle_set, LEFT_SPICKLE_PWM);
        cs_set_process_out(&cobboard.left_spickle.cs, encoders_spi_get_value, LEFT_SPICKLE_ENCODER);
        cs_set_consign(&cobboard.left_spickle.cs, 0);
 
@@ -132,21 +127,15 @@ void microb_cs_init(void)
        /* ---- CS right_spickle */
        /* PID */
        pid_init(&cobboard.right_spickle.pid);
-       pid_set_gains(&cobboard.right_spickle.pid, 500, 40, 5000);
-       pid_set_maximums(&cobboard.right_spickle.pid, 0, 5000, 2400); /* max is 12 V */
+       pid_set_gains(&cobboard.right_spickle.pid, 300, 10, 1500);
+       pid_set_maximums(&cobboard.right_spickle.pid, 0, 10000, 2400); /* max is 12 V */
        pid_set_out_shift(&cobboard.right_spickle.pid, 10);
-       pid_set_derivate_filter(&cobboard.right_spickle.pid, 6);
-
-       /* QUADRAMP */
-       quadramp_init(&cobboard.right_spickle.qr);
-       quadramp_set_1st_order_vars(&cobboard.right_spickle.qr, 1000, 1000); /* set speed */
-       quadramp_set_2nd_order_vars(&cobboard.right_spickle.qr, 20, 20); /* set accel */
+       pid_set_derivate_filter(&cobboard.right_spickle.pid, 4);
 
        /* CS */
        cs_init(&cobboard.right_spickle.cs);
-       cs_set_consign_filter(&cobboard.right_spickle.cs, quadramp_do_filter, &cobboard.right_spickle.qr);
        cs_set_correct_filter(&cobboard.right_spickle.cs, pid_do_filter, &cobboard.right_spickle.pid);
-       cs_set_process_in(&cobboard.right_spickle.cs, pwm_ng_set, RIGHT_SPICKLE_PWM);
+       cs_set_process_in(&cobboard.right_spickle.cs, spickle_set, RIGHT_SPICKLE_PWM);
        cs_set_process_out(&cobboard.right_spickle.cs, encoders_spi_get_value, RIGHT_SPICKLE_ENCODER);
        cs_set_consign(&cobboard.right_spickle.cs, 0);
 
@@ -158,19 +147,14 @@ void microb_cs_init(void)
        /* ---- CS shovel */
        /* PID */
        pid_init(&cobboard.shovel.pid);
-       pid_set_gains(&cobboard.shovel.pid, 500, 40, 5000);
-       pid_set_maximums(&cobboard.shovel.pid, 0, 5000, 2400); /* max is 12 V */
+       pid_set_gains(&cobboard.shovel.pid, 1000, 10, 200);
+       pid_set_maximums(&cobboard.shovel.pid, 0, 10000, 3200); /* max is 18 V */
        pid_set_out_shift(&cobboard.shovel.pid, 10);
-       pid_set_derivate_filter(&cobboard.shovel.pid, 6);
-
-       /* QUADRAMP */
-       quadramp_init(&cobboard.shovel.qr);
-       quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 1000); /* set speed */
-       quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 20, 20); /* set accel */
+       pid_set_derivate_filter(&cobboard.shovel.pid, 4);
 
        /* CS */
        cs_init(&cobboard.shovel.cs);
-       cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);
+       //cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);
        cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid);
        cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM);
        cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER);
@@ -181,10 +165,10 @@ void microb_cs_init(void)
        bd_set_speed_threshold(&cobboard.shovel.bd, 150);
        bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40);
 
-       /* set them on !! */
-       cobboard.left_spickle.on = 1;
-       cobboard.right_spickle.on = 1;
-       cobboard.shovel.on = 1;
+       /* set them on (or not) !! */
+       cobboard.left_spickle.on = 0;
+       cobboard.right_spickle.on = 0;
+       cobboard.shovel.on = 0;
 
        scheduler_add_periodical_event_priority(do_cs, NULL, 
                                                CS_PERIOD / SCHEDULER_UNIT, 
index 418f5e1..d0526a5 100755 (executable)
@@ -57,7 +57,7 @@
 #include "sensor.h"
 #include "state.h"
 #include "actuator.h"
-#include "arm_xy.h"
+#include "spickle.h"
 #include "cs.h"
 #include "i2c_protocol.h"
 
@@ -166,7 +166,7 @@ int main(void)
 #  error not supported
 #endif
 
-       //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
+       eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
        /* check eeprom to avoid to run the bad program */
        if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
            EEPROM_MAGIC_COBBOARD) {
@@ -200,12 +200,11 @@ int main(void)
        PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, 
                                 TIMER4_PRESCALER_DIV_1);
        
-       PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
-                     &PORTD, 4);
+       PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
+                     PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4);
        PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
                      &PORTD, 5);
-       PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED | 
-                     PWM_NG_MODE_SIGN_INVERTED,
+       PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
                      &PORTD, 6);
        PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED |
                      PWM_NG_MODE_SIGN_INVERTED,
@@ -246,10 +245,13 @@ int main(void)
 
        sei();
 
-       /* finger + other actuators */
+       /* actuators */
        actuator_init();
 
-       state_init();
+       /* spickle */
+       spickle_init();
+
+/*     state_init(); */
 
        printf_P(PSTR("\r\n"));
        printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
@@ -259,7 +261,7 @@ int main(void)
        gen.log_level = 5;
        cobboard.flags |= DO_CS;
 
-       state_machine();
+/*     state_machine(); */
        cmdline_interact();
 
        return 0;
index 33fb833..d50b610 100755 (executable)
 
 #define LEFT_SPICKLE_ENCODER   ((void *)0)
 #define RIGHT_SPICKLE_ENCODER  ((void *)1)
-#define SHOVEL_ENCODER         ((void *)1)
+#define SHOVEL_ENCODER         ((void *)2)
 
 #define LEFT_SPICKLE_PWM       ((void *)&gen.pwm1_4A)
 #define RIGHT_SPICKLE_PWM      ((void *)&gen.pwm2_4B)
-#define SHOVEL_PWM             ((void *)&gen.pwm2_4B)
+#define SHOVEL_PWM             ((void *)&gen.pwm3_1A)
 
 /** ERROR NUMS */
 #define E_USER_I2C_PROTO       195
@@ -64,6 +64,7 @@
 
 #define LED_PRIO           170
 #define TIME_PRIO          160
+#define SPICKLE_PRIO       130
 #define ADC_PRIO           120
 #define CS_PRIO            100
 #define I2C_POLL_PRIO       20
index a80201f..0889320 100644 (file)
@@ -146,8 +146,8 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = {
        [S_FRONT] =     { 5, 0, 4, 1, 0, 0 },  /* 1 */
        [S_CAP3] =      { 10, 0, 3, 7, 0, 0 }, /* 2 */
        [S_CAP4] =      { 1, 0, 0, 1, 0, 0 }, /* 3 */
-       [S_COL_LEFT] =  { 5, 0, 4, 1, 0, 1 }, /* 4 */
-       [S_LEFT] =      { 5, 0, 4, 1, 0, 1 }, /* 5 */
+       [S_LCOB] =      { 1, 0, 0, 1, 0, 1 }, /* 4 */
+       [S_LEFT] =      { 5, 0, 4, 1, 0, 0 }, /* 5 */
        [S_RIGHT] =     { 5, 0, 4, 1, 0, 1 }, /* 6 */
        [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */
        [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
index 27460db..6f47742 100644 (file)
@@ -32,7 +32,7 @@
 #define S_FRONT         1
 #define S_CAP3          2
 #define S_CAP4          3
-#define S_COL_LEFT      4
+#define S_LCOB          4
 #define S_LEFT          5
 #define S_RIGHT         6
 #define S_COL_RIGHT     7
diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
new file mode 100644 (file)
index 0000000..db954ed
--- /dev/null
@@ -0,0 +1,200 @@
+/*  
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+
+#include "sensor.h"
+#include "../common/i2c_commands.h"
+#include "actuator.h"
+#include "main.h"
+
+
+#define OFF 0
+#define WAIT_SENSOR 1
+#define SENSOR_OK 2
+#define WAIT_DOWN 3
+
+static volatile uint8_t spickle_state = OFF;
+static volatile uint32_t spickle_pos_up =  35000;
+static volatile uint32_t spickle_pos_down = 0;
+static volatile uint32_t spickle_delay_up = 500;
+static volatile uint32_t spickle_delay_down = 2000;
+static volatile uint32_t delay = 0;
+static volatile int32_t spickle_k1 = 500, spickle_k2 = 20;
+static volatile int32_t spickle_cmd = 0;
+
+/* init spickle position at beginning */
+static void spickle_autopos(void)
+{
+       pwm_ng_set(LEFT_SPICKLE_PWM, -500);
+       wait_ms(3000);
+       pwm_ng_set(LEFT_SPICKLE_PWM, 0);
+       encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
+}
+
+/* set CS command for spickle */
+void spickle_set(void *mot, int32_t cmd)
+{
+       static int32_t oldpos_left, oldpos_right;
+       int32_t oldpos, pos, maxcmd, speed;
+       
+       if (mot == LEFT_SPICKLE_PWM) {
+               pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
+               oldpos = oldpos_left;
+       }
+       else {
+               pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
+               oldpos = oldpos_right;
+       }
+
+       speed = pos - oldpos;
+       if (speed > 0 && cmd < 0)
+               maxcmd = spickle_k1;
+       else if (speed < 0 && cmd > 0)
+               maxcmd = spickle_k1;
+       else {
+               speed = ABS(speed);
+               maxcmd = spickle_k1 + spickle_k2 * speed;
+       }
+       if (cmd > maxcmd)
+               cmd = maxcmd;
+       else if (cmd < -maxcmd)
+               cmd = -maxcmd;
+
+       pwm_ng_set(mot, cmd);
+
+       if (mot == LEFT_SPICKLE_PWM)
+               oldpos_left = pos;
+       else
+               oldpos_right = pos;
+}
+
+void spickle_set_coefs(uint32_t k1, uint32_t k2)
+{
+       spickle_k1 = k1;
+       spickle_k2 = k2;
+}
+
+void spickle_set_delays(uint32_t delay_up, uint32_t delay_down)
+{
+       spickle_delay_up = delay_up;
+       spickle_delay_down = delay_down;
+}
+
+void spickle_set_pos(uint32_t pos_up, uint32_t pos_down)
+{
+       spickle_pos_up = pos_up;
+       spickle_pos_down = pos_down;
+}
+
+void spickle_dump_params(void)
+{
+       printf_P(PSTR("coef %ld %ld\r\n"), spickle_k1, spickle_k2);
+       printf_P(PSTR("pos %ld %ld\r\n"), spickle_pos_up, spickle_pos_down);
+       printf_P(PSTR("delay %ld %ld\r\n"), spickle_delay_up, spickle_delay_down);
+}
+
+void spickle_up(void)
+{
+       spickle_state = 0;
+       cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
+}
+
+void spickle_down(void)
+{
+       spickle_state = 0;
+       cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down);
+}
+
+void spickle_stop(void)
+{
+       spickle_state = 0;
+}
+
+void spickle_auto(void)
+{
+       spickle_state = WAIT_SENSOR;
+       cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
+}
+
+/* for spickle auto mode */
+static void spickle_cb(__attribute__((unused)) void *dummy)
+{
+       static uint8_t prev = 0;
+       uint8_t val;
+
+       val = sensor_get(S_LCOB);
+
+       switch (spickle_state) {
+       case OFF:
+               break;
+       case WAIT_SENSOR:
+               if (val && !prev) {
+                       delay = spickle_delay_up;
+                       spickle_state = SENSOR_OK;
+               }
+               break;
+       case SENSOR_OK:
+               if (delay-- == 0) {
+                       cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down);
+                       spickle_state = WAIT_DOWN;
+                       delay = spickle_delay_down;
+               }
+               break;
+       case WAIT_DOWN:
+               if (delay-- == 0) {
+                       cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
+                       spickle_state = WAIT_SENSOR;
+               }
+               break;
+       default:
+               break;
+       }
+       prev = val;
+}
+
+void spickle_init(void)
+{
+       spickle_autopos();
+
+       scheduler_add_periodical_event_priority(spickle_cb, NULL, 
+                                               1000L / SCHEDULER_UNIT, 
+                                               SPICKLE_PRIO);
+}
diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h
new file mode 100644 (file)
index 0000000..c2ffcf0
--- /dev/null
@@ -0,0 +1,37 @@
+/*  
+ *  Copyright Droids Corporation (2010)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+#ifndef _SPICKLE_H_
+#define _SPICKLE_H_
+
+void spickle_set(void *dummy, int32_t cmd);
+void spickle_set_coefs(uint32_t k1, uint32_t k2);
+void spickle_set_delays(uint32_t delay_up, uint32_t delay_down);
+void spickle_set_pos(uint32_t pos_up, uint32_t pos_down);
+void spickle_dump_params(void);
+void spickle_left_manage(void);
+void spickle_up(void);
+void spickle_down(void);
+void spickle_stop(void);
+void spickle_auto(void);
+void spickle_init(void);
+
+#endif
index 35605b8..9672d6a 100644 (file)
@@ -22,6 +22,8 @@
 #ifndef _I2C_COMMANDS_H_
 #define _I2C_COMMANDS_H_
 
+#define I2C_OPPONENT_NOT_THERE -1000
+
 #define I2C_MAINBOARD_ADDR   1
 #define I2C_COBBOARD_ADDR    2
 #define I2C_BALLBOARD_ADDR   3
index 21ce9e5..e4e1ace 100644 (file)
@@ -83,6 +83,7 @@ CONFIG_MODULE_CIRBUF=y
 CONFIG_MODULE_FIXED_POINT=y
 CONFIG_MODULE_VECT2=y
 CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_HOSTSIM is not set
 CONFIG_MODULE_SCHEDULER=y
 CONFIG_MODULE_SCHEDULER_STATS=y
 # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
index 5867b4e..2b18f20 100755 (executable)
@@ -6,15 +6,11 @@ AVERSIVE_DIR = ../../..
 SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c 
 SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
 SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
-SRC += strat_utils.c strat_base.c strat_avoid.c strat.c
-SRC += strat_static_columns.c strat_lintel.c
-SRC += strat_column_disp.c strat_building.c strat_scan.c
+SRC += strat_utils.c strat_base.c strat.c
 
 ASRC = 
 
 CFLAGS += -Wall -Werror
-#CFLAGS += -DHOMOLOGATION
-CFLAGS += -DTEST_BEACON
 LDFLAGS = -T ../common/avr6.x
 
 ########################################
index 19d2fc7..2088874 100644 (file)
@@ -31,7 +31,7 @@
 #include <pwm_ng.h>
 #include <timer.h>
 #include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
 
 #include <pid.h>
 #include <quadramp.h>
@@ -65,13 +65,3 @@ void pwm_set_and_save(void *pwm, int32_t val)
        pwm_ng_set(pwm, val);
 }
 
-void pickup_wheels_on(void)
-{
-       mainboard.enable_pickup_wheels = 1;
-}
-
-void pickup_wheels_off(void)
-{
-       mainboard.enable_pickup_wheels = 0;
-}
-
index 9c7174a..22d10b7 100644 (file)
@@ -20,6 +20,4 @@
  */
 
 void pwm_set_and_save(void *pwm, int32_t val);
-void pickup_wheels_on(void);
-void pickup_wheels_off(void);
 
index 575f5bc..de5bb88 100644 (file)
@@ -28,7 +28,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 #include <spi.h>
 
 #include <pid.h>
index 7c3ec9e..eeeede6 100644 (file)
@@ -31,7 +31,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 
 #include <pid.h>
 #include <quadramp.h>
index 26060d6..5d49e78 100644 (file)
@@ -70,28 +70,11 @@ extern parse_pgm_inst_t cmd_interact;
 extern parse_pgm_inst_t cmd_color;
 extern parse_pgm_inst_t cmd_rs;
 extern parse_pgm_inst_t cmd_i2cdebug;
-extern parse_pgm_inst_t cmd_mechboard_show;
-extern parse_pgm_inst_t cmd_mechboard_setmode1;
-extern parse_pgm_inst_t cmd_mechboard_setmode2;
-extern parse_pgm_inst_t cmd_mechboard_setmode3;
-extern parse_pgm_inst_t cmd_mechboard_setmode4;
-extern parse_pgm_inst_t cmd_mechboard_setmode5;
-extern parse_pgm_inst_t cmd_pickup_wheels;
+extern parse_pgm_inst_t cmd_cobboard_show;
+extern parse_pgm_inst_t cmd_cobboard_setmode1;
+extern parse_pgm_inst_t cmd_cobboard_setmode2;
+extern parse_pgm_inst_t cmd_cobboard_setmode3;
 extern parse_pgm_inst_t cmd_beacon_start;
-extern parse_pgm_inst_t cmd_pump_current;
-extern parse_pgm_inst_t cmd_build_test;
-extern parse_pgm_inst_t cmd_column_test;
-extern parse_pgm_inst_t cmd_column_test2;
-extern parse_pgm_inst_t cmd_lintel_test;
-extern parse_pgm_inst_t cmd_pickup_test;
-extern parse_pgm_inst_t cmd_scan_test;
-extern parse_pgm_inst_t cmd_scan_test2;
-extern parse_pgm_inst_t cmd_time_monitor;
-extern parse_pgm_inst_t cmd_scanner;
-extern parse_pgm_inst_t cmd_build_z1;
-#ifdef TEST_BEACON
-extern parse_pgm_inst_t cmd_beacon_opp_dump;
-#endif
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
@@ -170,28 +153,10 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_color,
        (parse_pgm_inst_t *)&cmd_rs,
        (parse_pgm_inst_t *)&cmd_i2cdebug,
-       (parse_pgm_inst_t *)&cmd_mechboard_show,
-       (parse_pgm_inst_t *)&cmd_mechboard_setmode1,
-       (parse_pgm_inst_t *)&cmd_mechboard_setmode2,
-       (parse_pgm_inst_t *)&cmd_mechboard_setmode3,
-       (parse_pgm_inst_t *)&cmd_mechboard_setmode4,
-       (parse_pgm_inst_t *)&cmd_mechboard_setmode5,
-       (parse_pgm_inst_t *)&cmd_pickup_wheels,
-       (parse_pgm_inst_t *)&cmd_beacon_start,
-       (parse_pgm_inst_t *)&cmd_pump_current,
-       (parse_pgm_inst_t *)&cmd_build_test,
-       (parse_pgm_inst_t *)&cmd_column_test,
-       (parse_pgm_inst_t *)&cmd_column_test2,
-       (parse_pgm_inst_t *)&cmd_lintel_test,
-       (parse_pgm_inst_t *)&cmd_pickup_test,
-       (parse_pgm_inst_t *)&cmd_scan_test,
-       (parse_pgm_inst_t *)&cmd_scan_test2,
-       (parse_pgm_inst_t *)&cmd_time_monitor,
-       (parse_pgm_inst_t *)&cmd_scanner,
-       (parse_pgm_inst_t *)&cmd_build_z1,
-#ifdef TEST_BEACON
-       (parse_pgm_inst_t *)&cmd_beacon_opp_dump,
-#endif
+       (parse_pgm_inst_t *)&cmd_cobboard_show,
+       (parse_pgm_inst_t *)&cmd_cobboard_setmode1,
+       (parse_pgm_inst_t *)&cmd_cobboard_setmode2,
+       (parse_pgm_inst_t *)&cmd_cobboard_setmode3,
        (parse_pgm_inst_t *)&cmd_test,
 
        /* commands_traj.c */
@@ -217,7 +182,6 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_strat_conf,
        (parse_pgm_inst_t *)&cmd_strat_conf2,
        (parse_pgm_inst_t *)&cmd_strat_conf3,
-       (parse_pgm_inst_t *)&cmd_strat_conf4,
        (parse_pgm_inst_t *)&cmd_subtraj,
        NULL,
 };
index 804e084..75616f7 100644 (file)
@@ -30,7 +30,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 
 #include <pid.h>
 #include <quadramp.h>
index a39ab07..e6850f3 100644 (file)
@@ -30,7 +30,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 
 #include <pid.h>
 #include <quadramp.h>
@@ -60,9 +60,13 @@ struct csb_list {
 
 prog_char csb_angle_str[] = "angle";
 prog_char csb_distance_str[] = "distance";
+prog_char csb_left_cobroller_str[] = "left_cobroller";
+prog_char csb_right_cobroller_str[] = "right_cobroller";
 struct csb_list csb_list[] = {
        { .name = csb_angle_str, .csb = &mainboard.angle },
        { .name = csb_distance_str, .csb = &mainboard.distance },
+       { .name = csb_left_cobroller_str, .csb = &mainboard.left_cobroller },
+       { .name = csb_right_cobroller_str, .csb = &mainboard.right_cobroller },
 };
 
 struct cmd_cs_result {
@@ -71,7 +75,7 @@ struct cmd_cs_result {
 };
 
 /* token to be used for all cs-related commands */
-prog_char str_csb_name[] = "angle#distance";
+prog_char str_csb_name[] = "angle#distance#left_cobroller#right_cobroller";
 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
 
 struct cs_block *cs_from_name(const char *name)
index b20dcc8..fb9024d 100644 (file)
@@ -30,7 +30,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 #include <encoders_spi.h>
 #include <adc.h>
 
index 7c222b3..6349ad7 100644 (file)
@@ -31,7 +31,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 #include <spi.h>
 #include <i2c.h>
 
@@ -293,23 +293,15 @@ static void cmd_start_parsed(void *parsed_result, void *data)
 
        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);
+               i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED);
+               i2c_set_color(I2C_BALLBOARD_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);
+               i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN);
+               i2c_set_color(I2C_BALLBOARD_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;
@@ -553,13 +545,13 @@ static void cmd_color_parsed(void *parsed_result, void *data)
        struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
        if (!strcmp_P(res->color, PSTR("red"))) {
                mainboard.our_color = I2C_COLOR_RED;
-               i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
-               i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
+               i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED);
+               i2c_set_color(I2C_BALLBOARD_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);
+               i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN);
+               i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN);
        }
        printf_P(PSTR("Done\r\n"));
 }
@@ -657,1465 +649,162 @@ parse_pgm_inst_t cmd_i2cdebug = {
 };
 
 /**********************************************************/
-/* Mechboard_Show */
+/* Cobboard_Show */
 
-/* this structure is filled when cmd_mechboard_show is parsed successfully */
-struct cmd_mechboard_show_result {
+/* this structure is filled when cmd_cobboard_show is parsed successfully */
+struct cmd_cobboard_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)
+/* function called when cmd_cobboard_show is parsed successfully */
+static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
 {
-       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);
-
+       printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
+       printf_P(PSTR("status = %x\r\n"), cobboard.status);
 }
 
-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 str_cobboard_show_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
+prog_char str_cobboard_show_arg1[] = "show";
+parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_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 */
+prog_char help_cobboard_show[] = "show cobboard status";
+parse_pgm_inst_t cmd_cobboard_show = {
+       .f = cmd_cobboard_show_parsed,  /* function to call */
        .data = NULL,      /* 2nd arg of func */
-       .help_str = help_mechboard_show,
+       .help_str = help_cobboard_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_mechboard_show_arg0, 
-               (prog_void *)&cmd_mechboard_show_arg1, 
+               (prog_void *)&cmd_cobboard_show_arg0, 
+               (prog_void *)&cmd_cobboard_show_arg1, 
                NULL,
        },
 };
 
 /**********************************************************/
-/* Mechboard_Setmode1 */
+/* Cobboard_Setmode1 */
 
-/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
-struct cmd_mechboard_setmode1_result {
+/* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
+struct cmd_cobboard_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)
+/* function called when cmd_cobboard_setmode1 is parsed successfully */
+static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
 {
-       struct cmd_mechboard_setmode1_result *res = parsed_result;
+       struct cmd_cobboard_setmode1_result *res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("init")))
-               i2c_mechboard_mode_init();
+               i2c_cobboard_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();
+               i2c_cobboard_mode_manual();
        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();
+               i2c_cobboard_mode_harvest();
 }
 
-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 str_cobboard_setmode1_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
+prog_char str_cobboard_setmode1_arg1[] = "init#manual#harvest";
+parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_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 */
+prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
+parse_pgm_inst_t cmd_cobboard_setmode1 = {
+       .f = cmd_cobboard_setmode1_parsed,  /* function to call */
        .data = NULL,      /* 2nd arg of func */
-       .help_str = help_mechboard_setmode1,
+       .help_str = help_cobboard_setmode1,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_mechboard_setmode1_arg0, 
-               (prog_void *)&cmd_mechboard_setmode1_arg1, 
+               (prog_void *)&cmd_cobboard_setmode1_arg0, 
+               (prog_void *)&cmd_cobboard_setmode1_arg1, 
                NULL,
        },
 };
 
 /**********************************************************/
-/* Mechboard_Setmode2 */
+/* Cobboard_Setmode2 */
 
-/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
-struct cmd_mechboard_setmode2_result {
+/* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
+struct cmd_cobboard_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)
+/* function called when cmd_cobboard_setmode2 is parsed successfully */
+static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
 {
-       struct cmd_mechboard_setmode2_result *res = parsed_result;
+       struct cmd_cobboard_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);
-}
-
-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)
-{
-       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);
-}
-
-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)
-{
-       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);
-}
-
-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)
-{
-       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);
-}
-
-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)
-{
-       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);
-}
-
-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)
-{
-       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);
-}
-
-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)
-{
-       //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);
-}
-
-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)
-{
-       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;
-}
-
-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)
-{
-       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();
+       if (!strcmp_P(res->arg1, PSTR("yyy")))
+               printf("faux\r\n");
+       else if (!strcmp_P(res->arg1, PSTR("xxx")))
+               printf("faux\r\n");
 }
 
-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 str_cobboard_setmode2_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
+prog_char str_cobboard_setmode2_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
+prog_char str_cobboard_setmode2_arg2[] = "left#right";
+parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
 
-prog_char help_pickup_test[] = "Pickup_Test function";
-parse_pgm_inst_t cmd_pickup_test = {
-       .f = cmd_pickup_test_parsed,  /* function to call */
+prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
+parse_pgm_inst_t cmd_cobboard_setmode2 = {
+       .f = cmd_cobboard_setmode2_parsed,  /* function to call */
        .data = NULL,      /* 2nd arg of func */
-       .help_str = help_pickup_test,
+       .help_str = help_cobboard_setmode2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_pickup_test_arg0,
-               (prog_void *)&cmd_pickup_test_arg1,
-               (prog_void *)&cmd_pickup_test_arg2,
+               (prog_void *)&cmd_cobboard_setmode2_arg0, 
+               (prog_void *)&cmd_cobboard_setmode2_arg1, 
+               (prog_void *)&cmd_cobboard_setmode2_arg2, 
                NULL,
        },
 };
 
 /**********************************************************/
-/* Lintel_Test */
+/* Cobboard_Setmode3 */
 
-/* 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)
-{
-       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;
-}
-
-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 {
+/* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
+struct cmd_cobboard_setmode3_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)
+/* function called when cmd_cobboard_setmode3 is parsed successfully */
+static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
 {
-       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);
-       }
-}
-
-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)
-{
-       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);
-}
-
-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)
-{
-       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();
-       }
+       struct cmd_cobboard_setmode3_result *res = parsed_result;
+       if (!strcmp_P(res->arg1, PSTR("xxx")))
+               printf("faux\r\n");
 }
 
-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 str_cobboard_setmode3_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
+prog_char str_cobboard_setmode3_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
+parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
 
-prog_char help_scanner[] = "send commands to scanner";
-parse_pgm_inst_t cmd_scanner = {
-       .f = cmd_scanner_parsed,  /* function to call */
+prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
+parse_pgm_inst_t cmd_cobboard_setmode3 = {
+       .f = cmd_cobboard_setmode3_parsed,  /* function to call */
        .data = NULL,      /* 2nd arg of func */
-       .help_str = help_scanner,
+       .help_str = help_cobboard_setmode3,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_scanner_arg0, 
-               (prog_void *)&cmd_scanner_arg1, 
+               (prog_void *)&cmd_cobboard_setmode3_arg0, 
+               (prog_void *)&cmd_cobboard_setmode3_arg1, 
+               (prog_void *)&cmd_cobboard_setmode3_arg2, 
                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)
-{
-       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);
-}
-
-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)
-{
-       beacon_dump_samples();
-}
-
-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 */
@@ -2123,18 +812,10 @@ 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";
index ea124d3..0d83688 100644 (file)
@@ -30,7 +30,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 #include <spi.h>
 #include <encoders_spi.h>
 
@@ -56,7 +56,6 @@
 #include "cmdline.h"
 #include "strat_utils.h"
 #include "strat_base.h"
-#include "strat_avoid.h"
 #include "strat.h"
 #include "../common/i2c_commands.h"
 #include "i2c_protocol.h"
@@ -273,11 +272,8 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data)
                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"));
+       printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
+                mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
 }
 
 prog_char str_rs_gains_arg0[] = "rs_gains";
@@ -399,6 +395,11 @@ 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("avoid_start"))) {
+               printf_P(PSTR("not implemented\r\n"));
+               return;
+       }
+
        if (!strcmp_P(res->arg1, PSTR("append"))) {
                res->arg2 = pt_list_len;
        }
@@ -447,6 +448,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
                        trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
                        why = wait_traj_end(0xFF); /* all */
                }
+#if 0
                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);
@@ -455,6 +457,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
                                        break;
                        }
                }
+#endif
                if (why & (~(END_TRAJ | END_NEAR)))
                        trajectory_stop(&mainboard.traj);
        }
@@ -580,14 +583,24 @@ static void cmd_goto_parsed(void * parsed_result, void * data)
                trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
        }
        else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
+#if 0
                err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
                if (err != END_TRAJ && err != END_NEAR)
                        strat_hardstop();
+#else
+               printf_P(PSTR("not implemented\r\n"));
+               return;
+#endif
        }
        else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
+#if 0
                err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
                if (err != END_TRAJ && err != END_NEAR)
                        strat_hardstop();
+#else
+               printf_P(PSTR("not implemented\r\n"));
+               return;
+#endif
        }
        else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
                trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
@@ -732,14 +745,14 @@ static void cmd_position_parsed(void * parsed_result, void * data)
        }
        else if (!strcmp_P(res->arg1, PSTR("autoset_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);
+               i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN);
+               i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN);
                auto_position();
        }
        else if (!strcmp_P(res->arg1, PSTR("autoset_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);
+               i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED);
+               i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED);
                auto_position();
        }
 
@@ -840,74 +853,15 @@ struct cmd_strat_conf_result {
 /* 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;
-       }
+       //      struct cmd_strat_conf_result *res = parsed_result;
+
        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";
+prog_char str_strat_conf_arg1[] = "show#base";
 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";
@@ -943,6 +897,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
        else
                on = 0;
        
+#if 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")))
@@ -959,6 +914,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
                bit = STRAT_CONF_EARLY_SCAN;
        else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
                bit = STRAT_CONF_PUSH_OPP_COLS;
+#endif
 
        if (on)
                strat_infos.conf.flags |= bit;
@@ -971,7 +927,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 
 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";
+prog_char str_strat_conf2_arg1[] = "faux";
 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);
@@ -1003,6 +959,7 @@ struct cmd_strat_conf3_result {
 /* function called when cmd_strat_conf3 is parsed successfully */
 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
 {
+#if 0
        struct cmd_strat_conf3_result *res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
@@ -1031,13 +988,14 @@ static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
        else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
                strat_infos.conf.lintel_min_time = res->arg2;
        }
+#endif
        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";
+prog_char str_strat_conf3_arg1[] = "faux2";
 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);
 
@@ -1054,47 +1012,6 @@ parse_pgm_inst_t cmd_strat_conf3 = {
        },
 };
 
-/**********************************************************/
-/* 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 */
@@ -1112,76 +1029,15 @@ struct cmd_subtraj_result {
 /* function called when cmd_subtraj is parsed successfully */
 static void cmd_subtraj_parsed(void *parsed_result, void *data)
 {
-       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);
-       }
+/*     struct cmd_subtraj_result *res = parsed_result; */
 
-       printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
+       printf_P(PSTR("TODO\r\n"));
        trajectory_hardstop(&mainboard.traj);
 }
 
 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";
+prog_char str_subtraj_arg1[] = "faux";
 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);
index ec70f9a..8fe9718 100644 (file)
@@ -32,7 +32,7 @@
 #include <pwm_ng.h>
 #include <timer.h>
 #include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
 #include <adc.h>
 
 #include <pid.h>
 #include "strat.h"
 #include "actuator.h"
 
+int32_t encoders_left_cobroller_speed(void *number)
+{
+       static volatile int32_t roller_pos;
+       int32_t tmp, speed;
+       tmp = encoders_spi_get_value(number);
+       speed = tmp - roller_pos;
+       roller_pos = tmp;
+       return speed;
+}
+
+int32_t encoders_right_cobroller_speed(void *number)
+{
+       static volatile int32_t roller_pos;
+       int32_t tmp, speed;
+       tmp = encoders_spi_get_value(number);
+       speed = tmp - roller_pos;
+       roller_pos = tmp;
+       return speed;
+}
+
 /* called every 5 ms */
 static void do_cs(void *dummy) 
 {
@@ -87,6 +107,10 @@ static void do_cs(void *dummy)
                        cs_manage(&mainboard.angle.cs);
                if (mainboard.distance.on)
                        cs_manage(&mainboard.distance.cs);
+               if (mainboard.left_cobroller.on)
+                       cs_manage(&mainboard.left_cobroller.cs);
+               if (mainboard.right_cobroller.on)
+                       cs_manage(&mainboard.right_cobroller.cs);
        }
        if ((cpt & 1) && (mainboard.flags & DO_POS)) {
                /* about 1.5ms (worst case without centrifugal force
@@ -96,6 +120,8 @@ static void do_cs(void *dummy)
        if (mainboard.flags & DO_BD) {
                bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
                bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
+               bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs);
+               bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs);
        }
        if (mainboard.flags & DO_TIMER) {
                uint8_t second;
@@ -228,9 +254,63 @@ void microb_cs_init(void)
        bd_set_speed_threshold(&mainboard.distance.bd, 60);
        bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
 
+       /* ---- CS left_cobroller */
+       /* PID */
+       pid_init(&mainboard.left_cobroller.pid);
+       pid_set_gains(&mainboard.left_cobroller.pid, 500, 10, 7000);
+       pid_set_maximums(&mainboard.left_cobroller.pid, 0, 2000, 4095);
+       pid_set_out_shift(&mainboard.left_cobroller.pid, 10);
+       pid_set_derivate_filter(&mainboard.left_cobroller.pid, 6);
+
+       /* QUADRAMP */
+       quadramp_init(&mainboard.left_cobroller.qr);
+       quadramp_set_1st_order_vars(&mainboard.left_cobroller.qr, 2000, 2000); /* set speed */
+       quadramp_set_2nd_order_vars(&mainboard.left_cobroller.qr, 17, 17); /* set accel */
+
+       /* CS */
+       cs_init(&mainboard.left_cobroller.cs);
+       cs_set_consign_filter(&mainboard.left_cobroller.cs, quadramp_do_filter, &mainboard.left_cobroller.qr);
+       cs_set_correct_filter(&mainboard.left_cobroller.cs, pid_do_filter, &mainboard.left_cobroller.pid);
+       cs_set_process_in(&mainboard.left_cobroller.cs, pwm_ng_set, LEFT_COBROLLER_PWM);
+       cs_set_process_out(&mainboard.left_cobroller.cs, encoders_left_cobroller_speed, LEFT_COBROLLER_ENCODER);
+       cs_set_consign(&mainboard.left_cobroller.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&mainboard.left_cobroller.bd);
+       bd_set_speed_threshold(&mainboard.left_cobroller.bd, 60);
+       bd_set_current_thresholds(&mainboard.left_cobroller.bd, 500, 8000, 1000000, 50);
+
+       /* ---- CS right_cobroller */
+       /* PID */
+       pid_init(&mainboard.right_cobroller.pid);
+       pid_set_gains(&mainboard.right_cobroller.pid, 500, 10, 7000);
+       pid_set_maximums(&mainboard.right_cobroller.pid, 0, 2000, 4095);
+       pid_set_out_shift(&mainboard.right_cobroller.pid, 10);
+       pid_set_derivate_filter(&mainboard.right_cobroller.pid, 6);
+
+       /* QUADRAMP */
+       quadramp_init(&mainboard.right_cobroller.qr);
+       quadramp_set_1st_order_vars(&mainboard.right_cobroller.qr, 2000, 2000); /* set speed */
+       quadramp_set_2nd_order_vars(&mainboard.right_cobroller.qr, 17, 17); /* set accel */
+
+       /* CS */
+       cs_init(&mainboard.right_cobroller.cs);
+       cs_set_consign_filter(&mainboard.right_cobroller.cs, quadramp_do_filter, &mainboard.right_cobroller.qr);
+       cs_set_correct_filter(&mainboard.right_cobroller.cs, pid_do_filter, &mainboard.right_cobroller.pid);
+       cs_set_process_in(&mainboard.right_cobroller.cs, pwm_ng_set, RIGHT_COBROLLER_PWM);
+       cs_set_process_out(&mainboard.right_cobroller.cs, encoders_left_cobroller_speed, RIGHT_COBROLLER_ENCODER);
+       cs_set_consign(&mainboard.right_cobroller.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&mainboard.right_cobroller.bd);
+       bd_set_speed_threshold(&mainboard.right_cobroller.bd, 60);
+       bd_set_current_thresholds(&mainboard.right_cobroller.bd, 500, 8000, 1000000, 50);
+
        /* set them on !! */
        mainboard.angle.on = 1;
        mainboard.distance.on = 1;
+       mainboard.left_cobroller.on = 1;
+       mainboard.right_cobroller.on = 1;
 
 
        scheduler_add_periodical_event_priority(do_cs, NULL,
index a503f6d..2ceb7e8 100644 (file)
@@ -30,7 +30,7 @@
 #include <uart.h>
 #include <pwm_ng.h>
 #include <i2c.h>
-#include <time.h>
+#include <clock_time.h>
 
 #include <pid.h>
 #include <quadramp.h>
@@ -77,8 +77,8 @@ uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
 volatile int8_t command_dest=-1;
 volatile uint8_t command_size=0;
 
-static int8_t i2c_req_mechboard_status(void);
-static int8_t i2c_req_sensorboard_status(void);
+static int8_t i2c_req_cobboard_status(void);
+static int8_t i2c_req_ballboard_status(void);
 
 #define I2C_ERROR(args...) do {                                                \
                if (error_log < I2C_MAX_LOG) {                          \
@@ -159,32 +159,32 @@ void i2c_poll_slaves(void *dummy)
 
        switch(i2c_state) {
 
-       /* poll status of mechboard */
-#define I2C_REQ_MECHBOARD 0
-       case I2C_REQ_MECHBOARD:
-               if ((err = i2c_req_mechboard_status()))
+       /* poll status of cobboard */
+#define I2C_REQ_COBBOARD 0
+       case I2C_REQ_COBBOARD:
+               if ((err = i2c_req_cobboard_status()))
                        goto error;
                break;
 
-#define I2C_ANS_MECHBOARD 1
-       case I2C_ANS_MECHBOARD:
-               if ((err = i2c_recv(I2C_MECHBOARD_ADDR, 
-                                   sizeof(struct i2c_ans_mechboard_status), 
+#define I2C_ANS_COBBOARD 1
+       case I2C_ANS_COBBOARD:
+               if ((err = i2c_recv(I2C_COBBOARD_ADDR, 
+                                   sizeof(struct i2c_ans_cobboard_status), 
                                    I2C_CTRL_GENERIC)))
                        goto error;
                break;
 
-       /* poll status of sensorboard */
-#define I2C_REQ_SENSORBOARD 2
-       case I2C_REQ_SENSORBOARD:
-               if ((err = i2c_req_sensorboard_status()))
+       /* poll status of ballboard */
+#define I2C_REQ_BALLBOARD 2
+       case I2C_REQ_BALLBOARD:
+               if ((err = i2c_req_ballboard_status()))
                        goto error;
                break;
 
-#define I2C_ANS_SENSORBOARD 3
-       case I2C_ANS_SENSORBOARD:
-               if ((err = i2c_recv(I2C_SENSORBOARD_ADDR, 
-                                   sizeof(struct i2c_ans_sensorboard_status), 
+#define I2C_ANS_BALLBOARD 3
+       case I2C_ANS_BALLBOARD:
+               if ((err = i2c_recv(I2C_BALLBOARD_ADDR, 
+                                   sizeof(struct i2c_ans_ballboard_status), 
                                    I2C_CTRL_GENERIC)))
                        goto error;
                break;
@@ -253,53 +253,27 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 
        switch (buf[0]) {
 
-       case I2C_ANS_MECHBOARD_STATUS: {
-               struct i2c_ans_mechboard_status * ans = 
-                       (struct i2c_ans_mechboard_status *)buf;
+       case I2C_ANS_COBBOARD_STATUS: {
+               struct i2c_ans_cobboard_status * ans = 
+                       (struct i2c_ans_cobboard_status *)buf;
                
                if (size != sizeof (*ans))
                        goto error;
 
                /* status */
-               mechboard.mode = ans->mode;
-               mechboard.status = ans->status;
-               mechboard.lintel_count = ans->lintel_count;
-               mechboard.column_flags = ans->column_flags;
-               /* pumps pwm */
-               mechboard.pump_left1 = ans->pump_left1;
-               mechboard.pump_left2 = ans->pump_left2;
-               mechboard.pump_right1 = ans->pump_right1;
-               mechboard.pump_right2 = ans->pump_right2;
-               pwm_ng_set(LEFT_PUMP1_PWM, mechboard.pump_left1);
-               pwm_ng_set(LEFT_PUMP2_PWM, mechboard.pump_left2);
-               /* pumps current */
-               mechboard.pump_right1_current = ans->pump_right1_current;
-               mechboard.pump_right2_current = ans->pump_right2_current;
-               /* servos */
-               mechboard.servo_lintel_left = ans->servo_lintel_left;
-               mechboard.servo_lintel_right = ans->servo_lintel_right;
-               pwm_ng_set(&gen.servo2, mechboard.servo_lintel_right);
-               pwm_ng_set(&gen.servo3, mechboard.servo_lintel_left);
+               cobboard.mode = ans->mode;
+               cobboard.status = ans->status;
 
                break;
        }
                
-       case I2C_ANS_SENSORBOARD_STATUS: {
-               struct i2c_ans_sensorboard_status * ans = 
-                       (struct i2c_ans_sensorboard_status *)buf;
+       case I2C_ANS_BALLBOARD_STATUS: {
+               struct i2c_ans_ballboard_status * ans = 
+                       (struct i2c_ans_ballboard_status *)buf;
                
                if (size != sizeof (*ans))
                        goto error;
-               sensorboard.status = ans->status;
-               sensorboard.opponent_x = ans->opponent_x;
-               sensorboard.opponent_y = ans->opponent_y;
-               sensorboard.opponent_a = ans->opponent_a;
-               sensorboard.opponent_d = ans->opponent_d;
-
-               sensorboard.scan_status = ans->scan_status;
-               sensorboard.dropzone_h = ans->dropzone_h;
-               sensorboard.dropzone_x = ans->dropzone_x;
-               sensorboard.dropzone_y = ans->dropzone_y;
+               ballboard.status = ans->status;
                break;
        }
 
@@ -353,33 +327,24 @@ i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
        return -EBUSY;
 }
 
-static int8_t i2c_req_mechboard_status(void)
+static int8_t i2c_req_cobboard_status(void)
 {
-       struct i2c_req_mechboard_status buf;
+       struct i2c_req_cobboard_status buf;
        int8_t err;
 
-       buf.hdr.cmd = I2C_REQ_MECHBOARD_STATUS;
-       buf.pump_left1_current = sensor_get_adc(ADC_CSENSE3);
-       buf.pump_left2_current = sensor_get_adc(ADC_CSENSE4);
-       err = i2c_send(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,
+       buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS;
+       err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf,
                        sizeof(buf), I2C_CTRL_GENERIC);
 
        return err;
 }
 
-static int8_t i2c_req_sensorboard_status(void)
+static int8_t i2c_req_ballboard_status(void)
 {
-       struct i2c_req_sensorboard_status buf;
+       struct i2c_req_ballboard_status buf;
        
-       buf.hdr.cmd = I2C_REQ_SENSORBOARD_STATUS;
-       /* robot position */
-       buf.x = position_get_x_s16(&mainboard.pos);
-       buf.y = position_get_y_s16(&mainboard.pos);
-       buf.a = position_get_a_deg_s16(&mainboard.pos);
-       /* pickup wheels */
-       buf.enable_pickup_wheels = mainboard.enable_pickup_wheels;
-       
-       return i2c_send(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf,
+       buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS;
+       return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf,
                        sizeof(buf), I2C_CTRL_GENERIC);
 }
 
@@ -387,7 +352,7 @@ int8_t i2c_set_color(uint8_t addr, uint8_t color)
 {
        struct i2c_cmd_generic_color buf;
 
-       if (addr == I2C_SENSORBOARD_ADDR)
+       if (addr == I2C_BALLBOARD_ADDR)
                return 0; /* XXX disabled for now */
        buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
        buf.color = color;
@@ -403,285 +368,27 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
        return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
 }
 
-int8_t i2c_mechboard_mode_manual(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_MANUAL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_harvest(void)
+int8_t i2c_cobboard_mode_manual(void)
 {
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_HARVEST;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       struct i2c_cmd_cobboard_set_mode buf;
+       buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
+       buf.mode = I2C_COBBOARD_MODE_MANUAL;
+       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 
-int8_t i2c_mechboard_mode_lazy_harvest(void)
+int8_t i2c_cobboard_mode_harvest(void)
 {
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       struct i2c_cmd_cobboard_set_mode buf;
+       buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
+       buf.mode = I2C_COBBOARD_MODE_HARVEST;
+       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 
-int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side)
+int8_t i2c_cobboard_mode_init(void)
 {
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-       buf.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-       buf.prep_pickup.side = side;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       struct i2c_cmd_cobboard_set_mode buf;
+       buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
+       buf.mode = I2C_COBBOARD_MODE_INIT;
+       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 
-int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC;
-       buf.prep_pickup.side = side;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-       buf.prep_pickup.next_mode = next_mode;
-       buf.prep_pickup.side = side;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_pickup(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PICKUP;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_eject(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_EJECT;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_manivelle(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_MANIVELLE;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_push_temple(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE;
-       buf.push_temple.level = level;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
-       buf.prep_build.level_l = level;
-       buf.prep_build.level_r = level;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
-       buf.prep_build.level_l = level_l;
-       buf.prep_build.level_r = level_r;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
-       buf.prep_inside.level_l = level;
-       buf.prep_inside.level_r = level;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
-       buf.prep_inside.level_l = level_l;
-       buf.prep_inside.level_r = level_r;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
-       buf.autobuild.level_left = level;
-       buf.autobuild.level_right = level;
-       buf.autobuild.count_left = 2;
-       buf.autobuild.count_right = 2;
-       buf.autobuild.do_lintel = 1;
-       buf.autobuild.distance_left = 210;
-       buf.autobuild.distance_right = 210;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_autobuild(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)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
-       buf.autobuild.level_left = level_l;
-       buf.autobuild.level_right = level_r;
-       buf.autobuild.count_left = count_l;
-       buf.autobuild.count_right = count_r;
-       buf.autobuild.distance_left = dist_l;
-       buf.autobuild.distance_right = dist_r;
-       buf.autobuild.do_lintel = do_lintel;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_init(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_INIT;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_get_lintel(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_get_lintel(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_GET_LINTEL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_put_lintel(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PUT_LINTEL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_clear(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_CLEAR;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_loaded(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_LOADED;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_store(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_STORE;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_lazy_pickup(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_set_beacon(uint8_t enable)
-{
-       struct i2c_cmd_sensorboard_start_beacon buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_BEACON;
-       buf.enable = enable;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_set(uint8_t mode)
-{
-       struct i2c_cmd_sensorboard_scanner buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_SCANNER;
-       buf.mode = mode;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_calib(void)
-{
-       struct i2c_cmd_sensorboard_calib_scanner buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_CALIB_SCANNER;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone,
-                                          int16_t x, int16_t y)
-{
-       struct i2c_cmd_sensorboard_scanner_algo buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
-       buf.algo = I2C_SCANNER_ALGO_COLUMN_DROPZONE;
-       buf.drop_zone.working_zone = zone;
-       buf.drop_zone.center_x = x;
-       buf.drop_zone.center_y = y;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone,
-                                          int16_t x, int16_t y)
-{
-       struct i2c_cmd_sensorboard_scanner_algo buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
-       buf.algo = I2C_SCANNER_ALGO_TEMPLE_DROPZONE;
-       buf.drop_zone.working_zone = zone;
-       buf.drop_zone.center_x = x;
-       buf.drop_zone.center_y = y;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_algo_check(uint8_t level,
-                                         int16_t x, int16_t y)
-{
-       struct i2c_cmd_sensorboard_scanner_algo buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
-       buf.algo = I2C_SCANNER_ALGO_CHECK_TEMPLE;
-       buf.check_temple.level = level;
-       buf.check_temple.temple_x = x;
-       buf.check_temple.temple_y = y;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
index 0d40205..1e22a4f 100644 (file)
@@ -35,58 +35,8 @@ void i2c_sendevent(int8_t size);
 int8_t i2c_set_color(uint8_t addr, uint8_t color);
 int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state);
 
-int8_t i2c_mechboard_mode_manual(void);
-int8_t i2c_mechboard_mode_harvest(void);
-int8_t i2c_mechboard_mode_lazy_harvest(void);
-int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side);
-int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode);
-int8_t i2c_mechboard_mode_pickup(void);
-int8_t i2c_mechboard_mode_eject(void);
-int8_t i2c_mechboard_mode_lazy_pickup(void);
-
-int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level);
-int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r);
-int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level);
-int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r);
-int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level);
-int8_t i2c_mechboard_mode_autobuild(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);
-int8_t i2c_mechboard_mode_init(void);
-int8_t i2c_mechboard_mode_eject(void);
-int8_t i2c_mechboard_mode_prepare_get_lintel(void);
-int8_t i2c_mechboard_mode_get_lintel(void);
-int8_t i2c_mechboard_mode_put_lintel(void);
-int8_t i2c_mechboard_mode_clear(void);
-int8_t i2c_mechboard_mode_loaded(void);
-int8_t i2c_mechboard_mode_store(void);
-int8_t i2c_mechboard_mode_manivelle(void);
-int8_t i2c_mechboard_mode_push_temple(uint8_t level);
-int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side);
-
-int8_t i2c_sensorboard_set_beacon(uint8_t enable);
-
-int8_t i2c_sensorboard_scanner_set(uint8_t mode);
-static inline int8_t i2c_sensorboard_scanner_stop(void) {
-       return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_STOP);
-}
-static inline int8_t i2c_sensorboard_scanner_start(void) {
-       return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_START);
-}
-static inline int8_t i2c_sensorboard_scanner_prepare(void) {
-       return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_PREPARE);
-}
-
-int8_t i2c_sensorboard_scanner_calib(void);
-
-int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone,
-                                          int16_t x, int16_t y);
-int8_t i2c_sensorboard_scanner_algo_check(uint8_t level,
-                                         int16_t x, int16_t y);
-int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone,
-                                          int16_t x, int16_t y);
-
+int8_t i2c_cobboard_mode_manual(void);
+int8_t i2c_cobboard_mode_harvest(void);
+int8_t i2c_cobboard_mode_init(void);
 
 #endif
index d69d058..d0c9d14 100755 (executable)
@@ -37,7 +37,7 @@
 #include <pwm_ng.h>
 #include <timer.h>
 #include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
 #include <adc.h>
 
 #include <pid.h>
 #include "cs.h"
 #include "i2c_protocol.h"
 
-#if __AVR_LIBC_VERSION__ == 10602UL
-#error "won't work with this version"
-#endif
-
 /* 0 means "programmed"
  * ---- with 16 Mhz quartz
  * CKSEL 3-0 : 0111
@@ -85,8 +81,8 @@
 
 struct genboard gen;
 struct mainboard mainboard;
-struct mechboard mechboard;
-struct sensorboard sensorboard;
+struct cobboard cobboard;
+struct ballboard ballboard;
 
 /***********************/
 
@@ -172,7 +168,6 @@ int main(void)
        memset(&mainboard, 0, sizeof(mainboard));
        mainboard.flags = DO_ENCODERS | DO_RS |
                DO_POS | DO_POWER | DO_BD;
-       sensorboard.opponent_x = I2C_OPPONENT_NOT_THERE;
 
        /* UART */
        uart_init();
@@ -244,10 +239,6 @@ int main(void)
                      NULL, 0);
        PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);
-       pwm_ng_set(&gen.servo2, 290); /* right */
-       pwm_ng_set(&gen.servo3, 400); /* left */
-       /* 2 lintels 180, 485 */
-       /* 1 lintel 155, 520 */
 
        /* SCHEDULER */
        scheduler_init();
index 78fccc4..7f21058 100755 (executable)
 #define IMP_COEF 10.
 #define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF)
 
-#define LEFT_ENCODER        ((void *)1)
-#define RIGHT_ENCODER       ((void *)0)
+#define LEFT_ENCODER            ((void *)1)
+#define RIGHT_ENCODER           ((void *)0)
+#define LEFT_COBROLLER_ENCODER  ((void *)2)
+#define RIGHT_COBROLLER_ENCODER ((void *)3)
 
 #define LEFT_PWM            ((void *)&gen.pwm1_4A)
 #define RIGHT_PWM           ((void *)&gen.pwm2_4B)
-
-#define LEFT_PUMP1_PWM      ((void *)&gen.pwm3_1A)
-#define LEFT_PUMP2_PWM      ((void *)&gen.pwm4_1B)
+#define LEFT_COBROLLER_PWM  ((void *)&gen.pwm3_1A)
+#define RIGHT_COBROLLER_PWM ((void *)&gen.pwm4_1B)
 
 /** ERROR NUMS */
 #define E_USER_STRAT           194
@@ -142,6 +143,8 @@ struct mainboard {
        /* control systems */
         struct cs_block angle;
         struct cs_block distance;
+        struct cs_block left_cobroller;
+        struct cs_block right_cobroller;
 
        /* x,y positionning */
        struct robot_system rs;
@@ -154,54 +157,23 @@ struct mainboard {
        volatile int16_t speed_d;     /* current dist speed */
        int32_t pwm_l;                /* current left pwm */
        int32_t pwm_r;                /* current right pwm */
-       uint8_t enable_pickup_wheels; /* these PWM are on sensorboard */
-
 };
 
-/* state of mechboard, synchronized through i2c */
-struct mechboard {
+/* state of cobboard, synchronized through i2c */
+struct cobboard {
        uint8_t mode;   
        uint8_t status;
-       int8_t lintel_count;
-       uint8_t column_flags;
-       
-       /* pwm */
-       int16_t pump_left1;
-       int16_t pump_right1;
-       int16_t pump_left2;
-       int16_t pump_right2;
-
-       /* currents (for left arm, we can just read it on adc) */
-       int16_t pump_right1_current;
-       int16_t pump_right2_current;
-       
-       /* pwm for lintel servos */
-       uint16_t servo_lintel_left;
-       uint16_t servo_lintel_right;
 };
 
-/* state of sensorboard, synchronized through i2c */
-struct sensorboard {
+/* state of ballboard, synchronized through i2c */
+struct ballboard {
        uint8_t status;
-       /* opponent pos */
-       int16_t opponent_x;
-       int16_t opponent_y;
-       int16_t opponent_a;
-       int16_t opponent_d;
-
-       /* scanner */
-#define I2C_SCAN_DONE 1
-       uint8_t scan_status;
-#define I2C_COLUMN_NO_DROPZONE -1
-       int8_t dropzone_h;
-       int16_t dropzone_x;
-       int16_t dropzone_y;
 };
 
 extern struct genboard gen;
 extern struct mainboard mainboard;
-extern struct mechboard mechboard;
-extern struct sensorboard sensorboard;
+extern struct cobboard cobboard;
+extern struct ballboard ballboard;
 
 /* start the bootloader */
 void bootloader(void);
index 3f54716..864b00b 100644 (file)
@@ -33,7 +33,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 #include <spi.h>
 
 #include <pid.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",
-               },
-       }
 };
 
 /*************************************************************/
@@ -278,12 +108,7 @@ void strat_preinit(void)
        mainboard.flags =  DO_ENCODERS | DO_CS | DO_RS |
                DO_POS | DO_BD | DO_POWER;
 
-       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);
-
+       i2c_cobboard_mode_init();
        strat_dump_conf();
        strat_dump_infos(__FUNCTION__);
 }
@@ -295,201 +120,6 @@ void strat_dump_conf(void)
 
        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 */
@@ -499,64 +129,18 @@ void strat_dump_infos(const char *caller)
                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)
 {
-       pickup_wheels_on();
+       /* XXX init rollers, .. */
        strat_reset_infos();
 
        /* we consider that the color is correctly set */
@@ -568,11 +152,6 @@ void strat_init(void)
        /* 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
 }
 
 
@@ -581,7 +160,6 @@ void strat_exit(void)
 {
        uint8_t flags;
 
-       pickup_wheels_off();
        mainboard.flags &= ~(DO_TIMER);
        strat_hardstop();
        time_reset();
@@ -598,80 +176,11 @@ void strat_event(void *dummy)
 {
        /* limit speed when opponent is close */
        strat_limit_speed();
-
-#ifdef TEST_BEACON
-       beacon_update_samples();
-#endif
 }
 
-/* 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;
-}
-
-/* return true if we need to grab some more elements (lintel/cols) */
-uint8_t need_more_elements(void)
-{
-       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;
-       }
-       return 1;
+       return END_TRAJ;
 }
 
 /* dump state (every 5 s max) */
@@ -687,121 +196,9 @@ uint8_t need_more_elements(void)
 uint8_t strat_main(void)
 {
        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;
+       return err;
 }
index 2b2b96a..add640a 100644 (file)
 #define CORNER_Y COLOR_Y(2100)
 
 /*
- *              /- line 0
- *             |   /- line 1
- *             |  |   /- line 2
- *             |  |  |
- *     +---C1--------------------------C1---+
- *     |          z0a   z1    z0b           |
- *     |                 .                  |
- *     |                 .                  |
- *    C3       0  1  2   ^                  C3
- *  y  |       3  4  5 <   >                |
- *    C2       6  7  8   v                  C2 
- *     |------ 9 10 11   .            ------| 
- *     |     |           .            |     | 
- *     | red |           .            |green
- *     +-----+--L1--L2-------L2--L1---+-----+
+ *
+ *
+ *
+ *
+ *     +------------------------------------+
+ *     |                                   |
+ *     |                                   |
+ *     |                                   |
+ *     |                                   | 
+ *  y  |                                   |
+ *     |                                   |  
+ *     |------                       ------| 
+ *     |     |                       |     | 
+ *     |     |                       |     
+ *     +-----+------------------------+-----+
  *                       x
  */
 
-/* static columns */
-#define LINE0_X 600
-#define LINE1_X 850
-#define LINE2_X 1100
-
-#define COL0_X 600
-#define COL0_Y COLOR_Y(1175)
-#define COL1_X 850
-#define COL1_Y COLOR_Y(1175)
-#define COL2_X 1100
-#define COL2_Y COLOR_Y(1175)
-
-#define COL3_X 600
-#define COL3_Y COLOR_Y(975)
-#define COL4_X 850
-#define COL4_Y COLOR_Y(975)
-#define COL5_X 1100
-#define COL5_Y COLOR_Y(975)
-
-#define COL6_X 600
-#define COL6_Y COLOR_Y(775)
-#define COL7_X 850
-#define COL7_Y COLOR_Y(775)
-#define COL8_X 1100
-#define COL8_Y COLOR_Y(775)
-
-#define COL9_X 600
-#define COL9_Y COLOR_Y(575)
-#define COL10_X 850
-#define COL10_Y COLOR_Y(575)
-#define COL11_X 1100
-#define COL11_Y COLOR_Y(575)
-
-/* distance to go backward before pickup in dispenser */
-#define DIST_BACK_DISPENSER 35
-
-/* diag of the pentagon (pentacle ?) */
-#define DISC_PENTA_DIAG 530
-
-#define COL_DISP_MAX_TRIES 5
-#define LIN_DISP_MAX_TRIES 3
-
 /* useful traj flags */
 #define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR))
 #define TRAJ_FLAGS_STD (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR|END_TIMER)
@@ -129,128 +87,21 @@ struct bbox {
 };
 
 struct conf {
-#define STRAT_CONF_ONLY_ONE_ON_DISC      0x01
-#define STRAT_CONF_BYPASS_STATIC2        0x02
-#define STRAT_CONF_TAKE_ONE_LINTEL       0x04
-#define STRAT_CONF_SKIP_WHEN_CHECK_FAILS 0x08
-#define STRAT_CONF_STORE_STATIC2         0x10
-#define STRAT_CONF_BIG_3_TEMPLE          0x20
-#define STRAT_CONF_EARLY_SCAN            0x40
-#define STRAT_CONF_PUSH_OPP_COLS         0x80
-       uint8_t flags;
-       uint8_t scan_opp_min_time;
-       uint8_t delay_between_opp_scan;
-       uint8_t scan_our_min_time;
-       uint8_t delay_between_our_scan;
-       uint8_t wait_opponent;
-       uint8_t lintel_min_time;
-       int16_t scan_opp_angle;
-};
-
-struct static_columns {
-#define STATIC_COL_LINE0_DONE 0x01
-#define STATIC_COL_LINE1_DONE 0x02
-#define STATIC_COL_LINE2_DONE 0x04
-       uint8_t flags;
-       uint8_t configuration;
-};
-
-struct column_dispenser {
-       int8_t count;
-       uint8_t last_try_time;
-       uint8_t scan_left;
-       int16_t checkpoint_x;
-       int16_t checkpoint_y;
-       int16_t scan_a;
-       int16_t eject_a;
-       int16_t pickup_a;
-       int16_t recalib_x;
-       int16_t recalib_y;
-       char *name;
-};
-
-struct lintel_dispenser {
-       int8_t count;
-       uint8_t last_try_time;
-       int16_t x;
-       char *name;
-};
-
-struct temple {
-#define TEMPLE_F_VALID    0x01 /* structure is valid */
-#define TEMPLE_F_MONOCOL  0x02 /* temple has only one col */
-#define TEMPLE_F_ON_DISC  0x04 /* temple is on disc (else it's on other zone) */
-#define TEMPLE_F_OPPONENT 0x08 /* temple was originally built by opponent */
-#define TEMPLE_F_LINTEL   0x10 /* lintel on top (don't put another lintel) */
-
+#define STRAT_CONF_XXX   0x01
        uint8_t flags;
-       /* position of the robot when we built it */
-       int16_t x;
-       int16_t y;
-       int16_t a;
-
-       /* position of the robot checkpoint */
-       int16_t checkpoint_x;
-       int16_t checkpoint_y;
-
-       /* position and level of each col */
-       uint8_t level_l;
-       uint8_t dist_l;
-       uint8_t angle_l;
-
-       uint8_t level_r;
-       uint8_t dist_r;
-       uint8_t angle_r;
-
-#define TEMPLE_DISABLE_TIME 5
-       uint8_t last_try_time;
-
-       struct build_zone *zone;
 };
 
-struct build_zone {
-#define ZONE_F_VALID 0x01      /* zone is valid */
-#define ZONE_F_DISC  0x02      /* specific disc zone */
-#define ZONE_F_ZONE1 0x04      /* specific zone 1 */
-#define ZONE_F_ZONE0 0x08      /* specific zone 0 */
-#define ZONE_F_BUSY  0x10      /* this zone is busy */
-       uint8_t flags;
-       uint8_t level;
-       int16_t checkpoint_x;
-       int16_t checkpoint_y;
-
-#define ZONE_DISABLE_TIME 5
-       uint8_t last_try_time;
-       char *name;
-};
-
-#define MAX_TEMPLE 5
-#define MAX_ZONE 5
-
 /* all infos related to strat */
 struct strat_infos {
        uint8_t dump_enabled;
        struct conf conf;
        struct bbox area_bbox;
-       uint8_t taken_lintel;
-       uint8_t col_in_boobs;
-       uint8_t lazy_pickup_done;
-       uint8_t i2c_loaded_skipped;
-       struct static_columns s_cols;
-       struct column_dispenser c1;
-       struct column_dispenser c2;
-       struct column_dispenser c3;
-       struct lintel_dispenser l1;
-       struct lintel_dispenser l2;
-       struct build_zone zone_list[MAX_ZONE];
-       struct temple temple_list[MAX_TEMPLE];
 };
 extern struct strat_infos strat_infos;
 
 /* in strat.c */
 void strat_dump_infos(const char *caller); /* show current known state
                                              of area */
-void strat_dump_temple(struct temple *temple);
 void strat_dump_conf(void);
 void strat_reset_infos(void); /* reset current known state */
 void strat_preinit(void);
@@ -261,42 +112,4 @@ void strat_goto_near(int16_t x, int16_t y, uint16_t dist);
 uint8_t strat_main(void);
 void strat_event(void *dummy);
 
-/* in strat_static_columns.c */
-uint8_t strat_static_columns(uint8_t configuration);
-uint8_t strat_static_columns_pass2(void);
-
-/* in strat_lintel.c */
-uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp);
-uint8_t strat_pickup_lintels(void);
-
-/* in strat_column_disp.c */
-uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a);
-uint8_t strat_pickup_columns(void);
-uint8_t strat_goto_col_disp(struct column_dispenser **disp);
-
-/* in strat_building.c */
-uint8_t strat_goto_disc(int8_t level);
-uint8_t strat_goto_build_zone(struct build_zone *build_zone, uint8_t level);
-uint8_t strat_build_new_temple(struct build_zone *build_zone);
-uint8_t strat_goto_temple(struct temple *temple);
-uint8_t strat_grow_temple(struct temple *temple);
-uint8_t strat_grow_temple_column(struct temple *temple);
-struct temple *strat_get_best_temple(void);
-struct temple *strat_get_our_temple_on_disc(uint8_t valid);
-struct build_zone *strat_get_best_zone(void);
-struct temple *strat_get_free_temple(void);
-
-/* in strat_scan.c */
-struct scan_disc_result;
-void scanner_dump_state(void);
-int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x,
-                                int16_t *ckpt_rel_y, int16_t *back_mm);
-uint8_t strat_scan_disc(int16_t angle, uint8_t mode,
-                       struct scan_disc_result *result);
-uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level);
-int16_t strat_get_temple_angle(struct temple *temple);
-int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle);
-uint8_t strat_build_on_opponent_temple(void);
-uint8_t strat_check_temple_and_build(void);
-
 #endif
diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
deleted file mode 100644 (file)
index 6e75dd0..0000000
+++ /dev/null
@@ -1,535 +0,0 @@
-/*  
- *  Copyright Droids Corporation, Microb Technology (2009)
- * 
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *  Revision : $Id: strat_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.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 "sensor.h"
-
-#define EDGE_NUMBER 5
-void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt,
-                         int16_t radius, int16_t x, int16_t y)
-{
-
-       double c_a, s_a;
-       uint8_t i;
-       double px1, py1, px2, py2;
-       double a_rad;
-
-       a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
-
-       /* generate pentagon  */
-       c_a = cos(-2*M_PI/EDGE_NUMBER);
-       s_a = sin(-2*M_PI/EDGE_NUMBER);
-
-       /*
-       px1 = radius;
-       py1 = 0;
-       */
-       px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER));
-       py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER));
-
-
-       for (i = 0; i < EDGE_NUMBER; i++){
-               oa_poly_set_point(pol, x + px1, y + py1, i);
-               
-               px2 = px1*c_a + py1*s_a;
-               py2 = -px1*s_a + py1*c_a;
-
-               px1 = px2;
-               py1 = py2;
-       }
-}
-
-void set_rotated_poly(poly_t *pol, const point_t *robot_pt, 
-                     int16_t w, int16_t l, int16_t x, int16_t y)
-{
-       double tmp_x, tmp_y;
-       double a_rad;
-
-       a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
-
-       DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f", 
-             __FUNCTION__, x, y, a_rad);
-
-       /* point 1 */
-       tmp_x = w;
-       tmp_y = l;
-       rotate(&tmp_x, &tmp_y, a_rad);
-       tmp_x += x;
-       tmp_y += y;
-       oa_poly_set_point(pol, tmp_x, tmp_y, 0);
-       
-       /* point 2 */
-       tmp_x = -w;
-       tmp_y = l;
-       rotate(&tmp_x, &tmp_y, a_rad);
-       tmp_x += x;
-       tmp_y += y;
-       oa_poly_set_point(pol, tmp_x, tmp_y, 1);
-       
-       /* point 3 */
-       tmp_x = -w;
-       tmp_y = -l;
-       rotate(&tmp_x, &tmp_y, a_rad);
-       tmp_x += x;
-       tmp_y += y;
-       oa_poly_set_point(pol, tmp_x, tmp_y, 2);
-       
-       /* point 4 */
-       tmp_x = w;
-       tmp_y = -l;
-       rotate(&tmp_x, &tmp_y, a_rad);
-       tmp_x += x;
-       tmp_y += y;
-       oa_poly_set_point(pol, tmp_x, tmp_y, 3);
-}
-
-#define DISC_X CENTER_X
-#define DISC_Y CENTER_Y
-
-void set_central_disc_poly(poly_t *pol, const point_t *robot_pt)
-{
-       set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG,
-                            DISC_X, DISC_Y);
-}
-
-#ifdef HOMOLOGATION
-/* /!\ half size */
-#define O_WIDTH  400
-#define O_LENGTH 550
-#else
-/* /!\ half size */
-#define O_WIDTH  360
-#define O_LENGTH 500
-#endif
-
-void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l)
-{
-       int16_t x, y;
-       get_opponent_xy(&x, &y);
-       DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y);
-       
-       /* place poly even if invalid, because it's -100 */
-       set_rotated_poly(pol, robot_pt, w, l, x, y);
-}
-
-/* don't care about polygons further than this distance for escape */
-#define ESCAPE_POLY_THRES 1000
-
-/* don't reduce opp if opp is too far */
-#define REDUCE_POLY_THRES 600
-
-/* has to be longer than any poly */
-#define ESCAPE_VECT_LEN 3000
-
-/*
- * Go in playground, loop until out of poly. The argument robot_pt is 
- * the pointer to the current position of the robot.
- * Return 0 if there was nothing to do.
- * Return 1 if we had to move. In this case, update the theorical 
- * position of the robot in robot_pt.
- */
-static int8_t go_in_area(point_t *robot_pt)
-{
-       point_t poly_pts_area[4];
-       poly_t poly_area;
-       point_t disc_pt, dst_pt;
-
-       disc_pt.x = DISC_X;
-       disc_pt.y = DISC_Y;
-
-       /* Go in playground */
-       if (!is_in_boundingbox(robot_pt)){
-               NOTICE(E_USER_STRAT, "not in playground %"PRIi32", %"PRIi32"",
-                      robot_pt->x, robot_pt->y);
-
-               poly_area.l = 4;
-               poly_area.pts = poly_pts_area;
-               poly_pts_area[0].x = strat_infos.area_bbox.x1;
-               poly_pts_area[0].y = strat_infos.area_bbox.y1;
-
-               poly_pts_area[1].x = strat_infos.area_bbox.x2;
-               poly_pts_area[1].y = strat_infos.area_bbox.y1;
-
-               poly_pts_area[2].x = strat_infos.area_bbox.x2;
-               poly_pts_area[2].y = strat_infos.area_bbox.y2;
-
-               poly_pts_area[3].x = strat_infos.area_bbox.x1;
-               poly_pts_area[3].y = strat_infos.area_bbox.y2;
-
-               is_crossing_poly(*robot_pt, disc_pt, &dst_pt, &poly_area);
-               NOTICE(E_USER_STRAT, "pt dst %"PRIi32", %"PRIi32"", dst_pt.x, dst_pt.y);
-               
-               strat_goto_xy_force(dst_pt.x, dst_pt.y);
-
-               robot_pt->x = dst_pt.x;
-               robot_pt->y = dst_pt.y;
-
-               NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
-                      dst_pt.x, dst_pt.y);
-
-               return 1;
-       }
-
-       return 0;
-}
-
-
-/*
- * Escape from polygons if needed.
- * robot_pt is the current position of the robot, it will be
- * updated.
- */
-static int8_t escape_from_poly(point_t *robot_pt,
-                              poly_t *pol_disc,
-                              int16_t opp_x, int16_t opp_y, 
-                              int16_t opp_w, int16_t opp_l, 
-                              poly_t *pol_opp)
-{
-       uint8_t in_disc = 0, in_opp = 0;
-       double escape_dx = 0, escape_dy = 0;
-       double disc_dx = 0, disc_dy = 0;
-       double opp_dx = 0, opp_dy = 0;
-       double len;
-       point_t opp_pt, disc_pt, dst_pt;
-       point_t intersect_disc_pt, intersect_opp_pt;
-
-       opp_pt.x = opp_x;
-       opp_pt.y = opp_y;
-       disc_pt.x = DISC_X;
-       disc_pt.y = DISC_Y;
-
-       /* escape from other poly if necessary */
-       if (is_in_poly(robot_pt, pol_disc) == 1)
-               in_disc = 1;
-       if (is_in_poly(robot_pt, pol_opp) == 1)
-               in_opp = 1;
-
-       if (in_disc == 0 && in_opp == 0) {
-               NOTICE(E_USER_STRAT, "no need to escape");
-               return 0;
-       }
-       
-       NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp);
-       
-       /* process escape vector */
-
-       if (distance_between(robot_pt->x, robot_pt->y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) {
-               disc_dx = robot_pt->x - DISC_X;
-               disc_dy = robot_pt->y - DISC_Y;
-               NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f",
-                      disc_dx, disc_dy);
-               len = norm(disc_dx, disc_dy);
-               if (len != 0) {
-                       disc_dx /= len;
-                       disc_dy /= len;
-               }
-               else {
-                       disc_dx = 1.0;
-                       disc_dy = 0.0;
-               }
-               escape_dx += disc_dx;
-               escape_dy += disc_dy;
-       }
-
-       if (distance_between(robot_pt->x, robot_pt->y, opp_x, opp_y) < ESCAPE_POLY_THRES) {
-               opp_dx = robot_pt->x - opp_x;
-               opp_dy = robot_pt->y - opp_y;
-               NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f",
-                      opp_dx, opp_dy);
-               len = norm(opp_dx, opp_dy);
-               if (len != 0) {
-                       opp_dx /= len;
-                       opp_dy /= len;
-               }
-               else {
-                       opp_dx = 1.0;
-                       opp_dy = 0.0;
-               }
-               escape_dx += opp_dx;
-               escape_dy += opp_dy;
-       }
-
-       /* normalize escape vector */
-       len = norm(escape_dx, escape_dy);
-       if (len != 0) {
-               escape_dx /= len;
-               escape_dy /= len;
-       }
-       else {
-               if (pol_disc != NULL) {
-                       /* rotate 90° */
-                       escape_dx = disc_dy;
-                       escape_dy = disc_dx;
-               }
-               else if (pol_opp != NULL) {
-                       /* rotate 90° */
-                       escape_dx = opp_dy;
-                       escape_dy = opp_dx;
-               }
-               else { /* should not happen */
-                       opp_dx = 1.0;
-                       opp_dy = 0.0;
-               }
-       }
-
-       NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f",
-              escape_dx, escape_dy);
-
-       /* process the correct len of escape vector */
-
-       dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN;
-       dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN;
-
-       NOTICE(E_USER_STRAT, "robot pt %"PRIi32" %"PRIi32,
-              robot_pt->x, robot_pt->y);
-       NOTICE(E_USER_STRAT, "dst point %"PRIi32",%"PRIi32,
-              dst_pt.x, dst_pt.y);
-
-       if (in_disc) {
-               if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt,
-                                    pol_disc) == 1) {
-                       /* we add 2 mm to be sure we are out of th polygon */
-                       dst_pt.x = intersect_disc_pt.x + escape_dx * 2;
-                       dst_pt.y = intersect_disc_pt.y + escape_dy * 2;
-                       if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) {
-
-                               if (!is_in_boundingbox(&dst_pt))
-                                       return -1;
-                               
-                               NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
-                                      dst_pt.x, dst_pt.y);
-
-                               strat_goto_xy_force(dst_pt.x, dst_pt.y);
-
-                               robot_pt->x = dst_pt.x;
-                               robot_pt->y = dst_pt.y;
-
-                               return 0;
-                       }
-               }
-       }
-
-       if (in_opp) {
-               if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt,
-                                    pol_opp) == 1) {
-                       /* we add 2 cm to be sure we are out of th polygon */
-                       dst_pt.x = intersect_opp_pt.x + escape_dx * 2;
-                       dst_pt.y = intersect_opp_pt.y + escape_dy * 2;
-
-                       if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) {
-
-                               if (!is_in_boundingbox(&dst_pt))
-                                       return -1;
-                               
-                               NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
-                                      dst_pt.x, dst_pt.y);
-
-                               strat_goto_xy_force(dst_pt.x, dst_pt.y);
-
-                               robot_pt->x = dst_pt.x;
-                               robot_pt->y = dst_pt.y;
-
-                               return 0;
-                       }
-               }
-       }
-
-       /* should not happen */
-       return -1;
-}
-
-
-static int8_t __goto_and_avoid(int16_t x, int16_t y,
-                              uint8_t flags_intermediate,
-                              uint8_t flags_final,
-                              uint8_t forward)
-{
-       int8_t len = -1, i;
-       point_t *p;
-       poly_t *pol_disc, *pol_opp;
-       int8_t ret;
-       int16_t opp_w, opp_l, opp_x, opp_y;
-       point_t p_dst, robot_pt;
-
-       DEBUG(E_USER_STRAT, "%s(%d,%d) flags_i=%x flags_f=%x forw=%d",
-             __FUNCTION__, x, y, flags_intermediate, flags_final, forward);
-
- retry:
-       get_opponent_xy(&opp_x, &opp_y);
-       opp_w = O_WIDTH;
-       opp_l = O_LENGTH;
-
-       robot_pt.x = position_get_x_s16(&mainboard.pos);
-       robot_pt.y = position_get_y_s16(&mainboard.pos);
-       
-       oa_init();
-       pol_disc = oa_new_poly(5);
-       set_central_disc_poly(pol_disc, &robot_pt);
-       pol_opp = oa_new_poly(4);
-       set_opponent_poly(pol_opp, &robot_pt, O_WIDTH, O_LENGTH);
-
-       /* If we are not in the limited area, try to go in it. */
-       ret = go_in_area(&robot_pt);
-
-       /* check that destination is valid */
-       p_dst.x = x;
-       p_dst.y = y;
-       if (!is_in_boundingbox(&p_dst)) {
-               NOTICE(E_USER_STRAT, " dst is not in playground");
-               return END_ERROR;
-       }
-       if (is_point_in_poly(pol_disc, x, y)) {
-               NOTICE(E_USER_STRAT, " dst is in disc");
-               return END_ERROR;
-       }
-       if (is_point_in_poly(pol_opp, x, y)) {
-               NOTICE(E_USER_STRAT, " dst is in opp");
-               return END_ERROR;
-       }
-
-       /* now start to avoid */
-       while (opp_w && opp_l) {
-
-               /* robot_pt is not updated if it fails */
-               ret = escape_from_poly(&robot_pt,
-                                      pol_disc, opp_x, opp_y, 
-                                      opp_w, opp_l, pol_opp);
-               if (ret == 0) {
-                       oa_reset();
-                       oa_start_end_points(robot_pt.x, robot_pt.y, x, y);
-                       /* oa_dump(); */
-       
-                       len = oa_process();
-                       if (len >= 0)
-                               break;
-               }
-               if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) {
-                       if (opp_w == 0)
-                               opp_l /= 2;
-                       opp_w /= 2;
-               }
-               else {
-                       NOTICE(E_USER_STRAT, "oa_process() returned %d", len);
-                       return END_ERROR;
-               }
-
-               NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l);
-               set_opponent_poly(pol_opp, &robot_pt, opp_w, opp_l);
-       }
-       
-       p = oa_get_path();
-       for (i=0 ; i<len ; i++) {
-               DEBUG(E_USER_STRAT, "With avoidance %d: x=%"PRIi32" y=%"PRIi32"", i, p->x, p->y);
-
-               if (forward)
-                       trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y);
-               else
-                       trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y);
-
-               /* no END_NEAR for the last point */
-               if (i == len - 1)
-                       ret = wait_traj_end(flags_final);
-               else
-                       ret = wait_traj_end(flags_intermediate);
-
-               if (ret == END_BLOCKING) {
-                       DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)",
-                             __FUNCTION__, x, y);
-                       goto retry;
-               }
-               else if (ret == END_OBSTACLE) {
-                       /* brake and wait the speed to be slow */
-                       DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)",
-                             __FUNCTION__, x, y);
-                       goto retry;
-               }
-               /* else if it is not END_TRAJ or END_NEAR, return */
-               else if (!TRAJ_SUCCESS(ret)) {
-                       return ret;
-               }
-               p++;
-       }
-       
-       return END_TRAJ;
-}
-
-/* go forward to a x,y point. use current speed for that */
-uint8_t goto_and_avoid_forward(int16_t x, int16_t y, uint8_t flags_intermediate,
-                              uint8_t flags_final)
-{
-       return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1);
-}
-
-/* go backward to a x,y point. use current speed for that */
-uint8_t goto_and_avoid_backward(int16_t x, int16_t y, uint8_t flags_intermediate,
-                      uint8_t flags_final)
-{
-       return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0);
-}
-
-/* go to a x,y point. prefer backward but go forward if the point is
- * near and in front of us */
-uint8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate,
-                              uint8_t flags_final)
-{
-       double d,a;
-       abs_xy_to_rel_da(x, y, &d, &a); 
-
-       if (d < 300 && a < RAD(90) && a > RAD(-90))
-               return __goto_and_avoid(x, y, flags_intermediate,
-                                       flags_final, 1);
-       else
-               return __goto_and_avoid(x, y, flags_intermediate,
-                                       flags_final, 0);
-}
diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h
deleted file mode 100644 (file)
index 0978fd7..0000000
+++ /dev/null
@@ -1,30 +0,0 @@
-/*  
- *  Copyright Droids Corporation, Microb Technology (2009)
- * 
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *  Revision : $Id: strat_avoid.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $
- *
- */
-
-void set_opponent_poly(poly_t *pol, int16_t w, int16_t l);
-int8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate,
-                     uint8_t flags_final);
-int8_t goto_and_avoid_backward(int16_t x, int16_t y,
-                              uint8_t flags_intermediate,
-                              uint8_t flags_final);
-int8_t goto_and_avoid_forward(int16_t x, int16_t y,
-                             uint8_t flags_intermediate,
-                             uint8_t flags_final);
index 21d526a..40c6f86 100644 (file)
@@ -31,7 +31,7 @@
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 #include <spi.h>
 
 #include <pid.h>
@@ -126,27 +126,6 @@ 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);
@@ -200,41 +179,6 @@ uint8_t strat_calib(int16_t dist, uint8_t flags)
        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;
@@ -290,12 +234,6 @@ void strat_limit_speed(void)
        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;
@@ -310,7 +248,6 @@ void strat_limit_speed(void)
                        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;
index 27e1497..6ff0666 100644 (file)
@@ -59,10 +59,6 @@ void strat_start(void);
  * times. */
 uint8_t strat_goto_xy_force(int16_t x, int16_t y);
 
-/* escape from disc polygon or another zone */
-struct build_zone;
-uint8_t strat_escape(struct build_zone *zone, uint8_t flags);
-
 /* return true if we have to brake due to an obstacle */
 uint8_t strat_obstacle(void);
 
diff --git a/projects/microb2010/mainboard/strat_building.c b/projects/microb2010/mainboard/strat_building.c
deleted file mode 100644 (file)
index 9abdb5a..0000000
+++ /dev/null
@@ -1,907 +0,0 @@
-/*  
- *  Copyright Droids, Microb Technology (2008)
- * 
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *  Revision : $Id: strat_building.c,v 1.5 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 <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.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 "../common/i2c_commands.h"
-#include "main.h"
-#include "cmdline.h"
-#include "i2c_protocol.h"
-#include "strat.h"
-#include "strat_base.h"
-#include "strat_utils.h"
-#include "strat_avoid.h"
-#include "sensor.h"
-
-
-#define DISC_DIST_NEED_GOTO_AVOID 1000
-#define DISC_DIST_PREPARE_BUILD   700
-#define DISC_DIST_SLOW            500
-
-#define ERROUT(e) do {                         \
-               err = e;                        \
-               goto end;                       \
-       } while(0)
-
-static uint8_t is_ready_for_prepare_build(void)
-{
-       double d, a;
-       if (distance_from_robot(CENTER_X, CENTER_Y) >
-           DISC_DIST_PREPARE_BUILD)
-               return 0;
-       abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
-       if (a < RAD(-30))
-               return 0;
-       if (a > RAD(30))
-               return 0;
-       return 1;
-}
-
-/* go to the nearest place on the disc. Also prepare the arms for
- * building at the correct level. If level==-1, don't move the
- * arms. */
-uint8_t strat_goto_disc(int8_t level)
-{
-       uint8_t err;
-       uint16_t old_spdd, old_spda;
-       double d, a, x, y;
-
-       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
-
-       strat_get_speed(&old_spdd, &old_spda);
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
-       /* workaround for some static cols configurations */
-       if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) {
-               if (time_get_s() > 15)
-                       i2c_mechboard_mode_loaded();
-       }
-
-       /* if we are far from the disc, goto backward faster */
-       abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
-       if (d > DISC_DIST_NEED_GOTO_AVOID) {
-               rel_da_to_abs_xy(d - DISC_DIST_PREPARE_BUILD, a, &x, &y);
-               err = goto_and_avoid(x, y, TRAJ_FLAGS_STD,
-                                    TRAJ_FLAGS_NO_NEAR);
-               if (!TRAJ_SUCCESS(err))
-                       ERROUT(err);
-       }
-
-#ifdef HOMOLOGATION
-       {
-               int16_t opp_d, opp_a;
-               trajectory_turnto_xy(&mainboard.traj,
-                                    CENTER_X, CENTER_Y);
-               err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-               
-               time_wait_ms(500);
-               
-               err = get_opponent_da(&opp_d, &opp_a);
-               if (err == 0 && opp_d < 600 &&
-                   (opp_a > 325 || opp_a < 35))
-                       return END_ERROR;
-       }
-#endif
-
-       trajectory_goto_forward_xy_abs(&mainboard.traj,
-                                      CENTER_X, CENTER_Y);
-       err = WAIT_COND_OR_TRAJ_END(is_ready_for_prepare_build(),
-                                   TRAJ_FLAGS_NO_NEAR);
-
-       if (err == END_BLOCKING) 
-               ERROUT(END_BLOCKING);
-       if (TRAJ_SUCCESS(err)) /* should not reach dest */
-               ERROUT(END_ERROR);
-
-       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
-       if (level != -1)
-               i2c_mechboard_mode_prepare_build_both(level);
-       
-       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(CENTER_X,
-                                                       CENTER_Y) < DISC_DIST_SLOW,
-                                   TRAJ_FLAGS_NO_NEAR);
-
-       if (err == END_BLOCKING) 
-               ERROUT(END_BLOCKING);
-       if (TRAJ_SUCCESS(err)) /* should not reach dest */
-               ERROUT(END_ERROR);
-
-       strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
-       if (err == END_BLOCKING) 
-               ERROUT(END_TRAJ);
-       if (TRAJ_SUCCESS(err)) /* should not reach dest */
-               ERROUT(END_ERROR);
-
-       ERROUT(err);
- end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-/* must be called from the checkpoint before zone 1. */
-static uint8_t strat_goto_build_zone1_near(uint8_t level)
-{
-       uint8_t err;
-
-       /* turn to build zone */
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-       trajectory_a_abs(&mainboard.traj, COLOR_A(90));
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (!TRAJ_SUCCESS(err))
-               return err;
-
-       /* move forward to reach the build zone */
-       strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
-       i2c_mechboard_mode_prepare_build_both(level);
-       err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
-       if (err == END_BLOCKING) {
-               err = END_TRAJ;
-       }
-
-       DEBUG(E_USER_STRAT, "build zone reached");
-       return err;
-}
-
-/* must be called from the checkpoint before zone 0 */
-static uint8_t strat_goto_build_zone0_near(uint8_t level)
-{
-       uint8_t err;
-#ifdef OLD_STYLE
-       int16_t cur_y, diff_y, dst_y;
-#endif
-
-       /* turn to build zone */
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-       trajectory_a_abs(&mainboard.traj, COLOR_A(90));
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (!TRAJ_SUCCESS(err))
-               return err;
-
-#ifdef OLD_STYLE
-       cur_y = position_get_y_s16(&mainboard.pos);
-       dst_y = COLOR_Y(AREA_Y - (ROBOT_LENGTH/2) - 100);
-       diff_y = ABS(cur_y - dst_y);
-
-       /* move forward to reach the build zone */
-       strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
-       i2c_mechboard_mode_prepare_build_both(level);
-       trajectory_d_rel(&mainboard.traj, diff_y);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (err == END_BLOCKING) { /* not very good for z0 but... */
-               err = END_TRAJ;
-       }
-#else
-       /* move forward to reach the build zone */
-       strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
-       i2c_mechboard_mode_prepare_build_both(level);
-       err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
-       if (err == END_BLOCKING) {
-               err = END_TRAJ;
-       }
-#endif
-
-       DEBUG(E_USER_STRAT, "build zone reached");
-       return err;
-}
-
-/* Go to any build zone: disc, 1a or 1b. Doesn't work with zone 0 for
- * now... */
-uint8_t strat_goto_build_zone(struct build_zone *zone, uint8_t level)
-{
-       uint8_t err = END_TRAJ;
-       uint16_t old_spdd, old_spda;
-       int16_t checkpoint_x, checkpoint_y;
-       int16_t errx;
-
-       zone->last_try_time = time_get_s();
-
-       if (zone->flags & ZONE_F_DISC)
-               return strat_goto_disc(level);
-
-       DEBUG(E_USER_STRAT, "goto build zone x=%d", zone->checkpoint_x);
-       
-       /* workaround for some static cols configurations */
-       if (time_get_s() > 15)
-               i2c_mechboard_mode_loaded();
-
-       strat_get_speed(&old_spdd, &old_spda);
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
-       checkpoint_x = zone->checkpoint_x;
-       checkpoint_y = COLOR_Y(zone->checkpoint_y);
-       errx = position_get_x_s16(&mainboard.pos) - checkpoint_x;
-
-       /* goto checkpoint if we are too far from it, or if error on x
-        * is too big. */
-       if (distance_from_robot(checkpoint_x, checkpoint_y) > 300 ||
-           ABS(errx) > 15) {
-               err = goto_and_avoid(checkpoint_x, checkpoint_y,
-                                    TRAJ_FLAGS_STD,
-                                    TRAJ_FLAGS_NO_NEAR);
-               if (!TRAJ_SUCCESS(err))
-                       ERROUT(err);
-       }
-
-       if (zone->flags & ZONE_F_ZONE1)
-               err = strat_goto_build_zone1_near(level);
-       else if (zone->flags & ZONE_F_ZONE0)
-               err = strat_goto_build_zone0_near(level);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-
- end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-/* return a free temple structure */
-struct temple *strat_get_free_temple(void)
-{
-       uint8_t i;
-
-       for (i=0; i<MAX_TEMPLE; i++) {
-               if (!(strat_infos.temple_list[i].flags & TEMPLE_F_VALID))
-                       return &strat_infos.temple_list[i];
-       }
-       return NULL;
-}
-
-uint8_t strat_can_build_on_temple(struct temple *temple)
-{
-       uint8_t col_l, col_r, max_col, lintel;
-
-       col_l = get_column_count_left();
-       col_r = get_column_count_right();
-       max_col = (col_l > col_r ? col_l : col_r);
-       lintel = (get_lintel_count() > 0);
-       
-       if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
-               if ((temple->level_r > 5) && 
-                   (temple->flags & TEMPLE_F_ON_DISC))
-                       return 0;
-       }
-
-       /* return symetric temples only */
-       if (temple->level_l != temple->level_r)
-               return 0;
-
-       if ((time_get_s() - temple->last_try_time) < TEMPLE_DISABLE_TIME)
-               return 0;
-               
-       /* we could do better to work on non-symetric temples */
-       if (temple->level_l + max_col + lintel > 9)
-               return 0;
-
-       if (temple->flags & TEMPLE_F_MONOCOL)
-               return 0;
-
-       /* XXX don't allow to build on opponent temple. For that we
-        * must support the little back_mm. */
-       if (temple->flags & TEMPLE_F_OPPONENT)
-               return 0;
-
-       return 1;
-}
-
-
-/* return the best existing temple for building */
-struct temple *strat_get_best_temple(void)
-{
-       uint8_t i;
-       struct temple *best = NULL;
-       struct temple *temple = NULL;
-
-       for (i=0; i<MAX_TEMPLE; i++) {
-               temple = &strat_infos.temple_list[i];
-               
-               if (!(temple->flags & TEMPLE_F_VALID))
-                       continue;
-
-               if (strat_can_build_on_temple(temple) == 0)
-                       continue;
-
-               if (best == NULL) {
-                       best = temple;
-                       continue;
-               }
-
-               /* take the higher temple between 'best' and 'temple' */
-               if (best->level_l < temple->level_l)
-                       best = temple;
-       }
-
-       DEBUG(E_USER_STRAT, "%s() return %p", __FUNCTION__, best);
-       return best;
-}
-
-/* return the temple we built on the disc if any. If valid == 1, the
- * temple must be buildable. */
-struct temple *strat_get_our_temple_on_disc(uint8_t valid)
-{
-       uint8_t i;
-       struct temple *temple = NULL;
-
-       if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
-               return NULL;
-       }
-
-       for (i=0; i<MAX_TEMPLE; i++) {
-               temple = &strat_infos.temple_list[i];
-               
-               if (!(temple->flags & TEMPLE_F_VALID))
-                       continue;
-
-               if (valid == 1 && strat_can_build_on_temple(temple) == 0)
-                       continue;
-
-               if (temple->flags & TEMPLE_F_ON_DISC)
-                       return temple;
-       }
-       return NULL;
-}
-
-#define COL_MAX(x,y) (((x)>(y)) ? (x) : (y))
-
-#define TIME_FOR_LINTEL 3000L
-#define TIME_FOR_BUILD  0L
-#define TIME_FOR_COL    800L
-#define TIME_MARGIN     2000L
-
-#define CHECKPOINT_DISC_DIST 380
-#define CHECKPOINT_OTHER_DIST 200
-/* Grow a temple. It will update temple list. */
-uint8_t strat_grow_temple(struct temple *temple)
-{
-       double checkpoint_x, checkpoint_y;
-       uint8_t add_level = 0;
-       uint8_t do_lintel = 1;
-       uint8_t col_l, col_r, col_max;
-       uint8_t err;
-       uint16_t timeout;
-
-       /* XXX temple must be symetric */
-       uint8_t level = temple->level_l;
-
-       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
-
-       if (temple->level_l >= 9)
-               return END_ERROR;
-
-       if ( (temple->zone->flags & ZONE_F_ZONE1) ||
-            (temple->zone->flags & ZONE_F_ZONE0) ) {
-               strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
-               trajectory_d_rel(&mainboard.traj, -17);
-               wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       }
-
-       col_l = get_column_count_left();
-       col_r = get_column_count_right();
-       
-       if (time_get_s() < 75) {
-               /* make temple symetric: if we have 1 col on left and 2 cols
-                * on right, only build one on both sides. */
-               if (col_l > col_r) {
-                       col_r = col_l;
-                       do_lintel = 0;
-               }
-               if (col_r > col_l) {
-                       col_r = col_l;
-                       do_lintel = 0;
-               }
-               if (get_lintel_count() == 0)
-                       do_lintel = 0;
-       }
-       else if (col_l != col_r)
-               do_lintel = 0;
-
-       if (col_l == 0 || col_r == 0) {
-               if (temple->flags & TEMPLE_F_LINTEL)
-                       do_lintel = 0;
-       }
-
-       if (col_l == 0 && col_r == 0 && do_lintel == 0) {
-               DEBUG(E_USER_STRAT, "nothing to do");
-               return END_ERROR;
-       }
-
-       add_level = do_lintel + col_l;
-       while (level + add_level > 9) {
-               if (do_lintel) {
-                       do_lintel = 0;
-                       add_level = do_lintel + col_l;
-                       continue;
-               }
-               /* we know col_r and col_l are > 0 */
-               col_r--;
-               col_l--;
-       }
-
-       col_max = COL_MAX(col_r, col_l);
-
-       /* Reduce nb elts if we don't have time */
-       timeout = (!!col_max) * TIME_FOR_BUILD;
-       timeout += col_max * TIME_FOR_COL;
-       timeout += do_lintel * TIME_FOR_LINTEL;
-       if ((timeout / 1000L) + time_get_s() > 89 && do_lintel) {
-               do_lintel = 0;
-               timeout -= TIME_FOR_LINTEL;
-       }
-       if ((timeout / 1000L) + time_get_s() > 89 && col_max) {
-               if (col_r > 0)
-                       col_r--;
-               if (col_l > 0)
-                       col_l--;
-               col_max--;
-               timeout -= TIME_FOR_COL;
-       }
-
-       /* take a margin for timeout */
-       timeout += (!!col_max) * TIME_MARGIN;
-       
-       if (col_l == 0 && col_r == 0 && do_lintel == 0) {
-               DEBUG(E_USER_STRAT, "nothing to do (2)");
-               return END_ERROR;
-       }
-
-       DEBUG(E_USER_STRAT, "Autobuild: left=%d,%d right=%d,%d lintel=%d",
-             level, col_l, level, col_r, do_lintel);
-               
-       i2c_mechboard_mode_autobuild(level, col_l, I2C_AUTOBUILD_DEFAULT_DIST,
-                                    level, col_r, I2C_AUTOBUILD_DEFAULT_DIST,
-                                    do_lintel);
-       WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == 
-                            I2C_MECHBOARD_MODE_AUTOBUILD, 100);
-       err = WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != 
-                                  I2C_MECHBOARD_MODE_AUTOBUILD, timeout);
-       if (err == 0) {
-               DEBUG(E_USER_STRAT, "timeout building temple (timeout was %d)", timeout);
-               temple->flags = 0; /* remove temple from list */
-               return END_TRAJ;
-       }
-       else
-               DEBUG(E_USER_STRAT, "temple built");
-
-       /* position of the robot when build the new temple */
-       temple->x = position_get_x_s16(&mainboard.pos);
-       temple->y = position_get_y_s16(&mainboard.pos);
-       temple->a = position_get_a_deg_s16(&mainboard.pos);
-
-       /* checkpoint is a bit behind us */
-       if (temple->zone->flags & ZONE_F_DISC) {
-               rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
-                                &checkpoint_x, &checkpoint_y);
-       }
-       else {
-               rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
-                                &checkpoint_x, &checkpoint_y);
-       }
-       temple->checkpoint_x = checkpoint_x;
-       temple->checkpoint_y = checkpoint_y;
-
-       temple->level_l = level + add_level;
-       temple->dist_l = 0;
-       temple->angle_l = 0;
-       
-       temple->level_r = level + add_level;
-       temple->dist_r = 0;
-       temple->angle_r = 0;
-
-       temple->flags = TEMPLE_F_VALID;
-       
-       if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
-               temple->flags |= TEMPLE_F_ON_DISC;
-
-       if (do_lintel)
-               temple->flags |= TEMPLE_F_LINTEL;
-
-       /* we must push the temple */
-       if ( ((temple->zone->flags & ZONE_F_ZONE1) ||
-             (temple->zone->flags & ZONE_F_ZONE0)) &&
-            level <= 1) {
-               strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
-               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, 100);
-               wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       }
-
-       /* Special case for big 3 */
-       if (strat_infos.col_in_boobs) {
-               uint16_t old_spdd, old_spda;
-               strat_get_speed(&old_spdd, &old_spda);
-               strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_SLOW);
-               DEBUG(E_USER_STRAT, "%s() big 3", __FUNCTION__);
-               strat_infos.col_in_boobs = 0;
-               trajectory_d_rel(&mainboard.traj, -120);
-               wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-               i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
-                                                      I2C_MECHBOARD_MODE_CLEAR);
-               WAIT_COND_OR_TIMEOUT(get_column_count() >= 2, 4000L);
-               i2c_mechboard_mode_prepare_build_both(level + add_level);
-               time_wait_ms(800);
-               strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
-               trajectory_d_rel(&mainboard.traj, 120);
-               wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-               err = strat_grow_temple(temple);
-               strat_set_speed(old_spdd, old_spda);
-               return err;
-       }
-
-       return END_TRAJ;
-}
-
-#define COL_BACK_DIST 70
-#define COL_ANGLE     20
-#define COL_ARM_DIST  220
-
-#define COL_BACK_DIST_ZONE1 35
-#define COL_ARM_DIST_ZONE1  230
-#define COL_ANGLE_ZONE1     19
-
-static uint8_t try_build_col(uint8_t l, uint8_t r,
-                            uint8_t lp, uint8_t rp,
-                            uint8_t lvl)
-{
-       uint8_t max_lvl = lvl + r + l;
-
-       if (l == 0 && r == 0)
-               return 0;
-       if (lp - l == 2 && rp - r == 0)
-               return 0;
-       if (lp - l == 0 && rp - r == 2)
-               return 0;
-       if (max_lvl > 9)
-               return 0;
-       if (max_lvl == 9 && rp == 2 && r == 1)
-               return 0;
-       return max_lvl;
-}
-
-/* Grow a temple by building a column on it. It will update temple
- * list. */
-uint8_t strat_grow_temple_column(struct temple *temple)
-{
-       uint16_t old_spdd, old_spda;
-       double checkpoint_x, checkpoint_y;
-       uint8_t add_level = 0;
-       uint8_t col_l, col_r;
-       uint8_t col_l_before, col_r_before;
-       uint8_t err;
-       int16_t a_abs, a;
-       uint8_t level = temple->level_l;
-       uint8_t lvl_ok = 0, col_l_ok = 0, col_r_ok = 0;
-       uint8_t tmp_lvl;
-       int16_t col_arm_dist = COL_ARM_DIST;
-       int16_t col_back_dist = COL_BACK_DIST;
-       int16_t col_angle = COL_ANGLE;
-
-       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
-
-       if (level >= 9)
-               return END_ERROR;
-
-       strat_get_speed(&old_spdd, &old_spda);
-
-       if ( (temple->zone->flags & ZONE_F_ZONE1) ||
-            (temple->zone->flags & ZONE_F_ZONE0) ) {
-                    if (level == 1)
-                            col_arm_dist = COL_ARM_DIST_ZONE1;
-                    col_back_dist = COL_BACK_DIST_ZONE1;
-                    col_angle = COL_ANGLE_ZONE1;
-       }
-
-       a_abs = position_get_a_deg_s16(&mainboard.pos);
-
-       col_l_before = get_column_count_left();
-       col_r_before = get_column_count_right();
-       col_l = col_l_before;
-       col_r = col_r_before;
-       
-       /* check number of cols */
-       for (col_l = 0; col_l < col_l_before + 1; col_l++) {
-               for (col_r = 0; col_r < col_r_before + 1; col_r++) {
-                       tmp_lvl = try_build_col(col_l, col_r,
-                                               col_l_before,
-                                               col_r_before, level);
-                       if (tmp_lvl > lvl_ok) {
-                               lvl_ok = tmp_lvl;
-                               col_l_ok = col_l;
-                               col_r_ok = col_r;
-                       }
-               }
-       }
-
-       col_l = col_l_ok;
-       col_r = col_r_ok;
-       add_level = col_l + col_r;
-
-       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
-       
-       if (col_l == 0 && col_r == 0)
-               ERROUT(END_ERROR);
-
-       DEBUG(E_USER_STRAT, "Build col: left=%d right=%d",
-             col_l, col_r);
-
-       i2c_mechboard_mode_prepare_inside_both(level);
-       trajectory_d_rel(&mainboard.traj, -col_back_dist);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-               
-       /* build with left arm */
-       if (col_l) {
-               a = a_abs - col_angle;
-               if (a < -180)
-                       a += 360;
-               trajectory_a_abs(&mainboard.traj, a);
-               err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-               if (!TRAJ_SUCCESS(err))
-                       ERROUT(err);
-
-               if (time_get_s() > 88)
-                       return END_TIMER;
-
-               if (level >= 7 && get_column_count_left() == 2)
-                       i2c_mechboard_mode_prepare_build_select(level+1, -1);
-               else
-                       i2c_mechboard_mode_prepare_build_select(level, -1);
-               time_wait_ms(200);
-               i2c_mechboard_mode_autobuild(level, col_l, col_arm_dist,
-                                            0, 0, col_arm_dist, 0);
-               while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
-               while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
-               if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
-                   (col_r == 0) &&
-                   (temple->flags & TEMPLE_F_OPPONENT)) {
-                       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_LEFT_SIDE);
-                       time_wait_ms(500);
-                       trajectory_d_rel(&mainboard.traj, 100);
-                       wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-               }
-               else if ((level == 1 || level == 0) && (col_r == 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);
-               }
-
-               i2c_mechboard_mode_prepare_inside_select(level+col_l, -1);
-       }
-
-       /* build with right arm */
-       if (col_r) {
-               a = a_abs + col_angle;
-               if (a > 180)
-                       a -= 360;
-               trajectory_a_abs(&mainboard.traj, a);
-               err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-               if (!TRAJ_SUCCESS(err))
-                       ERROUT(err);
-               
-               if (time_get_s() > 88)
-                       return END_TIMER;
-               
-               if ((level+col_l) >= 7 && get_column_count_right() == 2)
-                       i2c_mechboard_mode_prepare_build_select(-1, level + col_l + 1);
-               else
-                       i2c_mechboard_mode_prepare_build_select(-1, level + col_l);
-               time_wait_ms(200);
-               i2c_mechboard_mode_autobuild(0, 0, col_arm_dist,
-                                            level + col_l, col_r,
-                                            col_arm_dist, 0);
-               while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
-               while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-               
-               if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
-                   (temple->flags & TEMPLE_F_OPPONENT)) {
-                       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);
-               }
-
-               i2c_mechboard_mode_prepare_inside_select(-1, level + col_l + col_r);
-               
-       }
-
-       trajectory_a_abs(&mainboard.traj, a_abs);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-
-       /* position of the robot when build the new temple */
-       temple->x = position_get_x_s16(&mainboard.pos);
-       temple->y = position_get_y_s16(&mainboard.pos);
-       temple->a = position_get_a_deg_s16(&mainboard.pos);
-
-       /* checkpoint is a bit behind us */
-       if (temple->zone->flags | ZONE_F_DISC) {
-               rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
-                                &checkpoint_x, &checkpoint_y);
-       }
-       else {
-               rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
-                                &checkpoint_x, &checkpoint_y);
-       }
-       temple->checkpoint_x = checkpoint_x;
-       temple->checkpoint_y = checkpoint_y;
-
-       temple->level_l = level + add_level;
-       temple->dist_l = 0; /* XXX */
-       temple->angle_l = 0;
-       
-       temple->level_r = level + add_level;
-       temple->dist_r = 0;
-       temple->angle_r = 0;
-
-       temple->flags = TEMPLE_F_VALID | TEMPLE_F_MONOCOL;
-       
-       if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
-               temple->flags |= TEMPLE_F_ON_DISC;
-
-       if ( (temple->zone->flags & ZONE_F_ZONE1) ||
-            (temple->zone->flags & ZONE_F_ZONE0) ) {
-
-       }
-       return END_TRAJ;
- end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-uint8_t strat_build_new_temple(struct build_zone *zone)
-{
-       struct temple *temple;
-       uint8_t level = zone->level;
-       uint8_t err;
-
-       /* create a dummy temple */
-       temple = strat_get_free_temple();
-       if (!temple)
-               return END_ERROR;
-       
-       memset(temple, 0, sizeof(*temple));
-       temple->level_l = level;
-       temple->level_r = level;
-       temple->flags = TEMPLE_F_VALID | TEMPLE_F_LINTEL;
-       temple->zone = zone;
-
-       zone->flags |= ZONE_F_BUSY;
-
-       if (time_get_s() > 50 && time_get_s() < 85 &&
-           get_lintel_count() == 0)
-               err = strat_grow_temple_column(temple);
-       else
-               err = strat_grow_temple(temple);
-
-       if (!TRAJ_SUCCESS(err))
-               temple->flags = 0;
-       return err;
-}
-
-uint8_t strat_goto_temple(struct temple *temple)
-{
-       uint16_t old_spdd, old_spda;
-       uint8_t err;
-
-       DEBUG(E_USER_STRAT, "goto temple %p checkpoint=%d,%d",
-             temple, temple->checkpoint_x, temple->checkpoint_y);
-       
-       temple->last_try_time = time_get_s();
-
-       strat_get_speed(&old_spdd, &old_spda);
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
-       i2c_mechboard_mode_loaded();
-
-       err = goto_and_avoid(temple->checkpoint_x, 
-                            temple->checkpoint_y,
-                            TRAJ_FLAGS_STD,
-                            TRAJ_FLAGS_NO_NEAR);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-       
-       err = strat_goto_build_zone(temple->zone, temple->level_r);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-
-       DEBUG(E_USER_STRAT, "zone reached");
-       ERROUT(END_TRAJ);
-
- end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-/* return the best existing temple for building */
-struct build_zone *strat_get_best_zone(void)
-{
-       uint8_t i;
-       struct build_zone *zone = NULL;
-
-       for (i=0; i<MAX_ZONE; i++) {
-               zone = &strat_infos.zone_list[i];
-
-               if (zone->flags & ZONE_F_BUSY)
-                       continue;
-               if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME)
-                       continue;
-               
-               return zone;
-       }
-       return NULL;
-}
diff --git a/projects/microb2010/mainboard/strat_column_disp.c b/projects/microb2010/mainboard/strat_column_disp.c
deleted file mode 100644 (file)
index e0b481e..0000000
+++ /dev/null
@@ -1,485 +0,0 @@
-/*  
- *  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_column_disp.c,v 1.5 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 <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.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 "../common/i2c_commands.h"
-#include "main.h"
-#include "actuator.h"
-#include "strat.h"
-#include "strat_base.h"
-#include "strat_avoid.h"
-#include "strat_utils.h"
-#include "sensor.h"
-#include "i2c_protocol.h"
-
-#define ERROUT(e) do {                         \
-               err = e;                        \
-               goto end;                       \
-       } while(0)
-
-/* distance between the wheel axis and the IR sensor */
-#define IR_SHIFT_DISTANCE_RIGHT 85
-#define IR_SHIFT_DISTANCE_LEFT  95
-
-/* return red or green sensor */
-#define COLOR_IR_SENSOR(left)                                          \
-       ({                                                              \
-               uint8_t __ret = 0;                                      \
-               if (left)                                               \
-                       __ret = sensor_get(S_DISP_LEFT);                \
-               else                                                    \
-                       __ret = sensor_get(S_DISP_RIGHT);               \
-                                                                       \
-               __ret;                                                  \
-       })                                                              \
-
-/* eject one col, some error codes are ignored here: we want to be
- * sure that the column is correctly ejected. */
-uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a)
-{
-       uint8_t err;
-
-       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
-       trajectory_d_rel(&mainboard.traj, -300);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
-       i2c_mechboard_mode_eject();
-       time_wait_ms(600);
-       trajectory_a_abs(&mainboard.traj, eject_a);
-
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
-       i2c_mechboard_mode_clear();
-       time_wait_ms(1000);
-       trajectory_a_abs(&mainboard.traj, pickup_a);
-
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       return err;
-}
-
-/* get columns from dispenser. Must be called when the robot is in
- * front of the dispenser. */
-static uint8_t strat_pickup_col_disp(struct column_dispenser *disp)
-{
-       uint16_t old_spdd, old_spda;
-       int16_t recalib_x, recalib_y;
-       int16_t eject_a, pickup_a;
-       uint8_t err, timeout = 0;
-       int8_t cols_count_before, cols_count_after, cols;
-        microseconds us;
-       uint8_t first_try = 1;
-       uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
-
-       /* XXX set lazy pickup mode */
-
-       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
-
-       strat_get_speed(&old_spdd, &old_spda);
-
-       cols_count_before = get_column_count();
-       pickup_a = COLOR_A(disp->pickup_a);
-       eject_a = COLOR_A(disp->eject_a);
-       recalib_x = disp->recalib_x;
-       recalib_y = COLOR_Y(disp->recalib_y);
-
-       strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
-
-       /* turn to dispenser */
-       i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
-       trajectory_a_abs(&mainboard.traj, pickup_a);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-
-       /* go forward until blocking, then go back ~30mm */
-
-       pickup_wheels_on();     
- retry:
-       if (time_get_s() > 86) {
-               DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
-               return END_TIMER;
-       }
-
-       if ((strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE) &&
-           strat_infos.col_in_boobs == 0 &&
-           strat_infos.lazy_pickup_done == 0) {
-               DEBUG(E_USER_STRAT, "%s() mode lazy", __FUNCTION__);
-               pickup_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
-               strat_infos.col_in_boobs = 1;
-               strat_infos.lazy_pickup_done = 1;
-       }
-       else {
-               pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
-               strat_infos.col_in_boobs = 0;
-       }
-
-       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, 120);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       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))
-               ERROUT(err);
-
-       if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
-               strat_eject_col(eject_a, pickup_a);
-               goto retry;
-       }
-
-       /* start to pickup with finger / arms */
-
-       DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__);
-       
-       if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
-               i2c_mechboard_mode_pickup();
-       else
-               i2c_mechboard_mode_lazy_pickup();
-       WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100);
-        us = time_get_us2();
-       cols = get_column_count();
-       while (get_mechboard_mode() == pickup_mode) {
-               if (get_column_count() != cols) {
-                       cols = get_column_count();
-                       us = time_get_us2();
-               }
-               if ((get_column_count() - cols_count_before) >= disp->count) {
-                       DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__);
-                       break;
-               }
-               /* 1 second timeout */
-               if (time_get_us2() - us > 1000000L) {
-                       DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__);
-                       timeout = 1;
-                       break;
-               }
-       }
-
-       /* eject if we found a bad color column */
-       
-       if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
-               strat_eject_col(eject_a, pickup_a);
-               goto retry;
-       }
-
-       /* only recalib if it was not a timeout or if we got at least
-        * 2 cols. */
-       if (timeout == 0 || (get_column_count() - cols_count_before >= 2))
-               strat_reset_pos(recalib_x, recalib_y, pickup_a);
-       else {
-               /* else just update x or y depending on disp */
-               if (disp == &strat_infos.c1)
-                       strat_reset_pos(recalib_x, DO_NOT_SET_POS,
-                                       DO_NOT_SET_POS);
-               else
-                       strat_reset_pos(recalib_x, DO_NOT_SET_POS,
-                                       DO_NOT_SET_POS);
-       }
-
-       /* go back */
-       
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-       trajectory_d_rel(&mainboard.traj, -300);
-       wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
-
-       /* update dispenser count */
-
-       cols_count_after = get_column_count();
-       cols = cols_count_after - cols_count_before;
-       if (cols > 0) {
-               DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
-               disp->count -= cols;
-               if (disp->count < 0)
-                       disp->count = 0;
-       }
-
-       pickup_wheels_off();
-       if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
-               i2c_mechboard_mode_clear();
-       else
-               disp->count -= 2;
-
-        ERROUT(END_TRAJ);
-
-end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-/* 
- * Go in front of a dispenser. It will update the dispenser if it is
- * c2 or c3 if we detect that this dispenser does not exist.
- */
-uint8_t strat_goto_col_disp(struct column_dispenser **pdisp)
-{
-       uint8_t err;
-       int16_t checkpoint_x, checkpoint_y;
-       int16_t scan_a;
-       uint16_t old_spdd, old_spda, scan_left;
-       int16_t pos1x, pos1y, pos2x, pos2y, pos, dist;
-       int16_t margin_col2, margin_col3;
-       struct column_dispenser *disp = *pdisp;
-
-       if (disp->count <= 0)
-               return END_ERROR;
-
-       if (disp->last_try_time >= time_get_s())
-               return END_ERROR;
-
-       disp->last_try_time = time_get_s();
-
-       strat_get_speed(&old_spdd, &old_spda);
-
-       i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
-                                              I2C_MECHBOARD_MODE_CLEAR);
-
-       /* set some useful variables */
-       checkpoint_x = disp->checkpoint_x;
-       checkpoint_y = COLOR_Y(disp->checkpoint_y);
-       scan_a = COLOR_A(disp->scan_a);
-       scan_left = COLOR_INVERT(disp->scan_left);
-
-       /* goto checkpoint */
-       DEBUG(E_USER_STRAT, "%s(): goto %s (%d,%d) scan_left=%d",
-             __FUNCTION__, disp->name, checkpoint_x,
-             checkpoint_y, scan_left);
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
-#if 0
-       /* we have an intermediate checkpoint if we are on our
-        * side. If goto_and_avoid() returns END_ERROR, skip
-        * this checkpoint.  */
-       if (position_get_x_s16(&mainboard.pos) < 1500) {
-               err = goto_and_avoid(1000, COLOR_Y(1500),
-                                    TRAJ_FLAGS_STD,
-                                    TRAJ_FLAGS_STD);
-               if (!TRAJ_SUCCESS(err) && err != END_ERROR)
-                       ERROUT(err);
-       }
-#endif
-       /* go to checkpoint near the dispenser */
-       err = goto_and_avoid(checkpoint_x, checkpoint_y,
-                            TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-
-       /* turn to correct angle to prepare scanning */
-
-       trajectory_a_abs(&mainboard.traj, scan_a);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-       
-       /* scan now */
-
-       DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__);
-
-       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
-       trajectory_d_rel(&mainboard.traj, -1000);
-       err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(scan_left),
-                                   TRAJ_FLAGS_NO_NEAR);
-       if (err) /* we should not reach end */
-               ERROUT(END_ERROR);
-       pos1x = position_get_x_s16(&mainboard.pos);
-       pos1y = position_get_y_s16(&mainboard.pos);
-
-       err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left),
-                                   TRAJ_FLAGS_NO_NEAR);
-       if (err)
-               ERROUT(END_ERROR);
-       pos2x = position_get_x_s16(&mainboard.pos);
-       pos2y = position_get_y_s16(&mainboard.pos);
-
-       dist = distance_between(pos1x, pos1y, pos2x, pos2y);
-       DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist);
-
-       if (scan_left)
-               trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2);
-       else
-               trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2);
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
-       if (disp == &strat_infos.c1) 
-               ERROUT(END_TRAJ);
-
-       /* mark c2 or c3 as empty... */
-       if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0) 
-               ERROUT(END_TRAJ);
-
-       pos = (pos2y + pos1y) / 2;
-       if (scan_a == 90) /* y is decreasing when scanning */
-               pos -= 80;
-       else if (scan_a == -90) /* y is increasing when scanning */
-               pos += 80;
-
-       margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y));
-       margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y));
-               
-       if (margin_col3 > margin_col2) {
-               DEBUG(E_USER_STRAT, "%s(): delete disp c3 (scan_pos=%d)", __FUNCTION__, pos);
-               strat_infos.c3.count = 0;
-               *pdisp = &strat_infos.c2;
-               if (strat_infos.c3.last_try_time > strat_infos.c2.last_try_time)
-                       strat_infos.c2.last_try_time = strat_infos.c3.last_try_time;
-       }
-       else {
-               DEBUG(E_USER_STRAT, "%s(): delete disp c2 (scan_pos=%d)", __FUNCTION__, pos);
-               strat_infos.c2.count = 0;
-               *pdisp = &strat_infos.c3;
-               if (strat_infos.c2.last_try_time > strat_infos.c3.last_try_time)
-                       strat_infos.c3.last_try_time = strat_infos.c2.last_try_time;
-       }
-        ERROUT(END_TRAJ);
-
-end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-/* return the best dispenser between the 2 */
-static struct column_dispenser *
-strat_disp_compare(struct column_dispenser *a,
-                  struct column_dispenser *b)
-{
-       uint8_t want_cols = 4 - get_column_count();
-
-       DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols);
-
-       /* an empty dispenser is not valid */
-       if (a->count == 0)
-               return b;
-       if (b->count == 0)
-               return a;
-
-       /* try to do a round robin: this is not optimal, but at least
-        * we will try another dispenser when one fails. */
-       if (a->last_try_time < b->last_try_time) {
-               return a;
-       }
-       if (b->last_try_time < a->last_try_time) {
-               return b;
-       }
-
-       /* take the one with the most columns */
-       if (a->count >= want_cols && b->count < want_cols)
-               return a;
-
-       /* take the one with the most columns */
-       if (b->count >= want_cols && a->count < want_cols)
-               return b;
-
-       /* the closer is the better */
-       if (distance_from_robot(a->recalib_x, COLOR_Y(a->recalib_y)) <
-           distance_from_robot(b->recalib_x, COLOR_Y(b->recalib_y))) {
-               return a;
-       }
-       return b;
-}
-
-/* choose the best dispenser */
-static struct column_dispenser *strat_get_best_col_disp(void)
-{
-       struct column_dispenser *disp;
-
-       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
-
-       /* for the first call, use c3 */
-       if (strat_infos.c1.last_try_time == 0 &&
-           strat_infos.c2.last_try_time == 0 &&
-           strat_infos.c3.last_try_time == 0)
-               return &strat_infos.c2; // XXX c3
-       
-       DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__);
-
-       /* else compare with standard conditions */
-       disp = strat_disp_compare(&strat_infos.c1, &strat_infos.c2);
-       disp = strat_disp_compare(disp, &strat_infos.c3);
-
-       if (disp->count == 0)
-               return NULL;
-       
-       return disp;
-}
-
-/* choose the best dispenser, depending on disp count, distance,
- * tries, ... and go pickup on it. */
-uint8_t strat_pickup_columns(void)
-{
-       struct column_dispenser *disp;
-       uint8_t err;
-
-       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
-       disp = strat_get_best_col_disp();
-
-       if (disp == NULL) {
-               DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__);
-               return END_ERROR;
-       }
-
-       err = strat_goto_col_disp(&disp);
-       if (!TRAJ_SUCCESS(err))
-               return err;
-
-       err = strat_pickup_col_disp(disp);
-       if (!TRAJ_SUCCESS(err))
-               return err;
-
-       return END_TRAJ;
-}
diff --git a/projects/microb2010/mainboard/strat_lintel.c b/projects/microb2010/mainboard/strat_lintel.c
deleted file mode 100644 (file)
index 72ab299..0000000
+++ /dev/null
@@ -1,247 +0,0 @@
-/*  
- *  Copyright Droids, Microb Technology (2008)
- * 
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *  Revision : $Id: strat_lintel.c,v 1.5 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 <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.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 "../common/i2c_commands.h"
-#include "main.h"
-#include "strat.h"
-#include "strat_base.h"
-#include "strat_avoid.h"
-#include "strat_utils.h"
-#include "sensor.h"
-#include "i2c_protocol.h"
-
-#define ERROUT(e) do {                         \
-               err = e;                        \
-               goto end;                       \
-       } while(0)
-
-#define X_PRE_MARGIN 20
-#define X_POST_MARGIN 10
-
-/*
- * goto lintel disp. Return END_TRAJ if success or if there is nothing
- * to do. Return END_ERROR if dest cannot be reached, else, it may
- * return END_OBSTACLE or END_BLOCKING.
- */
-uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp)
-{
-       uint8_t err, first_try = 1, right_ok, left_ok;
-       uint16_t old_spdd, old_spda;
-       int16_t left_cur, right_cur, a;
-       
-       if (disp->count == 0)
-               return END_ERROR;
-
-       if (get_lintel_count() == 2)
-               return END_ERROR;
-
-       if (disp->last_try_time >= time_get_s())
-               return END_ERROR;
-
-       disp->last_try_time = time_get_s();
-       
-       strat_get_speed(&old_spdd, &old_spda);
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
-       DEBUG(E_USER_STRAT, "%s(): goto %s", __FUNCTION__, disp->name);
-       i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
-
-       err = goto_and_avoid_backward(disp->x, COLOR_Y(400),
-                                     TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-       
-       trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
-       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-       
-       if (time_get_s() > 86) {
-               DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
-               return END_TIMER;
-       }
-
-       i2c_mechboard_mode_prepare_get_lintel();
- retry:
-       strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
-       err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
-       if (err == END_BLOCKING) {
-               a = position_get_a_deg_s16(&mainboard.pos);
-               /* only reset pos if angle is not too different */
-               if (ABS(a - COLOR_A(-90)) < 5)
-                       strat_reset_pos(DO_NOT_SET_POS,
-                                       COLOR_Y(ROBOT_LENGTH/2),
-                                       COLOR_A(-90));
-       }
-       else if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-       
-       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);
-
-       DEBUG(E_USER_STRAT, "%s left_ok=%d (%d), right_ok=%d (%d)", __FUNCTION__,
-             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);
-               }
-               /* XXX recalib x ? */
-               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, -200, 30);
-                       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-                       trajectory_d_a_rel(&mainboard.traj, 190, -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, -200, -30);
-                       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-                       trajectory_d_a_rel(&mainboard.traj, 190, 30);
-                       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-                       first_try = 0;
-                       goto retry;
-               }
-               /* else, lintel is ok */
-               else {
-                       strat_infos.taken_lintel ++;
-                       i2c_mechboard_mode_put_lintel();
-               }
-       }
-       else {
-               if (right_ok && left_ok) {
-                       /* lintel is ok */
-                       strat_infos.taken_lintel ++;
-                       i2c_mechboard_mode_put_lintel();
-               }
-               else {
-                       i2c_mechboard_mode_prepare_get_lintel();
-                       time_wait_ms(300);
-               }
-       }
-       disp->count--;
-
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-       trajectory_d_rel(&mainboard.traj, -250);
-       err = wait_traj_end(TRAJ_FLAGS_STD);
-       
-       ERROUT(err);
-
-end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-/* go pickup lintels on dispensers. Return END_TRAJ on success or if
-
- * there is nothing to do, else return the error status. */
-uint8_t strat_pickup_lintels(void)
-{
-       uint8_t err;
-
-       if (get_column_count() != 0)
-               return END_ERROR;
-
-       if (strat_infos.l1.count == 0 && strat_infos.l2.count == 0)
-               return END_TRAJ;
-
-       /* skip if it's too early */
-       if (time_get_s() < strat_infos.conf.lintel_min_time)
-               return END_TRAJ;
-
-       /* skip next lintel if we want only one */
-       if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
-               if (strat_infos.taken_lintel)
-                       return END_TRAJ;
-       }
-       
-       /* don't take lintel now if we already have one and if there
-        * is not much time */
-       if (get_lintel_count() && time_get_s() > 75)
-               return END_TRAJ;
-
-       /* take lintel 1 */
-       err = strat_goto_lintel_disp(&strat_infos.l1);
-       if (!TRAJ_SUCCESS(err) && err != END_ERROR)
-               return err;
-
-       /* skip next lintel if we want only one */
-       if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
-               if (strat_infos.taken_lintel)
-                       return END_TRAJ;
-       }
-       
-       /* don't take lintel now if we already have one and if there
-        * is not much time */
-       if (get_lintel_count() && time_get_s() > 75)
-               return END_TRAJ;
-
-       /* take lintel 2 */
-       err = strat_goto_lintel_disp(&strat_infos.l2);
-       if (!TRAJ_SUCCESS(err) && err != END_ERROR)
-               return err;
-
-       return END_TRAJ;
-}
diff --git a/projects/microb2010/mainboard/strat_scan.c b/projects/microb2010/mainboard/strat_scan.c
deleted file mode 100644 (file)
index 4140226..0000000
+++ /dev/null
@@ -1,689 +0,0 @@
-/*  
- *  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_scan.c,v 1.2 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 <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.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 "../common/i2c_commands.h"
-#include "main.h"
-#include "cmdline.h"
-#include "i2c_protocol.h"
-#include "strat.h"
-#include "strat_base.h"
-#include "strat_utils.h"
-#include "strat_avoid.h"
-#include "sensor.h"
-
-#define ERROUT(e) do {                         \
-               err = e;                        \
-               goto end;                       \
-       } while(0)
-
-
-void scanner_dump_state(void)
-{
-       uint8_t status;
-
-       printf_P(PSTR("scanner state:\r\n"));
-       status = sensorboard.scan_status;
-
-       printf_P(PSTR("  status=%x: "), sensorboard.scan_status);
-
-       if (status & I2C_SCAN_DONE)
-               printf_P(PSTR("DONE "));
-       else
-               printf_P(PSTR("RUNNING "));
-       if (status & I2C_SCAN_MAX_COLUMN)
-               printf_P(PSTR("OBSTACLE "));
-
-       printf_P(PSTR("\r\n"));
-
-       if (sensorboard.dropzone_h == -1) {
-               printf_P(PSTR("No zone found\r\n"));
-               return;
-       }
-       
-       printf_P(PSTR("  column_h=%d\r\n"), sensorboard.dropzone_h);
-       printf_P(PSTR("  column_x=%d\r\n"), sensorboard.dropzone_x);
-       printf_P(PSTR("  column_y=%d\r\n"), sensorboard.dropzone_y);
-}
-
-/* must be larger than the disc poly */
-#define CHECKPOINT_DIST 600
-
-/* go to a specific angle on disc, if level == -1, don't move arms */
-uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level)
-{
-       uint8_t err;
-       uint16_t old_spdd, old_spda;
-       double x, y;
-       uint8_t need_clear = 0;
-
-       DEBUG(E_USER_STRAT, "%s(a_deg=%d, level=%d)", __FUNCTION__,
-             a_deg, level);
-
-       strat_get_speed(&old_spdd, &old_spda);
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
-       /* workaround for some static cols configurations */
-       if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) {
-               if (time_get_s() > 15)
-                       i2c_mechboard_mode_loaded();
-       }
-       /* another workaround for offensive configuration */
-       else {
-               if (strat_infos.i2c_loaded_skipped == 0) {
-                       DEBUG(E_USER_STRAT, "%s() need clear");
-                       strat_infos.i2c_loaded_skipped = 1;
-                       i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
-                                                              I2C_MECHBOARD_MODE_CLEAR);
-                       need_clear = 1;
-               }
-               else
-                       i2c_mechboard_mode_loaded();
-       }
-
-
-       /* calculate the checkpoint */
-       x = CHECKPOINT_DIST;
-       y = 0;
-       rotate(&x, &y, RAD(a_deg));
-       x += CENTER_X;
-       y += CENTER_Y;
-
-       err = goto_and_avoid(x, y, TRAJ_FLAGS_STD,
-                            TRAJ_FLAGS_NO_NEAR);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-       
-       /* early offensive conf only */
-       if (need_clear) {
-               err = WAIT_COND_OR_TIMEOUT(get_column_count() == 2,
-                                          3000L);
-               DEBUG(E_USER_STRAT, "%s() offensive: err=%d", err);
-               if (err == 0) /* timeout */
-                       return END_ERROR;
-       }
-       err = strat_goto_disc(level);
-
- end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-       
-}
-
-/* only valid for temple on disc */
-int16_t strat_get_temple_angle(struct temple *temple)
-{
-       int16_t x, y;
-       double a;
-
-       x = temple->x;
-       y = temple->y;
-       x -= CENTER_X;
-       y -= CENTER_Y;
-       a = atan2(y, x);
-       return DEG(a);
-}
-
-#define SCAN_ANGLE_OFFSET (-40)
-int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle)
-{
-       return temple_angle + SCAN_ANGLE_OFFSET;
-}
-
-/* start to scan after this distance */
-#define DIST_START_SCAN 50
-
-/* scan during this distance (includes DIST_START_SCAN) */
-#define DIST_SCAN 430
-
-/* speed of the scan */
-#define SPEED_SCAN 450
-
-/* from scanner point of view */
-#define DISC_CENTER_X 15
-#define DISC_CENTER_Y 13
-
-/* distance of the checkpoint */
-#define CKPT_DST 550.
-
-/* to convert in robot coordinates */
-#define SIDE_OFFSET (ROBOT_WIDTH/2)
-#define DIST_OFFSET (DIST_SCAN - DIST_START_SCAN)
-
-/* center of the disc in robot coordinates */
-#define CENTER_X_SCANNER 166
-#define CENTER_Y_SCANNER 174
-
-/* center of the disc in scanner millimeters coordinates */
-#define CENTER_X_SCANNER2 120
-#define CENTER_Y_SCANNER2 155
-
-/* structure filled by strat_scan_disc() */
-struct scan_disc_result {      
-#define SCAN_FAILED              0
-#define SCAN_VALID               1
-       uint8_t status;
-
-#define SCAN_ACTION_BUILD_TEMPLE 0
-#define SCAN_ACTION_BUILD_COL    1
-       uint8_t action;
-
-       uint8_t level;
-};
-
-#define SCAN_MODE_CHECK_TEMPLE 0
-#define SCAN_MODE_SCAN_COL     1
-#define SCAN_MODE_SCAN_TEMPLE  2
-
-int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x,
-                                int16_t *ckpt_rel_y, int16_t *back_mm)
-{
-       int16_t center_rel_x, center_rel_y;
-       int16_t col_rel_x, col_rel_y;
-       int16_t col_vect_x, col_vect_y;
-       double col_vect_norm;
-       int16_t ckpt_vect_x, ckpt_vect_y;
-
-       /* do some filtering */
-       if (mode == SCAN_MODE_SCAN_TEMPLE &&
-           sensorboard.dropzone_x > CENTER_X_SCANNER) {
-               DEBUG(E_USER_STRAT, "x too big");
-               return -1;
-       }
-               
-       /* process relative pos from robot point of view */
-       center_rel_x = DIST_OFFSET - CENTER_Y_SCANNER;
-       center_rel_y = -(SIDE_OFFSET + CENTER_X_SCANNER);
-       
-       col_rel_x = DIST_OFFSET - sensorboard.dropzone_y;
-       col_rel_y = -(SIDE_OFFSET + sensorboard.dropzone_x);
-       DEBUG(E_USER_STRAT, "col_rel = %d,%d", col_rel_x, col_rel_y);
-       
-       /* vector from center to column */
-       col_vect_x = col_rel_x - center_rel_x;
-       col_vect_y = col_rel_y - center_rel_y;
-       col_vect_norm = norm(col_vect_x, col_vect_y);
-       
-       /* vector from center to ckpt */
-       ckpt_vect_x = (double)(col_vect_x) * CKPT_DST / col_vect_norm;
-       ckpt_vect_y = (double)(col_vect_y) * CKPT_DST / col_vect_norm;
-       
-       /* rel pos of ckpt */
-       *ckpt_rel_x = center_rel_x + ckpt_vect_x;
-       *ckpt_rel_y = center_rel_y + ckpt_vect_y;
-
-       /* do some filtering */
-       if (col_vect_norm > 150 || col_vect_norm < 30) {
-               DEBUG(E_USER_STRAT, "bad norm");
-               return -1;
-       }
-               
-       if (mode == SCAN_MODE_SCAN_TEMPLE) {
-               if (col_vect_norm > 50) {
-                       *back_mm = ABS(col_vect_norm-50);
-               }
-       }
-       return 0;
-}
-
-/* 
- * scan the disc: return END_TRAJ on success (and status in result is
- * set to SCAN_VALID). In this case, all the scan_disc_result
- * structure is filled with appropriate parameters.  mode can be
- * 'check' or 'scan_col'. Note that if we do a check_temple, the level
- * field in structure must be filled first by the caller.
- */
-uint8_t strat_scan_disc(int16_t angle, uint8_t mode,
-                       struct scan_disc_result *result)
-{
-       uint16_t old_spdd, old_spda;
-       uint8_t err, stop_scanner = 0;
-       uint8_t original_mode = mode;
-       int16_t pos1x, pos1y, dist;
-       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;
-
-       /* mark status as failed for now */
-       result->status = SCAN_FAILED;
-
-       DEBUG(E_USER_STRAT, "%s(angle=%d)", __FUNCTION__, angle);
-
-       strat_get_speed(&old_spdd, &old_spda);
-
-       /* go on disc */
-       err = strat_goto_disc_angle(angle, -1);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-       
-       /* wait opponent before scanning */
-       if (strat_infos.conf.wait_opponent > 0) {
-               int16_t opp_x, opp_y, opp_d, opp_a;
-               int8_t err;
-               microseconds us;
-
-               us = time_get_us2();
-               while ((err = get_opponent_xyda(&opp_x, &opp_y,
-                                               &opp_d, &opp_a)) == 0) {
-                       if (opp_d > 600)
-                               break;
-                       if (opp_a < 180)
-                               break;
-
-                       if (time_get_us2() - us >= (uint32_t)strat_infos.conf.wait_opponent * 1000000L)
-                               return END_ERROR;
-               }
-       }
-       
-       /* save absolute position of disc */
-       rel_da_to_abs_xy(265, 0, &center_abs_x, &center_abs_y);
-
-       strat_limit_speed_disable();
-
-       /* 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))
-               ERROUT(err);
-
-       /* XXX check that opp is not behind us */
-
-       /* prepare scanner */
-
-       stop_scanner = 1;
-       i2c_sensorboard_scanner_prepare();
-       time_wait_ms(250); /* XXX to remove ? */
-
-       strat_set_speed(SPEED_SCAN, 1000);
-
-       pos1x = position_get_x_s16(&mainboard.pos);
-       pos1y = position_get_y_s16(&mainboard.pos);
-       trajectory_d_rel(&mainboard.traj, -DIST_SCAN);
-       
-       while (1) {
-               err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
-               if (err != 0)
-                       break;
-               
-               dist = distance_from_robot(pos1x, pos1y);
-
-               if (dist > DIST_START_SCAN)
-                       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);
-               ERROUT(err);
-       }
-
-       /* start the scanner */
-
-       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);
-               ERROUT(err);
-       }
-
-       wait_scan_done(1000);
-
-       i2c_sensorboard_scanner_stop();
-       stop_scanner = 0;
-
-       if (mode == SCAN_MODE_CHECK_TEMPLE) {
-               i2c_sensorboard_scanner_algo_check(result->level,
-                                                  CENTER_X_SCANNER2,
-                                                  CENTER_Y_SCANNER2);
-               i2cproto_wait_update();
-               wait_scan_done(1000);
-               scanner_dump_state();
-
-               if (sensorboard.dropzone_h == -1 && 
-                   !(strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)) {
-                       DEBUG(E_USER_STRAT, "-- try to build a temple");
-                       mode = SCAN_MODE_SCAN_TEMPLE;
-               }
-               else {
-                       result->action = SCAN_ACTION_BUILD_TEMPLE;
-                       /* level is already set by caller */
-               }
-       }
-
-       if (mode == SCAN_MODE_SCAN_TEMPLE) {
-               i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
-                                                   DISC_CENTER_X,
-                                                   DISC_CENTER_Y);
-               i2cproto_wait_update();
-               wait_scan_done(1000);
-               scanner_dump_state();
-
-               if (sensorboard.dropzone_h == -1 ||
-                   strat_scan_get_checkpoint(mode, &ckpt_rel_x,
-                                             &ckpt_rel_y, &back_mm)) {
-                       if (original_mode != SCAN_MODE_CHECK_TEMPLE) {
-                               DEBUG(E_USER_STRAT, "-- try to build a column");
-                               mode = SCAN_MODE_SCAN_COL;
-                       }
-                       else {
-                               DEBUG(E_USER_STRAT, "-- check failed");
-                       }
-               }
-               else {
-                       result->action = SCAN_ACTION_BUILD_TEMPLE;
-                       result->level = sensorboard.dropzone_h;
-               }
-       }
-
-       if (mode == SCAN_MODE_SCAN_COL) {
-               i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
-                                                   DISC_CENTER_X,
-                                                   DISC_CENTER_Y);
-               i2cproto_wait_update();
-               wait_scan_done(1000);
-               scanner_dump_state();
-               
-               if (sensorboard.dropzone_h == -1 ||
-                   strat_scan_get_checkpoint(mode, &ckpt_rel_x,
-                                             &ckpt_rel_y, &back_mm)) {
-                       ERROUT(END_ERROR);
-               }
-               else {
-                       result->action = SCAN_ACTION_BUILD_COL;
-                       result->level = sensorboard.dropzone_h;
-               }
-       }
-
-       if (sensorboard.dropzone_h == -1) {
-               ERROUT(END_ERROR);
-       }
-
-       if (mode == SCAN_MODE_CHECK_TEMPLE) {
-               ckpt_rel_x = 220;
-               ckpt_rel_y = 100;
-       }
-
-       DEBUG(E_USER_STRAT, "rel xy for ckpt is %d,%d", 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);
-
-       DEBUG(E_USER_STRAT, "abs ckpt is %2.2f,%2.2f", ckpt_abs_x, ckpt_abs_y);
-
-       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))
-                       ERROUT(err);
-       }
-
-       trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
-       err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-
-       if (result->action == SCAN_ACTION_BUILD_TEMPLE) {
-               i2c_mechboard_mode_prepare_build_both(result->level);
-       }
-
-       trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
-       err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-       if (!TRAJ_SUCCESS(err))
-               ERROUT(err);
-
-       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, 400);
-       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 (TRAJ_SUCCESS(err))
-               err = END_ERROR; /* should not reach end */
-       if (err != END_BLOCKING && !TRAJ_SUCCESS(err)) 
-               ERROUT(err);
-
-       if (back_mm) {
-               trajectory_d_rel(&mainboard.traj, -back_mm);
-               wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-       }
-
-       result->status = SCAN_VALID;
-
-       strat_limit_speed_enable();
-       return END_TRAJ;
-
- end:
-       if (stop_scanner)
-               i2c_sensorboard_scanner_stop();
-       strat_limit_speed_enable();
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-       
-}
-
-/* do action according to scanner result. temple argument can be NULL
- * if it's a new one (from opponent) or it can be our previous
- * temple. */
-uint8_t strat_scan_do_action(struct scan_disc_result *scan_result,
-                            struct temple *temple, struct build_zone *zone)
-{
-       uint8_t err;
-
-       /* remove the temple from the list */
-       if (scan_result->status != SCAN_VALID)
-               return END_ERROR;
-
-       if (temple) {
-               /* we were scanning a temple, remove it */
-               if (scan_result->level != temple->level_l) {
-                       temple->flags = 0;
-                       temple = NULL;
-               }
-       }
-
-       if (temple == NULL) {
-               temple = strat_get_free_temple();
-               if (temple == NULL)
-                       return END_ERROR;
-               memset(temple, 0, sizeof(*temple));
-               temple->level_l = scan_result->level;
-               temple->level_r = scan_result->level;
-               temple->flags = TEMPLE_F_OPPONENT | 
-                       TEMPLE_F_VALID | TEMPLE_F_LINTEL;
-               temple->zone = zone;
-       }
-       zone->flags |= ZONE_F_BUSY;
-
-       switch (scan_result->action) {
-
-       case SCAN_ACTION_BUILD_COL:
-               err = strat_grow_temple_column(temple);
-               break;
-
-       case SCAN_ACTION_BUILD_TEMPLE:
-               err = strat_grow_temple(temple);
-               break;
-       default:
-               err = END_TRAJ;
-               break;
-       }
-       if (!TRAJ_SUCCESS(err))
-               temple->flags = 0;
-       return err;
-}
-
-uint8_t strat_build_on_opponent_temple(void)
-{
-       struct temple *temple;
-       uint8_t err;
-       struct scan_disc_result scan_result;
-       int16_t temple_angle;
-
-       if (time_get_s() < strat_infos.conf.scan_opp_min_time)
-               return END_TRAJ;
-
-       strat_infos.conf.scan_opp_min_time = 
-               time_get_s() + strat_infos.conf.delay_between_opp_scan;
-
-       /* scan on disc only */
-       if (strat_infos.conf.scan_opp_angle == -1) {
-               temple = strat_get_our_temple_on_disc(0);
-
-               /* scan the opposite of our temple if we found
-                * one on disc */
-               if (temple) {
-                       temple_angle = strat_get_temple_angle(temple);
-                       temple_angle += 180;
-                       if (temple_angle > 180)
-                               temple_angle -= 360;
-               }
-               /* else scan at 0 deg (opponent side) */
-               else {
-                       temple_angle = 0;
-               }
-       }
-       else {
-               /* user specified scan position */
-               temple_angle = strat_infos.conf.delay_between_opp_scan;
-               if (temple_angle > 180)
-                       temple_angle -= 360;
-       }
-       temple_angle = strat_temple_angle_to_scan_angle(temple_angle);
-       
-
-       err = strat_scan_disc(temple_angle, SCAN_MODE_SCAN_TEMPLE,
-                             &scan_result);
-       if (!TRAJ_SUCCESS(err))
-               return err;
-
-       /* XXX on disc only */
-       err = strat_scan_do_action(&scan_result, NULL,
-                                  &strat_infos.zone_list[0]);
-
-       if (!TRAJ_SUCCESS(err))
-               return err;
-       
-       err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
-       return err;
-}
-
-uint8_t strat_check_temple_and_build(void)
-{
-       struct temple *temple;
-       uint8_t err;
-       struct scan_disc_result scan_result;
-       int16_t temple_angle;
-
-       if (time_get_s() < strat_infos.conf.scan_our_min_time)
-               return END_TRAJ;
-       strat_infos.conf.scan_our_min_time = 
-               time_get_s() + strat_infos.conf.delay_between_our_scan;
-
-       /* on disc only, symetric only */
-       temple = strat_get_our_temple_on_disc(1);
-       if (temple == NULL)
-               return END_TRAJ;
-
-       temple_angle = strat_get_temple_angle(temple);
-       temple_angle = strat_temple_angle_to_scan_angle(temple_angle);
-       
-       scan_result.level = temple->level_l;
-       err = strat_scan_disc(temple_angle, SCAN_MODE_CHECK_TEMPLE,
-                             &scan_result);
-       if (scan_result.status != SCAN_VALID) {
-               temple->flags = 0;
-               temple = NULL;
-       }
-       /* no column after a temple check */
-       else if (scan_result.action == SCAN_ACTION_BUILD_COL &&
-                time_get_s() < 70)
-               err = END_ERROR;
-       if (!TRAJ_SUCCESS(err))
-               return err;
-
-       err = strat_scan_do_action(&scan_result, temple,
-                                  temple->zone);
-       if (!TRAJ_SUCCESS(err))
-               return err;
-       
-       err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
-       return err;
-}
diff --git a/projects/microb2010/mainboard/strat_static_columns.c b/projects/microb2010/mainboard/strat_static_columns.c
deleted file mode 100644 (file)
index 7af6043..0000000
+++ /dev/null
@@ -1,416 +0,0 @@
-/*  
- *  Copyright Droids, Microb Technology (2008)
- * 
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *  Revision : $Id: strat_static_columns.c,v 1.5 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 <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.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 "../common/i2c_commands.h"
-#include "main.h"
-#include "strat.h"
-#include "strat_base.h"
-#include "strat_utils.h"
-#include "strat_avoid.h"
-#include "sensor.h"
-#include "i2c_protocol.h"
-
-#define ERROUT(e) do {                         \
-               err = e;                        \
-               goto end;                       \
-       } while(0)
-
-#define BIG_DIST 5000
-
-/*
- * must be called from start area.
- * get 4 static columns and build a temple on the disc
- */
-uint8_t strat_static_columns(uint8_t configuration)
-{
-       uint8_t err;
-       uint8_t col1_present = 0, col4_present = 0;
-       uint16_t old_spdd, old_spda;
-
-       DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration);
-
-       strat_get_speed(&old_spdd, &old_spda);
-
-       /* calibrate scanner */
-       i2c_sensorboard_scanner_calib();
-
-       i2c_mechboard_mode_harvest();
-
-       /* go straight. total distance is less than 5 meters */
-       strat_set_speed(1000, 1000);
-       trajectory_d_rel(&mainboard.traj, BIG_DIST);
-
-       /* when y > 50, break */
-       err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD);
-       if (TRAJ_SUCCESS(err)) /* we should not reach end */
-               ERROUT(END_ERROR);
-       else if (err)
-               ERROUT(err);
-
-       /* turn to 90° abs while going forward */
-       DEBUG(E_USER_STRAT, "turn now");
-       strat_set_speed(1000, 350);
-       trajectory_only_a_abs(&mainboard.traj, COLOR_A(90));
-
-       /* when y > 100, check the presence of column 4 */
-       err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD);
-       if (TRAJ_SUCCESS(err)) /* we should not reach end */
-               ERROUT(END_ERROR);
-       else if (err)
-               ERROUT(err);
-       if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
-               col4_present = 1;
-       if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
-               col4_present = 1;
-
-       /* when y > 120, check the presence of column 1 */
-       err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD);
-       if (TRAJ_SUCCESS(err)) /* we should not reach end */
-               ERROUT(END_ERROR);
-       else if (err)
-               ERROUT(err);
-       if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
-               col1_present = 1;
-       if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
-               col1_present = 1;
-
-       /* when y > 130, break */
-       err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD);
-       if (TRAJ_SUCCESS(err)) /* we should not reach end */
-               ERROUT(END_ERROR);
-       else if (err)
-               ERROUT(err);
-
-       strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE;
-
-       DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present);
-       DEBUG(E_USER_STRAT, "have %d cols", get_column_count());
-
-       if (configuration == 0) {
-               if (get_column_count() > 2) {
-                       configuration = 1;
-                       if (col4_present || col1_present) {
-                               strat_infos.s_cols.flags |= 
-                                       STATIC_COL_LINE2_DONE;
-                       }
-                       else {
-                               strat_infos.s_cols.flags |= 
-                                       STATIC_COL_LINE1_DONE;
-                       }
-               }
-
-               /* only 2 colums on the first line */
-               else {
-                       /* all other colums are on line 1 */
-                       if (col4_present && col1_present) {
-                               configuration = 2;
-                               strat_infos.s_cols.flags |= 
-                                       STATIC_COL_LINE2_DONE;
-                       }
-
-                       /* only 2 columns on line 1, so there are also
-                        * 2 on line 2 */
-                       else if (col4_present || col1_present) {
-                               configuration = 4;
-                               strat_infos.s_cols.flags |= 
-                                       STATIC_COL_LINE2_DONE;
-                       }
-
-                       /* all other columns are on line 2 */
-                       else {
-                               configuration = 3;
-                               strat_infos.s_cols.flags |= 
-                                       STATIC_COL_LINE1_DONE;
-                       }
-               }
-       }
-
-       strat_infos.s_cols.configuration = configuration;
-       DEBUG(E_USER_STRAT, "use configuration %d", configuration);
-
-       if (configuration == 1) {
-               /* we already got 4 columns, go to the disc directly */
-
-               strat_set_speed(1500, 900);
-               trajectory_only_a_abs(&mainboard.traj, COLOR_A(0));
-               err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD);
-
-               if (TRAJ_SUCCESS(err)) /* we should not reach end */
-                       ERROUT(END_ERROR);
-               else if (err)
-                       ERROUT(err);
-       }
-       else if (configuration == 2 /* go from line 0 to line 1 */) {
-               strat_set_speed(800, 1000);
-               /* relative is needed here */
-               trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
-               err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD);
-               if (TRAJ_SUCCESS(err)) /* we should not reach end */
-                       ERROUT(END_ERROR);
-               else if (err)
-                       ERROUT(err);
-               strat_set_speed(1000, 600);
-               err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100),
-                                           TRAJ_FLAGS_STD);
-               if (TRAJ_SUCCESS(err)) /* we should not reach end */
-                       ERROUT(END_ERROR);
-               else if (err)
-                       ERROUT(err);
-       }
-       else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns
-                   on line 2*/) {
-               strat_set_speed(1000, 600);
-               /* relative is needed here */
-               trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
-               err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD);
-               if (TRAJ_SUCCESS(err)) /* we should not reach end */
-                       ERROUT(END_ERROR);
-               else if (err)
-                       ERROUT(err);
-       }       
-       else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns
-                     on line 2 */) {
-               strat_set_speed(1000, 600);
-               /* relative is needed here */
-               trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
-               err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD);
-               if (TRAJ_SUCCESS(err)) /* we should not reach end */
-                       ERROUT(END_ERROR);
-               else if (err)
-                       ERROUT(err);
-       }
-       else {
-               trajectory_stop(&mainboard.traj);
-       }
-
-       ERROUT(END_TRAJ);
-
- end:
-       strat_set_speed(old_spdd, old_spda);
-       return err;
-}
-
-/*
- * get last 2 columns
- * must be called after the first temple building
- */
-uint8_t strat_static_columns_pass2(void)
-{
-       uint16_t old_spdd, old_spda;
-       uint8_t side, err, next_mode;
-
-       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
-
-       strat_get_speed(&old_spdd, &old_spda);
-
-       if (get_color() == I2C_COLOR_RED)
-               side = I2C_RIGHT_SIDE;
-       else
-               side = I2C_LEFT_SIDE;
-
-       if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
-               next_mode = I2C_MECHBOARD_MODE_STORE;
-       else
-               next_mode = I2C_MECHBOARD_MODE_HARVEST;
-
-       switch (strat_infos.s_cols.configuration) {
-
-       /* configuration 1: 4 cols on line 0 */
-       case 1:
-