microb 2010
authorzer0 <zer0@carbon.local>
Sat, 19 Dec 2009 18:29:44 +0000 (19:29 +0100)
committerzer0 <zer0@carbon.local>
Sat, 19 Dec 2009 18:29:44 +0000 (19:29 +0100)
192 files changed:
projects/microb2010/bootloader/.config [new file with mode: 0644]
projects/microb2010/bootloader/Makefile [new file with mode: 0755]
projects/microb2010/bootloader/main.c [new file with mode: 0755]
projects/microb2010/bootloader/uart_config.h [new file with mode: 0755]
projects/microb2010/common/avr6.x [new file with mode: 0644]
projects/microb2010/common/eeprom_mapping.h [new file with mode: 0644]
projects/microb2010/common/i2c_commands.h [new file with mode: 0644]
projects/microb2010/common/i2c_commands.h.~1.9.~ [new file with mode: 0644]
projects/microb2010/mainboard/.config [new file with mode: 0644]
projects/microb2010/mainboard/.config.~1.5.~ [new file with mode: 0644]
projects/microb2010/mainboard/Makefile [new file with mode: 0755]
projects/microb2010/mainboard/Makefile.~1.6.~ [new file with mode: 0755]
projects/microb2010/mainboard/actuator.c [new file with mode: 0644]
projects/microb2010/mainboard/actuator.h [new file with mode: 0644]
projects/microb2010/mainboard/adc_config.h [new file with mode: 0644]
projects/microb2010/mainboard/ax12_config.h [new file with mode: 0755]
projects/microb2010/mainboard/ax12_user.c [new file with mode: 0644]
projects/microb2010/mainboard/ax12_user.h [new file with mode: 0644]
projects/microb2010/mainboard/beacon_test.png [new file with mode: 0644]
projects/microb2010/mainboard/cmdline.c [new file with mode: 0644]
projects/microb2010/mainboard/cmdline.h [new file with mode: 0644]
projects/microb2010/mainboard/commands.c [new file with mode: 0644]
projects/microb2010/mainboard/commands_ax12.c [new file with mode: 0644]
projects/microb2010/mainboard/commands_cs.c [new file with mode: 0644]
projects/microb2010/mainboard/commands_gen.c [new file with mode: 0644]
projects/microb2010/mainboard/commands_mainboard.c [new file with mode: 0644]
projects/microb2010/mainboard/commands_traj.c [new file with mode: 0644]
projects/microb2010/mainboard/cs.c [new file with mode: 0644]
projects/microb2010/mainboard/cs.h [new file with mode: 0644]
projects/microb2010/mainboard/diagnostic_config.h [new file with mode: 0644]
projects/microb2010/mainboard/encoders_spi_config.h [new file with mode: 0644]
projects/microb2010/mainboard/error_config.h [new file with mode: 0644]
projects/microb2010/mainboard/i2c_config.h [new file with mode: 0644]
projects/microb2010/mainboard/i2c_protocol.c [new file with mode: 0644]
projects/microb2010/mainboard/i2c_protocol.h [new file with mode: 0644]
projects/microb2010/mainboard/main.c [new file with mode: 0755]
projects/microb2010/mainboard/main.h [new file with mode: 0755]
projects/microb2010/mainboard/obstacle_avoidance_config.h [new file with mode: 0644]
projects/microb2010/mainboard/pid_config.h [new file with mode: 0755]
projects/microb2010/mainboard/rdline_config.h [new file with mode: 0644]
projects/microb2010/mainboard/scheduler_config.h [new file with mode: 0755]
projects/microb2010/mainboard/sensor.c [new file with mode: 0644]
projects/microb2010/mainboard/sensor.h [new file with mode: 0644]
projects/microb2010/mainboard/spi_config.h [new file with mode: 0644]
projects/microb2010/mainboard/strat.c [new file with mode: 0644]
projects/microb2010/mainboard/strat.h [new file with mode: 0644]
projects/microb2010/mainboard/strat_avoid.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_avoid.h [new file with mode: 0644]
projects/microb2010/mainboard/strat_base.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_base.h [new file with mode: 0644]
projects/microb2010/mainboard/strat_building.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_column_disp.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_lintel.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_scan.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_static_columns.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_utils.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_utils.h [new file with mode: 0644]
projects/microb2010/mainboard/time_config.h [new file with mode: 0755]
projects/microb2010/mainboard/timer_config.h [new file with mode: 0755]
projects/microb2010/mainboard/uart_config.h [new file with mode: 0644]
projects/microb2010/mechboard/.config [new file with mode: 0644]
projects/microb2010/mechboard/Makefile [new file with mode: 0644]
projects/microb2010/mechboard/actuator.c [new file with mode: 0644]
projects/microb2010/mechboard/actuator.h [new file with mode: 0644]
projects/microb2010/mechboard/adc_config.h [new file with mode: 0644]
projects/microb2010/mechboard/arm_highlevel.c [new file with mode: 0644]
projects/microb2010/mechboard/arm_highlevel.h [new file with mode: 0644]
projects/microb2010/mechboard/arm_xy.c [new file with mode: 0644]
projects/microb2010/mechboard/arm_xy.h [new file with mode: 0644]
projects/microb2010/mechboard/ax12_config.h [new file with mode: 0755]
projects/microb2010/mechboard/ax12_user.c [new file with mode: 0644]
projects/microb2010/mechboard/ax12_user.h [new file with mode: 0644]
projects/microb2010/mechboard/cmdline.c [new file with mode: 0644]
projects/microb2010/mechboard/cmdline.h [new file with mode: 0644]
projects/microb2010/mechboard/commands.c [new file with mode: 0644]
projects/microb2010/mechboard/commands_ax12.c [new file with mode: 0644]
projects/microb2010/mechboard/commands_cs.c [new file with mode: 0644]
projects/microb2010/mechboard/commands_gen.c [new file with mode: 0644]
projects/microb2010/mechboard/commands_mechboard.c [new file with mode: 0644]
projects/microb2010/mechboard/cs.c [new file with mode: 0644]
projects/microb2010/mechboard/cs.h [new file with mode: 0644]
projects/microb2010/mechboard/diagnostic_config.h [new file with mode: 0644]
projects/microb2010/mechboard/encoders_spi_config.h [new file with mode: 0644]
projects/microb2010/mechboard/error_config.h [new file with mode: 0644]
projects/microb2010/mechboard/i2c_config.h [new file with mode: 0644]
projects/microb2010/mechboard/i2c_protocol.c [new file with mode: 0644]
projects/microb2010/mechboard/i2c_protocol.h [new file with mode: 0644]
projects/microb2010/mechboard/main.c [new file with mode: 0755]
projects/microb2010/mechboard/main.h [new file with mode: 0755]
projects/microb2010/mechboard/pid_config.h [new file with mode: 0755]
projects/microb2010/mechboard/rdline_config.h [new file with mode: 0644]
projects/microb2010/mechboard/scheduler_config.h [new file with mode: 0755]
projects/microb2010/mechboard/sensor.c [new file with mode: 0644]
projects/microb2010/mechboard/sensor.h [new file with mode: 0644]
projects/microb2010/mechboard/spi_config.h [new file with mode: 0644]
projects/microb2010/mechboard/state.c [new file with mode: 0644]
projects/microb2010/mechboard/state.h [new file with mode: 0644]
projects/microb2010/mechboard/time_config.h [new file with mode: 0755]
projects/microb2010/mechboard/timer_config.h [new file with mode: 0755]
projects/microb2010/mechboard/uart_config.h [new file with mode: 0644]
projects/microb2010/microb_cmd/microbcmd.py [new file with mode: 0755]
projects/microb2010/sensorboard/.config [new file with mode: 0644]
projects/microb2010/sensorboard/Makefile [new file with mode: 0644]
projects/microb2010/sensorboard/actuator.c [new file with mode: 0644]
projects/microb2010/sensorboard/actuator.h [new file with mode: 0644]
projects/microb2010/sensorboard/adc_config.h [new file with mode: 0644]
projects/microb2010/sensorboard/ax12_config.h [new file with mode: 0755]
projects/microb2010/sensorboard/ax12_user.c [new file with mode: 0644]
projects/microb2010/sensorboard/ax12_user.h [new file with mode: 0644]
projects/microb2010/sensorboard/beacon.c [new file with mode: 0755]
projects/microb2010/sensorboard/beacon.h [new file with mode: 0755]
projects/microb2010/sensorboard/cmdline.c [new file with mode: 0644]
projects/microb2010/sensorboard/cmdline.h [new file with mode: 0644]
projects/microb2010/sensorboard/commands.c [new file with mode: 0644]
projects/microb2010/sensorboard/commands_ax12.c [new file with mode: 0644]
projects/microb2010/sensorboard/commands_cs.c [new file with mode: 0644]
projects/microb2010/sensorboard/commands_gen.c [new file with mode: 0644]
projects/microb2010/sensorboard/commands_scan.c [new file with mode: 0644]
projects/microb2010/sensorboard/commands_sensorboard.c [new file with mode: 0644]
projects/microb2010/sensorboard/cs.c [new file with mode: 0644]
projects/microb2010/sensorboard/cs.h [new file with mode: 0644]
projects/microb2010/sensorboard/diagnostic_config.h [new file with mode: 0644]
projects/microb2010/sensorboard/encoders_spi_config.h [new file with mode: 0644]
projects/microb2010/sensorboard/error_config.h [new file with mode: 0644]
projects/microb2010/sensorboard/gen_scan_tab.c [new file with mode: 0644]
projects/microb2010/sensorboard/i2c_config.h [new file with mode: 0644]
projects/microb2010/sensorboard/i2c_protocol.c [new file with mode: 0644]
projects/microb2010/sensorboard/i2c_protocol.h [new file with mode: 0644]
projects/microb2010/sensorboard/img_processing.c [new file with mode: 0644]
projects/microb2010/sensorboard/img_processing.h [new file with mode: 0644]
projects/microb2010/sensorboard/main.c [new file with mode: 0755]
projects/microb2010/sensorboard/main.h [new file with mode: 0755]
projects/microb2010/sensorboard/pid_config.h [new file with mode: 0755]
projects/microb2010/sensorboard/rdline_config.h [new file with mode: 0644]
projects/microb2010/sensorboard/scanner.c [new file with mode: 0644]
projects/microb2010/sensorboard/scanner.h [new file with mode: 0644]
projects/microb2010/sensorboard/scheduler_config.h [new file with mode: 0755]
projects/microb2010/sensorboard/sensor.c [new file with mode: 0644]
projects/microb2010/sensorboard/sensor.h [new file with mode: 0644]
projects/microb2010/sensorboard/spi_config.h [new file with mode: 0644]
projects/microb2010/sensorboard/time_config.h [new file with mode: 0755]
projects/microb2010/sensorboard/timer_config.h [new file with mode: 0755]
projects/microb2010/sensorboard/uart_config.h [new file with mode: 0644]
projects/microb2010/tests/static_beacon/.config [new file with mode: 0644]
projects/microb2010/tests/static_beacon/.config.old [new file with mode: 0644]
projects/microb2010/tests/static_beacon/Makefile [new file with mode: 0755]
projects/microb2010/tests/static_beacon/coin.c [new file with mode: 0644]
projects/microb2010/tests/static_beacon/hfuse [new file with mode: 0644]
projects/microb2010/tests/static_beacon/hfuse_new [new file with mode: 0644]
projects/microb2010/tests/static_beacon/lfuse [new file with mode: 0644]
projects/microb2010/tests/static_beacon/lfuse_new [new file with mode: 0644]
projects/microb2010/tests/static_beacon/static_beacon.bin [new file with mode: 0755]
projects/microb2010/tests/static_beacon/static_beacon.c [new file with mode: 0755]
projects/microb2010/tests/test_board2008/.config [new file with mode: 0644]
projects/microb2010/tests/test_board2008/.config.old [new file with mode: 0644]
projects/microb2010/tests/test_board2008/Makefile [new file with mode: 0755]
projects/microb2010/tests/test_board2008/action.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/action.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/actuator.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/actuator.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/adc_config.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/cmdline.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/cmdline.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/commands.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/commands_ax12.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/commands_cs.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/commands_gen.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/commands_mainboard.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/commands_traj.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/cs.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/cs.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/diagnostic_config.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/encoders_microb_config.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/encoders_spi_config.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/error_config.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/main. [new file with mode: 0644]
projects/microb2010/tests/test_board2008/main.bin [new file with mode: 0755]
projects/microb2010/tests/test_board2008/main.c [new file with mode: 0755]
projects/microb2010/tests/test_board2008/main.h [new file with mode: 0755]
projects/microb2010/tests/test_board2008/pid_config.h [new file with mode: 0755]
projects/microb2010/tests/test_board2008/rdline_config.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/scheduler_config.h [new file with mode: 0755]
projects/microb2010/tests/test_board2008/sensor.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/sensor.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/spi_config.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/strat_base.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/strat_base.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/strat_utils.c [new file with mode: 0644]
projects/microb2010/tests/test_board2008/strat_utils.h [new file with mode: 0644]
projects/microb2010/tests/test_board2008/time_config.h [new file with mode: 0755]
projects/microb2010/tests/test_board2008/timer_config.h [new file with mode: 0755]
projects/microb2010/tests/test_board2008/uart_config.h [new file with mode: 0644]

diff --git a/projects/microb2010/bootloader/.config b/projects/microb2010/bootloader/.config
new file mode 100644 (file)
index 0000000..b70ba89
--- /dev/null
@@ -0,0 +1,278 @@
+#
+# Automatically generated make config: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+# CONFIG_MCU_ATMEGA8 is not set
+# CONFIG_MCU_ATMEGA48 is not set
+# CONFIG_MCU_ATMEGA88 is not set
+# CONFIG_MCU_ATMEGA8515 is not set
+# CONFIG_MCU_ATMEGA8535 is not set
+# CONFIG_MCU_ATTINY13 is not set
+# CONFIG_MCU_ATTINY2313 is not set
+# CONFIG_MCU_ATMEGA16 is not set
+# CONFIG_MCU_ATMEGA161 is not set
+# CONFIG_MCU_ATMEGA162 is not set
+# CONFIG_MCU_ATMEGA163 is not set
+# CONFIG_MCU_ATMEGA165 is not set
+# CONFIG_MCU_ATMEGA168 is not set
+# CONFIG_MCU_ATMEGA169 is not set
+# CONFIG_MCU_ATMEGA32 is not set
+# CONFIG_MCU_ATMEGA323 is not set
+# CONFIG_MCU_ATMEGA325 is not set
+# CONFIG_MCU_ATMEGA3250 is not set
+# CONFIG_MCU_ATMEGA64 is not set
+# CONFIG_MCU_ATMEGA645 is not set
+# CONFIG_MCU_ATMEGA6450 is not set
+# CONFIG_MCU_ATMEGA128 is not set
+# CONFIG_MCU_ATMEGA1281 is not set
+# CONFIG_MCU_AT90CAN128 is not set
+# CONFIG_MCU_AT94K is not set
+# CONFIG_MCU_AT90S1200 is not set
+CONFIG_MCU_ATMEGA2560=y
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+# CONFIG_MATH_LIB is not set
+# CONFIG_FDEVOPEN_COMPAT is not set
+CONFIG_NO_PRINTF=y
+# CONFIG_MINIMAL_PRINTF is not set
+# CONFIG_STANDARD_PRINTF is not set
+CONFIG_FORMAT_IHEX=y
+# CONFIG_FORMAT_SREC is not set
+# CONFIG_FORMAT_BINARY is not set
+
+#
+# Base modules
+#
+
+#
+# Enable math library in generation options to see all modules
+#
+# CONFIG_MODULE_CIRBUF is not set
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+# CONFIG_MODULE_GEOMETRY is not set
+# CONFIG_MODULE_GEOMETRY_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER is not set
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+CONFIG_MODULE_SCHEDULER_TIMER0=y
+# CONFIG_MODULE_SCHEDULER_MANUAL is not set
+# CONFIG_MODULE_TIME is not set
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
+# CONFIG_MODULE_UART is not set
+# CONFIG_MODULE_UART_9BITS is not set
+# CONFIG_MODULE_UART_CREATE_CONFIG is not set
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+# CONFIG_MODULE_TIMER is not set
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
+# CONFIG_MODULE_ADC is not set
+# CONFIG_MODULE_ADC_CREATE_CONFIG is not set
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+# CONFIG_MODULE_VT100 is not set
+# CONFIG_MODULE_RDLINE is not set
+# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set
+# CONFIG_MODULE_RDLINE_KILL_BUF is not set
+# CONFIG_MODULE_RDLINE_HISTORY is not set
+# CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+
+#
+# Filters
+#
+# CONFIG_MODULE_PID is not set
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+# CONFIG_MODULE_QUADRAMP is not set
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+
+#
+# Some radio devices require SPI to be activated
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+
+#
+# Crypto modules depend on utils module
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+
+#
+# Encoding modules depend on utils module
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+
+#
+# Debug modules depend on utils module
+#
+# CONFIG_MODULE_DIAGNOSTIC is not set
+# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
+# CONFIG_MODULE_ERROR is not set
+# CONFIG_MODULE_ERROR_CREATE_CONFIG is not set
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
diff --git a/projects/microb2010/bootloader/Makefile b/projects/microb2010/bootloader/Makefile
new file mode 100755 (executable)
index 0000000..04c078e
--- /dev/null
@@ -0,0 +1,41 @@
+TARGET = main
+
+# repertoire des modules
+AVERSIVE_DIR = ../../..
+# VALUE, absolute or relative path : example ../.. #
+
+CFLAGS += -Werror
+
+# atm128
+# address is 0xf000 (in words)
+# LDFLAGS += -Wl,--section-start=.text=1e000
+# UART_NUM = 0
+
+# atm2560
+# address is 0x1f800 (in words)
+LDFLAGS += -Wl,--section-start=.text=3f000
+UART_NUM = 1
+
+CFLAGS += -DUART_NUM=$(UART_NUM)
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC = $(TARGET).c 
+
+# List Assembler source files here.
+# Make them always end in a capital .S.  Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC = 
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep
+       echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+       $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+
diff --git a/projects/microb2010/bootloader/main.c b/projects/microb2010/bootloader/main.c
new file mode 100755 (executable)
index 0000000..8595d72
--- /dev/null
@@ -0,0 +1,361 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: main.c,v 1.4 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+/*
+ * A simple bootloader example.
+ */
+
+#include <aversive.h>
+#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
+
+#include <stdlib.h>
+#include <string.h>
+#include <util/crc16.h>
+#include <avr/boot.h>
+
+#define BRAKE_DDR()     do { DDRJ |= 0xF0; } while(0)
+#define BRAKE_ON()      do { PORTJ |= 0xF0; } while(0)
+#define BRAKE_OFF()     do { PORTJ &= 0x0F; } while(0)
+
+#define LED1_ON()      sbi(PORTJ, 2)
+#define LED1_OFF()     cbi(PORTJ, 2)
+
+#define LED2_ON()      sbi(PORTJ, 3)
+#define LED2_OFF()     cbi(PORTJ, 3)
+
+
+#define NOECHO
+
+#ifdef NOECHO
+#define echo(c) do {} while(0)
+#else
+#define echo(c) uart_send(c)
+#endif
+
+#if UART_NUM == 0
+
+#define UCSRxA UCSR0A
+#define UCSRxB UCSR0B
+#define UCSRxC UCSR0C
+#define RXCx RXC0
+#define UDRx UDR0
+#define UDREx UDRE0
+#define U2Xx U2X0
+#define RXENx RXEN0
+#define TXENx TXEN0
+#define UCSZx0 UCSZ00
+#define UCSZx1 UCSZ01
+#define UBRRx UBRR0
+
+#elif  UART_NUM == 1
+
+#define UCSRxA UCSR1A
+#define UCSRxB UCSR1B
+#define UCSRxC UCSR1C
+#define RXCx RXC1
+#define UDRx UDR1
+#define UDREx UDRE1
+#define U2Xx U2X1
+#define RXENx RXEN1
+#define TXENx TXEN1
+#define UCSZx0 UCSZ10
+#define UCSZx1 UCSZ11
+#define UBRRx UBRR1
+
+#elif  UART_NUM == 2
+
+#define UCSRxA UCSR2A
+#define UCSRxB UCSR2B
+#define UCSRxC UCSR2C
+#define RXCx RXC2
+#define UDRx UDR2
+#define UDREx UDRE2
+#define U2Xx U2X2
+#define RXENx RXEN2
+#define TXENx TXEN2
+#define UCSZx0 UCSZ20
+#define UCSZx1 UCSZ21
+#define UBRRx UBRR2
+
+#elif  UART_NUM == 3
+
+#define UCSRxA UCSR3A
+#define UCSRxB UCSR3B
+#define UCSRxC UCSR3C
+#define RXCx RXC3
+#define UDRx UDR3
+#define UDREx UDRE3
+#define U2Xx U2X3
+#define RXENx RXEN3
+#define TXENx TXEN3
+#define UCSZx0 UCSZ30
+#define UCSZx1 UCSZ31
+#define UBRRx UBRR3
+
+#endif
+
+
+static char uart_recv(void) 
+{ 
+       while ( !(UCSRxA & (1<<RXCx)) ) ; 
+       return UDRx; 
+}
+
+static void uart_send(char c) 
+{ 
+       while ( !( UCSRxA & (1<<UDREx)) ) ;
+       UDRx = c; 
+}
+
+static void uart_puts(const char *buf)
+{
+       while (*buf)
+               uart_send(*(buf++));
+}
+
+static int8_t bootloader_query_hex(uint32_t *val)
+{
+       uint32_t tmp = 0;
+       int c;
+
+       while (1) {
+               c = uart_recv();
+               echo(c);
+
+               if (c == '\n' || c == '\r') {
+                       *val = tmp;
+                       return 0;
+               }
+               else if (c >= '0' && c <= '9') {
+                       tmp <<= 4;
+                       tmp += (c - '0');
+               } 
+               else if (c >= 'a' && c <= 'f') {
+                       tmp <<= 4;
+                       tmp += (c - 'a' + 10);
+               } 
+               else if (c >= 'A' && c <= 'F') {
+                       tmp <<= 4;
+                       tmp += (c - 'A' + 10);
+               } 
+               else
+                       return -1;
+       }
+       return 0;
+}
+
+/* launch application */
+static void launch_app(void)
+{
+       uart_puts("Boot...");
+       MCUCR = (1 << IVCE);
+       MCUCR = (0 << IVSEL);
+       reset();
+}
+
+static void disp_digit(uint8_t x)
+{
+       if (x < 10)
+               x += '0';
+       else
+               x += 'a' - 10 ;
+       uart_send(x);
+}
+
+static void disp_hex8(uint8_t x)
+{
+       disp_digit(x>>4);
+       disp_digit(x&0xf);
+}
+
+static void disp_hex16(uint16_t x)
+{
+       disp_hex8(x>>8);
+       disp_hex8(x);
+}
+
+static void crc_app(void)
+{
+       uint32_t start_addr, addr, size;
+       uint8_t c;
+       uint16_t crc = 0xffff;
+       uint16_t sum = 0;
+
+       uart_puts("addr?\r\n");
+       if (bootloader_query_hex(&start_addr))
+               goto fail;
+       if (start_addr > FLASHEND)
+               goto fail;
+       uart_puts("size?\r\n");
+       if (bootloader_query_hex(&size))
+               goto fail;
+       if (start_addr + size > FLASHEND)
+               goto fail;
+       for (addr=start_addr; addr<start_addr+size; addr++) {
+#if 0
+               /* ignore the 2nd page, it contains microb infos */
+               if (addr >= 256 && addr < 512)
+                       continue;
+#endif
+               c = pgm_read_byte_far(addr);
+               crc = _crc_ccitt_update(crc, c);
+               sum += c;
+       }
+       disp_hex16(crc);
+       disp_hex16(sum);
+       return;
+ fail:
+       uart_puts("KO");
+}
+
+static void read32(void)
+{
+       uint32_t start_addr, val = 0;
+       uint8_t c, i;
+
+       uart_puts("addr?\r\n");
+       if (bootloader_query_hex(&start_addr))
+               goto fail;
+       if (start_addr > FLASHEND)
+               goto fail;
+       for (i=0; i<4; i++) {
+               c = pgm_read_byte_far(start_addr+i);
+               val <<= 8;
+               val |= c;
+       }
+       disp_hex16(val);
+       return;
+ fail:
+       uart_puts("KO");
+}
+
+static void prog_page(void)
+{
+       int c;
+       uint32_t addr;
+       uint16_t i;
+       uint16_t crc = 0xffff;
+       uint16_t sum = 0;
+       uint8_t buf[SPM_PAGESIZE];
+
+#define SPM_PAGEMASK ((uint32_t)SPM_PAGESIZE-1)
+       uart_puts("addr?\r\n");
+       if (bootloader_query_hex(&addr))
+               goto fail;
+       if (addr > FLASHEND)
+               goto fail;
+       /* start_addr must be page aligned */
+       if (addr & SPM_PAGEMASK)
+               goto fail;
+
+       uart_puts("ok\r\n");
+
+       PORTJ = 0xF0;
+
+       /* data is received like the .bin format (which is already in
+        * little endian) */
+       for (i=0; i<SPM_PAGESIZE; i++) {
+               c = uart_recv();
+               crc = _crc_ccitt_update(crc, c);
+               sum += c;
+               buf[i] = c;
+       }
+       disp_hex16(crc);
+       disp_hex16(sum);
+       uart_puts(" (y?)\r\n");
+       c = uart_recv();
+       if (c != 'y')
+               goto fail;
+
+       /* erase page */
+        eeprom_busy_wait();
+        boot_page_erase(addr);
+        boot_spm_busy_wait();
+       
+       /* Set up little-endian word and fill tmp buf. */
+       for (i=0; i<SPM_PAGESIZE; i+=2) {
+               uint16_t w = buf[i] + ((uint16_t)(buf[i+1]) << 8);
+               boot_page_fill(addr + i, w);
+       }
+       
+       PORTJ = 0xFC;
+
+       boot_page_write(addr);
+       boot_spm_busy_wait();
+       
+       /* Reenable RWW-section again. We need this if we want to jump
+        * back to the application after bootloading. */
+       boot_rww_enable();
+       
+       uart_puts("OK");
+       return;
+ fail:
+       uart_puts("KO");
+}
+
+int main(void)
+{
+       int c;
+       uint32_t i=0;
+
+       /* disable all motors and switch on leds */
+       DDRJ = 0xFC;
+       PORTJ = 0xFC;
+
+       UCSRxA = _BV(U2Xx);
+       UCSRxB = _BV(RXENx) | _BV(TXENx);
+       UCSRxC = _BV(UCSZx1) | _BV(UCSZx0); /* 8 bits no parity 1 stop */
+       UBRRx = 34; /* 57600 at 16 Mhz */
+
+       /* move interrupt vector in bootloader section */
+       MCUCR = (1 << IVCE);
+       MCUCR = (1 << IVSEL);
+
+       sei();
+
+       uart_puts("\r\ncmd> ");
+
+       /* timeout */
+       while ( !(UCSRxA & (1<<RXCx)) ) {
+               i++;
+               if (i>1000000) /* wait about 1 sec */
+                       launch_app();
+       }
+
+       while (1) {
+               uart_puts("\r\ncmd> ");
+               c = uart_recv();
+               if (c == 'x')
+                       launch_app();
+               else if (c == 'c')
+                       crc_app();
+               else if (c == 'p')
+                       prog_page();
+               else if (c == 'd')
+                       read32();
+               else
+                       uart_puts("p:prog_page c:crc x:exec d:dump32");
+       }
+       
+       return 0;
+}
diff --git a/projects/microb2010/bootloader/uart_config.h b/projects/microb2010/bootloader/uart_config.h
new file mode 100755 (executable)
index 0000000..506806b
--- /dev/null
@@ -0,0 +1,120 @@
+/*  \r
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
+ * \r
+ *  This program is free software; you can redistribute it and/or modify\r
+ *  it under the terms of the GNU General Public License as published by\r
+ *  the Free Software Foundation; either version 2 of the License, or\r
+ *  (at your option) any later version.\r
+ *\r
+ *  This program is distributed in the hope that it will be useful,\r
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+ *  GNU General Public License for more details.\r
+ *\r
+ *  You should have received a copy of the GNU General Public License\r
+ *  along with this program; if not, write to the Free Software\r
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
+ *\r
+ *  Revision : $Id: uart_config.h,v 1.3 2009-05-27 20:04:06 zer0 Exp $\r
+ *\r
+ */\r
+\r
+/* Droids-corp 2004 - Zer0\r
+ * config for uart module\r
+ */\r
+\r
+#ifndef UART_CONFIG_H\r
+#define UART_CONFIG_H\r
+\r
+#if UART_NUM == 0\r
+\r
+/* compile uart0 fonctions, undefine it to pass compilation */\r
+#define UART0_COMPILE  \r
+\r
+/* enable uart0 if == 1, disable if == 0 */\r
+#define UART0_ENABLED  1\r
+\r
+/* enable uart0 interrupts if == 1, disable if == 0 */\r
+#define UART0_INTERRUPT_ENABLED  1\r
+\r
+#define UART0_BAUDRATE 57600\r
+\r
+#define UART0_USE_DOUBLE_SPEED 0\r
+\r
+#define UART0_RX_FIFO_SIZE 64\r
+#define UART0_TX_FIFO_SIZE 16\r
+\r
+#define UART0_NBITS 8\r
+#define UART0_PARITY UART_PARTITY_NONE\r
+#define UART0_STOP_BIT UART_STOP_BITS_1\r
+\r
+#elif UART_NUM == 1\r
+\r
+/* compile uart1 fonctions, undefine it to pass compilation */\r
+#define UART1_COMPILE  \r
+\r
+/* enable uart1 if == 1, disable if == 0 */\r
+#define UART1_ENABLED  1\r
+\r
+/* enable uart1 interrupts if == 1, disable if == 0 */\r
+#define UART1_INTERRUPT_ENABLED  1\r
+\r
+#define UART1_BAUDRATE 57600\r
+\r
+#define UART1_USE_DOUBLE_SPEED 0\r
+\r
+#define UART1_RX_FIFO_SIZE 32\r
+#define UART1_TX_FIFO_SIZE 4\r
+\r
+#define UART1_NBITS 8\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+\r
+#elif UART_NUM == 2\r
+\r
+/* compile uart2 fonctions, undefine it to pass compilation */\r
+#define UART2_COMPILE  \r
+\r
+/* enable uart2 if == 1, disable if == 0 */\r
+#define UART2_ENABLED  1\r
+\r
+/* enable uart2 interrupts if == 1, disable if == 0 */\r
+#define UART2_INTERRUPT_ENABLED  1\r
+\r
+#define UART2_BAUDRATE 57600\r
+\r
+#define UART2_USE_DOUBLE_SPEED 0\r
+\r
+#define UART2_RX_FIFO_SIZE 32\r
+#define UART2_TX_FIFO_SIZE 4\r
+\r
+#define UART2_NBITS 8\r
+#define UART2_PARITY UART_PARTITY_NONE\r
+#define UART2_STOP_BIT UART_STOP_BITS_1\r
+\r
+#elif UART_NUM == 3\r
+\r
+/* compile uart3 fonctions, undefine it to pass compilation */\r
+#define UART3_COMPILE  \r
+\r
+/* enable uart3 if == 1, disable if == 0 */\r
+#define UART3_ENABLED  1\r
+\r
+/* enable uart3 interrupts if == 1, disable if == 0 */\r
+#define UART3_INTERRUPT_ENABLED  1\r
+\r
+#define UART3_BAUDRATE 57600\r
+\r
+#define UART3_USE_DOUBLE_SPEED 0\r
+\r
+#define UART3_RX_FIFO_SIZE 32\r
+#define UART3_TX_FIFO_SIZE 4\r
+\r
+#define UART3_NBITS 8\r
+#define UART3_PARITY UART_PARTITY_NONE\r
+#define UART3_STOP_BIT UART_STOP_BITS_1\r
+\r
+#endif /* uart num */\r
+\r
+#endif\r
+\r
diff --git a/projects/microb2010/common/avr6.x b/projects/microb2010/common/avr6.x
new file mode 100644 (file)
index 0000000..a758e96
--- /dev/null
@@ -0,0 +1,269 @@
+/* Default linker script, for normal executables */
+OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr")
+OUTPUT_ARCH(avr:6)
+MEMORY
+{
+  text      (rx)   : ORIGIN = 0, LENGTH = 1024K
+  data      (rw!x) : ORIGIN = 0x800200, LENGTH = 0xfe00
+  eeprom    (rw!x) : ORIGIN = 0x810000, LENGTH = 64K
+  fuse      (rw!x) : ORIGIN = 0x820000, LENGTH = 1K
+  lock      (rw!x) : ORIGIN = 0x830000, LENGTH = 1K
+  signature (rw!x) : ORIGIN = 0x840000, LENGTH = 1K
+}
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  .hash          : { *(.hash)          }
+  .dynsym        : { *(.dynsym)                }
+  .dynstr        : { *(.dynstr)                }
+  .gnu.version   : { *(.gnu.version)   }
+  .gnu.version_d   : { *(.gnu.version_d)       }
+  .gnu.version_r   : { *(.gnu.version_r)       }
+  .rel.init      : { *(.rel.init)              }
+  .rela.init     : { *(.rela.init)     }
+  .rel.text      :
+    {
+      *(.rel.text)
+      *(.rel.text.*)
+      *(.rel.gnu.linkonce.t*)
+    }
+  .rela.text     :
+    {
+      *(.rela.text)
+      *(.rela.text.*)
+      *(.rela.gnu.linkonce.t*)
+    }
+  .rel.fini      : { *(.rel.fini)              }
+  .rela.fini     : { *(.rela.fini)     }
+  .rel.rodata    :
+    {
+      *(.rel.rodata)
+      *(.rel.rodata.*)
+      *(.rel.gnu.linkonce.r*)
+    }
+  .rela.rodata   :
+    {
+      *(.rela.rodata)
+      *(.rela.rodata.*)
+      *(.rela.gnu.linkonce.r*)
+    }
+  .rel.data      :
+    {
+      *(.rel.data)
+      *(.rel.data.*)
+      *(.rel.gnu.linkonce.d*)
+    }
+  .rela.data     :
+    {
+      *(.rela.data)
+      *(.rela.data.*)
+      *(.rela.gnu.linkonce.d*)
+    }
+  .rel.ctors     : { *(.rel.ctors)     }
+  .rela.ctors    : { *(.rela.ctors)    }
+  .rel.dtors     : { *(.rel.dtors)     }
+  .rela.dtors    : { *(.rela.dtors)    }
+  .rel.got       : { *(.rel.got)               }
+  .rela.got      : { *(.rela.got)              }
+  .rel.bss       : { *(.rel.bss)               }
+  .rela.bss      : { *(.rela.bss)              }
+  .rel.plt       : { *(.rel.plt)               }
+  .rela.plt      : { *(.rela.plt)              }
+  /* Internal text space or external memory.  */
+  .text :
+  {
+    *(.vectors)
+    KEEP(*(.vectors))
+    . = 256 + ALIGN(256); /* placeholder for misc microb infos */
+    /* For data that needs to reside in the lower 64k of progmem.  */
+    *(.progmem.gcc*)
+    *(.progmem*)
+    . = ALIGN(2048);
+     __trampolines_start = . ;
+    /* The jump trampolines for the 16-bit limited relocs will reside here.  */
+    *(.trampolines)
+    *(.trampolines*)
+     __trampolines_end = . ;
+    /* For future tablejump instruction arrays for 3 byte pc devices.
+       We don't relax jump/call instructions within these sections.  */
+    *(.jumptables)
+    *(.jumptables*)
+    /* For code that needs to reside in the lower 128k progmem.  */
+    *(.lowtext)
+    *(.lowtext*)
+     __ctors_start = . ;
+     *(.ctors)
+     __ctors_end = . ;
+     __dtors_start = . ;
+     *(.dtors)
+     __dtors_end = . ;
+    KEEP(SORT(*)(.ctors))
+    KEEP(SORT(*)(.dtors))
+    /* From this point on, we don't bother about wether the insns are
+       below or above the 16 bits boundary.  */
+    *(.init0)  /* Start here after reset.  */
+    KEEP (*(.init0))
+    *(.init1)
+    KEEP (*(.init1))
+    *(.init2)  /* Clear __zero_reg__, set up stack pointer.  */
+    KEEP (*(.init2))
+    *(.init3)
+    KEEP (*(.init3))
+    *(.init4)  /* Initialize data and BSS.  */
+    KEEP (*(.init4))
+    *(.init5)
+    KEEP (*(.init5))
+    *(.init6)  /* C++ constructors.  */
+    KEEP (*(.init6))
+    *(.init7)
+    KEEP (*(.init7))
+    *(.init8)
+    KEEP (*(.init8))
+    *(.init9)  /* Call main().  */
+    KEEP (*(.init9))
+    *(.text.*) /* trucs de gcc ? */
+    . = ALIGN(2048);
+    uart*(.text)
+    pwm*(.text)
+    parse*(.text)
+    rdline*(.text)
+    vt100*(.text)
+    scheduler*(.text)
+    control_system*(.text)
+    pid*(.text)
+    f08*(.text)
+    f16*(.text)
+    f32*(.text)
+    f64*(.text)
+    vect2*(.text)
+    quadramp*(.text)
+    blocking*(.text)
+    obstacle*(.text)
+    trajectory*(.text)
+    position*(.text)
+    adc*(.text)
+    robot*(.text)
+    error*(.text)
+    encoders*(.text)
+    time*(.text)
+    cirbuf*(.text)
+    i2c*(.text)
+    spi*(.text)
+    ax12*(.text)
+    . = ALIGN(2048);
+    /* some libc stuff */
+    str*(.text)
+    mem*(.text)
+    printf*(.text)
+    vfprintf*(.text)
+    sprintf*(.text)
+    snprintf*(.text)
+    malloc*(.text)
+    free*(.text)
+    fdevopen*(.text)
+    fputc*(.text)
+    . = ALIGN(2048);
+    *(.text)
+    . = ALIGN(2);
+    *(.fini9)  /* _exit() starts here.  */
+    KEEP (*(.fini9))
+    *(.fini8)
+    KEEP (*(.fini8))
+    *(.fini7)
+    KEEP (*(.fini7))
+    *(.fini6)  /* C++ destructors.  */
+    KEEP (*(.fini6))
+    *(.fini5)
+    KEEP (*(.fini5))
+    *(.fini4)
+    KEEP (*(.fini4))
+    *(.fini3)
+    KEEP (*(.fini3))
+    *(.fini2)
+    KEEP (*(.fini2))
+    *(.fini1)
+    KEEP (*(.fini1))
+    *(.fini0)  /* Infinite loop after program termination.  */
+    KEEP (*(.fini0))
+     _etext = . ;
+  }  > text
+  .data          : AT (ADDR (.text) + SIZEOF (.text))
+  {
+     PROVIDE (__data_start = .) ;
+    *(.data)
+    *(.data*)
+    *(.rodata)  /* We need to include .rodata here if gcc is used */
+    *(.rodata*) /* with -fdata-sections.  */
+    *(.gnu.linkonce.d*)
+    . = ALIGN(2);
+     _edata = . ;
+     PROVIDE (__data_end = .) ;
+  }  > data
+  .bss  SIZEOF(.data) + ADDR(.data) :
+  {
+     PROVIDE (__bss_start = .) ;
+    *(.bss)
+    *(.bss*)
+    *(COMMON)
+     PROVIDE (__bss_end = .) ;
+  }  > data
+   __data_load_start = LOADADDR(.data);
+   __data_load_end = __data_load_start + SIZEOF(.data);
+  /* Global data not cleared after reset.  */
+  .noinit  SIZEOF(.bss) + ADDR(.bss) :
+  {
+     PROVIDE (__noinit_start = .) ;
+    *(.noinit*)
+     PROVIDE (__noinit_end = .) ;
+     _end = . ;
+     PROVIDE (__heap_start = .) ;
+  }  > data
+  .eeprom  :
+  {
+    *(.eeprom*)
+     __eeprom_end = . ;
+  }  > eeprom
+  .fuse  :
+  {
+    KEEP(*(.fuse))
+    KEEP(*(.lfuse))
+    KEEP(*(.hfuse))
+    KEEP(*(.efuse))
+  }  > fuse
+  .lock  :
+  {
+    KEEP(*(.lock*))
+  }  > lock
+  .signature  :
+  {
+    KEEP(*(.signature*))
+  }  > signature
+  /* Stabs debugging sections.  */
+  .stab 0 : { *(.stab) }
+  .stabstr 0 : { *(.stabstr) }
+  .stab.excl 0 : { *(.stab.excl) }
+  .stab.exclstr 0 : { *(.stab.exclstr) }
+  .stab.index 0 : { *(.stab.index) }
+  .stab.indexstr 0 : { *(.stab.indexstr) }
+  .comment 0 : { *(.comment) }
+  /* DWARF debug sections.
+     Symbols in the DWARF debugging sections are relative to the beginning
+     of the section so we begin them at 0.  */
+  /* DWARF 1 */
+  .debug          0 : { *(.debug) }
+  .line           0 : { *(.line) }
+  /* GNU DWARF 1 extensions */
+  .debug_srcinfo  0 : { *(.debug_srcinfo) }
+  .debug_sfnames  0 : { *(.debug_sfnames) }
+  /* DWARF 1.1 and DWARF 2 */
+  .debug_aranges  0 : { *(.debug_aranges) }
+  .debug_pubnames 0 : { *(.debug_pubnames) }
+  /* DWARF 2 */
+  .debug_info     0 : { *(.debug_info) *(.gnu.linkonce.wi.*) }
+  .debug_abbrev   0 : { *(.debug_abbrev) }
+  .debug_line     0 : { *(.debug_line) }
+  .debug_frame    0 : { *(.debug_frame) }
+  .debug_str      0 : { *(.debug_str) }
+  .debug_loc      0 : { *(.debug_loc) }
+  .debug_macinfo  0 : { *(.debug_macinfo) }
+}
diff --git a/projects/microb2010/common/eeprom_mapping.h b/projects/microb2010/common/eeprom_mapping.h
new file mode 100644 (file)
index 0000000..01bd6e3
--- /dev/null
@@ -0,0 +1,33 @@
+/*
+ *  Copyright Droids Corporation (2007)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: eeprom_mapping.h,v 1.2 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+#ifndef _EEPROM_MAPPING_H_
+#define _EEPROM_MAPPING_H_
+
+#define EEPROM_MAGIC_MAINBOARD      1
+#define EEPROM_MAGIC_MECHBOARD      2
+#define EEPROM_MAGIC_SENSORBOARD    3
+
+#define EEPROM_MAGIC_ADDRESS ((uint8_t *)0x100)
+
+#define EEPROM_TIME_ADDRESS ((uint16_t *)0x110)
+
+#endif
diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
new file mode 100644 (file)
index 0000000..fdb5d49
--- /dev/null
@@ -0,0 +1,295 @@
+/*
+ *  Copyright Droids Corporation (2007)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: i2c_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+#ifndef _I2C_COMMANDS_H_
+#define _I2C_COMMANDS_H_
+
+#define I2C_MAINBOARD_ADDR   1
+#define I2C_MECHBOARD_ADDR   2
+#define I2C_SENSORBOARD_ADDR 3
+
+#define I2C_LEFT_SIDE   0
+#define I2C_RIGHT_SIDE  1
+#define I2C_AUTO_SIDE   2 /* for prepare_pickup */
+#define I2C_CENTER_SIDE 3 /* for prepare_pickup */
+
+#define I2C_COLOR_RED   0
+#define I2C_COLOR_GREEN 1
+
+#define I2C_AUTOBUILD_DEFAULT_DIST 210
+
+struct i2c_cmd_hdr {
+       uint8_t cmd;
+};
+
+/****/
+/* commands that do not need and answer */
+/****/
+
+#define I2C_CMD_GENERIC_LED_CONTROL 0x00
+
+struct i2c_cmd_led_control {
+       struct i2c_cmd_hdr hdr;
+       uint8_t led_num:7;
+       uint8_t state:1;        
+};
+
+/****/
+
+#define I2C_CMD_GENERIC_SET_COLOR 0x01
+
+struct i2c_cmd_generic_color {
+       struct i2c_cmd_hdr hdr;
+       uint8_t color;
+};
+
+/****/
+
+#define I2C_CMD_MECHBOARD_SET_MODE 0x02
+
+struct i2c_cmd_mechboard_set_mode {
+       struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_MODE_MANUAL             0x00
+#define I2C_MECHBOARD_MODE_HARVEST            0x01
+#define I2C_MECHBOARD_MODE_PREPARE_PICKUP     0x02
+#define I2C_MECHBOARD_MODE_PICKUP             0x03
+#define I2C_MECHBOARD_MODE_PREPARE_BUILD      0x04
+#define I2C_MECHBOARD_MODE_AUTOBUILD          0x05
+#define I2C_MECHBOARD_MODE_WAIT               0x06
+#define I2C_MECHBOARD_MODE_INIT               0x07
+#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08
+#define I2C_MECHBOARD_MODE_GET_LINTEL         0x09
+#define I2C_MECHBOARD_MODE_PUT_LINTEL         0x0A
+#define I2C_MECHBOARD_MODE_PREPARE_EJECT      0x0B
+#define I2C_MECHBOARD_MODE_EJECT              0x0C
+#define I2C_MECHBOARD_MODE_CLEAR              0x0D
+#define I2C_MECHBOARD_MODE_LAZY_HARVEST       0x0E
+#define I2C_MECHBOARD_MODE_LOADED             0x0F
+#define I2C_MECHBOARD_MODE_PREPARE_INSIDE     0x10
+#define I2C_MECHBOARD_MODE_STORE              0x11
+#define I2C_MECHBOARD_MODE_LAZY_PICKUP        0x12
+#define I2C_MECHBOARD_MODE_MANIVELLE          0x13
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE        0x14
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC   0x15
+#define I2C_MECHBOARD_MODE_EXIT               0xFF
+       uint8_t mode;
+       union {
+               struct {
+
+               } manual;
+
+               struct {
+                       uint8_t side;
+                       uint8_t next_mode;
+               } prep_pickup;
+
+               struct {
+                       uint8_t level_l;
+                       uint8_t level_r;
+               } prep_build;
+
+               struct {
+                       uint8_t side;
+               } push_temple_disc;
+
+               struct {
+                       uint8_t level;
+               } push_temple;
+
+               struct {
+                       uint8_t level_left;
+                       uint8_t level_right;
+                       uint8_t count_left;
+                       uint8_t count_right;
+                       uint8_t distance_left;
+                       uint8_t distance_right;
+                       uint8_t do_lintel;
+               } autobuild;
+
+               struct {
+                       uint8_t level_l;
+                       uint8_t level_r;
+               } prep_inside;
+       };
+};
+
+/****/
+
+/* only valid in manual mode */
+#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03
+
+struct i2c_cmd_mechboard_arm_goto {
+       struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_ARM_LEFT     0
+#define I2C_MECHBOARD_ARM_RIGHT    1
+#define I2C_MECHBOARD_ARM_BOTH     2
+       uint8_t which;
+
+       uint8_t height; /* in cm */
+       uint8_t distance; /* in cm */
+};
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04
+
+struct i2c_cmd_sensorboard_start_beacon {
+       struct i2c_cmd_hdr hdr;
+       uint8_t enable;
+};
+
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05
+
+struct i2c_cmd_sensorboard_scanner {
+       struct i2c_cmd_hdr hdr;
+
+#define I2C_SENSORBOARD_SCANNER_STOP    0x00
+#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01
+#define I2C_SENSORBOARD_SCANNER_START   0x02
+       uint8_t mode;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06
+struct i2c_cmd_sensorboard_calib_scanner {
+       struct i2c_cmd_hdr hdr;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07
+struct i2c_cmd_sensorboard_scanner_algo {
+       struct i2c_cmd_hdr hdr;
+
+#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1
+#define I2C_SCANNER_ALGO_CHECK_TEMPLE    2
+#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3
+       uint8_t algo;
+
+       union {
+               struct {
+#define I2C_SCANNER_ZONE_0     0
+#define I2C_SCANNER_ZONE_1     1
+#define I2C_SCANNER_ZONE_DISC  2
+                       uint8_t working_zone;
+                       int16_t center_x;
+                       int16_t center_y;
+               } drop_zone;
+               
+               struct {
+                       uint8_t level;
+                       int16_t temple_x;
+                       int16_t temple_y;
+               } check_temple;
+       };
+};
+
+/****/
+/* requests and their answers */
+/****/
+
+
+#define I2C_REQ_MECHBOARD_STATUS 0x80
+
+struct i2c_req_mechboard_status {
+       struct i2c_cmd_hdr hdr;
+
+       int16_t pump_left1_current;
+       int16_t pump_left2_current;
+};
+
+#define I2C_ANS_MECHBOARD_STATUS 0x81
+
+struct i2c_ans_mechboard_status {
+       struct i2c_cmd_hdr hdr;
+       /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */
+       uint8_t mode;
+
+#define I2C_MECHBOARD_STATUS_F_READY         0x00
+#define I2C_MECHBOARD_STATUS_F_BUSY          0x01
+#define I2C_MECHBOARD_STATUS_F_EXCPT         0x02
+       uint8_t status;
+
+       uint8_t lintel_count;
+
+       /* flag is there if column was taken by this pump. Note that
+        * we should also check ADC (current) to see if the column is
+        * still there. */
+#define I2C_MECHBOARD_COLUMN_L1              0x01
+#define I2C_MECHBOARD_COLUMN_L2              0x02
+#define I2C_MECHBOARD_COLUMN_R1              0x04
+#define I2C_MECHBOARD_COLUMN_R2              0x08
+       uint8_t column_flags;
+       
+       int16_t pump_left1;
+       int16_t pump_right1;
+       int16_t pump_left2;
+       int16_t pump_right2;
+
+#define I2C_MECHBOARD_CURRENT_COLUMN 85
+       int16_t pump_right1_current;
+       int16_t pump_right2_current;
+
+       uint16_t servo_lintel_left;
+       uint16_t servo_lintel_right;
+};
+
+#define I2C_REQ_SENSORBOARD_STATUS 0x82
+
+struct i2c_req_sensorboard_status {
+       struct i2c_cmd_hdr hdr;
+
+       /* position sent by mainboard */
+       int16_t x;
+       int16_t y;
+       int16_t a;
+
+       /* PWM for pickup */
+       uint8_t enable_pickup_wheels;
+};
+
+#define I2C_ANS_SENSORBOARD_STATUS 0x83
+
+struct i2c_ans_sensorboard_status {
+       struct i2c_cmd_hdr hdr;
+
+       uint8_t status;
+#define I2C_OPPONENT_NOT_THERE -1000
+       int16_t opponent_x;
+       int16_t opponent_y;
+       int16_t opponent_a;
+       int16_t opponent_d;
+
+#define I2C_SCAN_DONE 1
+#define I2C_SCAN_MAX_COLUMN 2
+       uint8_t scan_status;
+
+#define I2C_COLUMN_NO_DROPZONE -1
+       int8_t dropzone_h;
+       int16_t dropzone_x;
+       int16_t dropzone_y;
+};
+
+#endif /* _I2C_PROTOCOL_H_ */
diff --git a/projects/microb2010/common/i2c_commands.h.~1.9.~ b/projects/microb2010/common/i2c_commands.h.~1.9.~
new file mode 100644 (file)
index 0000000..d3072bc
--- /dev/null
@@ -0,0 +1,291 @@
+/*
+ *  Copyright Droids Corporation (2007)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: i2c_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+#ifndef _I2C_COMMANDS_H_
+#define _I2C_COMMANDS_H_
+
+#define I2C_MAINBOARD_ADDR   1
+#define I2C_MECHBOARD_ADDR   2
+#define I2C_SENSORBOARD_ADDR 3
+
+#define I2C_LEFT_SIDE   0
+#define I2C_RIGHT_SIDE  1
+#define I2C_AUTO_SIDE   2 /* for prepare_pickup */
+#define I2C_CENTER_SIDE 3 /* for prepare_pickup */
+
+#define I2C_COLOR_RED   0
+#define I2C_COLOR_GREEN 1
+
+#define I2C_AUTOBUILD_DEFAULT_DIST 210
+
+struct i2c_cmd_hdr {
+       uint8_t cmd;
+};
+
+/****/
+/* commands that do not need and answer */
+/****/
+
+#define I2C_CMD_GENERIC_LED_CONTROL 0x00
+
+struct i2c_cmd_led_control {
+       struct i2c_cmd_hdr hdr;
+       uint8_t led_num:7;
+       uint8_t state:1;        
+};
+
+/****/
+
+#define I2C_CMD_GENERIC_SET_COLOR 0x01
+
+struct i2c_cmd_generic_color {
+       struct i2c_cmd_hdr hdr;
+       uint8_t color;
+};
+
+/****/
+
+#define I2C_CMD_MECHBOARD_SET_MODE 0x02
+
+struct i2c_cmd_mechboard_set_mode {
+       struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_MODE_MANUAL             0x00
+#define I2C_MECHBOARD_MODE_HARVEST            0x01
+#define I2C_MECHBOARD_MODE_PREPARE_PICKUP     0x02
+#define I2C_MECHBOARD_MODE_PICKUP             0x03
+#define I2C_MECHBOARD_MODE_PREPARE_BUILD      0x04
+#define I2C_MECHBOARD_MODE_AUTOBUILD          0x05
+#define I2C_MECHBOARD_MODE_WAIT               0x06
+#define I2C_MECHBOARD_MODE_INIT               0x07
+#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08
+#define I2C_MECHBOARD_MODE_GET_LINTEL         0x09
+#define I2C_MECHBOARD_MODE_PUT_LINTEL         0x0A
+#define I2C_MECHBOARD_MODE_PREPARE_EJECT      0x0B
+#define I2C_MECHBOARD_MODE_EJECT              0x0C
+#define I2C_MECHBOARD_MODE_CLEAR              0x0D
+#define I2C_MECHBOARD_MODE_LAZY_HARVEST       0x0E
+#define I2C_MECHBOARD_MODE_LOADED             0x0F
+#define I2C_MECHBOARD_MODE_PREPARE_INSIDE     0x10
+#define I2C_MECHBOARD_MODE_STORE              0x11
+#define I2C_MECHBOARD_MODE_LAZY_PICKUP        0x12
+#define I2C_MECHBOARD_MODE_MANIVELLE          0x13
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE        0x14
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC   0x15
+#define I2C_MECHBOARD_MODE_EXIT               0xFF
+       uint8_t mode;
+       union {
+               struct {
+
+               } manual;
+
+               struct {
+                       uint8_t side;
+                       uint8_t next_mode;
+               } prep_pickup;
+
+               struct {
+                       uint8_t level_l;
+                       uint8_t level_r;
+               } prep_build;
+
+               struct {
+                       uint8_t side;
+               } push_temple_disc;
+
+               struct {
+                       uint8_t level_left;
+                       uint8_t level_right;
+                       uint8_t count_left;
+                       uint8_t count_right;
+                       uint8_t distance_left;
+                       uint8_t distance_right;
+                       uint8_t do_lintel;
+               } autobuild;
+
+               struct {
+                       uint8_t level_l;
+                       uint8_t level_r;
+               } prep_inside;
+       };
+};
+
+/****/
+
+/* only valid in manual mode */
+#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03
+
+struct i2c_cmd_mechboard_arm_goto {
+       struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_ARM_LEFT     0
+#define I2C_MECHBOARD_ARM_RIGHT    1
+#define I2C_MECHBOARD_ARM_BOTH     2
+       uint8_t which;
+
+       uint8_t height; /* in cm */
+       uint8_t distance; /* in cm */
+};
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04
+
+struct i2c_cmd_sensorboard_start_beacon {
+       struct i2c_cmd_hdr hdr;
+       uint8_t enable;
+};
+
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05
+
+struct i2c_cmd_sensorboard_scanner {
+       struct i2c_cmd_hdr hdr;
+
+#define I2C_SENSORBOARD_SCANNER_STOP    0x00
+#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01
+#define I2C_SENSORBOARD_SCANNER_START   0x02
+       uint8_t mode;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06
+struct i2c_cmd_sensorboard_calib_scanner {
+       struct i2c_cmd_hdr hdr;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07
+struct i2c_cmd_sensorboard_scanner_algo {
+       struct i2c_cmd_hdr hdr;
+
+#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1
+#define I2C_SCANNER_ALGO_CHECK_TEMPLE    2
+#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3
+       uint8_t algo;
+
+       union {
+               struct {
+#define I2C_SCANNER_ZONE_0     0
+#define I2C_SCANNER_ZONE_1     1
+#define I2C_SCANNER_ZONE_DISC  2
+                       uint8_t working_zone;
+                       int16_t center_x;
+                       int16_t center_y;
+               } drop_zone;
+               
+               struct {
+                       uint8_t level;
+                       int16_t temple_x;
+                       int16_t temple_y;
+               } check_temple;
+       };
+};
+
+/****/
+/* requests and their answers */
+/****/
+
+
+#define I2C_REQ_MECHBOARD_STATUS 0x80
+
+struct i2c_req_mechboard_status {
+       struct i2c_cmd_hdr hdr;
+
+       int16_t pump_left1_current;
+       int16_t pump_left2_current;
+};
+
+#define I2C_ANS_MECHBOARD_STATUS 0x81
+
+struct i2c_ans_mechboard_status {
+       struct i2c_cmd_hdr hdr;
+       /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */
+       uint8_t mode;
+
+#define I2C_MECHBOARD_STATUS_F_READY         0x00
+#define I2C_MECHBOARD_STATUS_F_BUSY          0x01
+#define I2C_MECHBOARD_STATUS_F_EXCPT         0x02
+       uint8_t status;
+
+       uint8_t lintel_count;
+
+       /* flag is there if column was taken by this pump. Note that
+        * we should also check ADC (current) to see if the column is
+        * still there. */
+#define I2C_MECHBOARD_COLUMN_L1              0x01
+#define I2C_MECHBOARD_COLUMN_L2              0x02
+#define I2C_MECHBOARD_COLUMN_R1              0x04
+#define I2C_MECHBOARD_COLUMN_R2              0x08
+       uint8_t column_flags;
+       
+       int16_t pump_left1;
+       int16_t pump_right1;
+       int16_t pump_left2;
+       int16_t pump_right2;
+
+#define I2C_MECHBOARD_CURRENT_COLUMN 85
+       int16_t pump_right1_current;
+       int16_t pump_right2_current;
+
+       uint16_t servo_lintel_left;
+       uint16_t servo_lintel_right;
+};
+
+#define I2C_REQ_SENSORBOARD_STATUS 0x82
+
+struct i2c_req_sensorboard_status {
+       struct i2c_cmd_hdr hdr;
+
+       /* position sent by mainboard */
+       int16_t x;
+       int16_t y;
+       int16_t a;
+
+       /* PWM for pickup */
+       uint8_t enable_pickup_wheels;
+};
+
+#define I2C_ANS_SENSORBOARD_STATUS 0x83
+
+struct i2c_ans_sensorboard_status {
+       struct i2c_cmd_hdr hdr;
+
+       uint8_t status;
+#define I2C_OPPONENT_NOT_THERE -1000
+       int16_t opponent_x;
+       int16_t opponent_y;
+       int16_t opponent_a;
+       int16_t opponent_d;
+
+#define I2C_SCAN_DONE 1
+#define I2C_SCAN_MAX_COLUMN 2
+       uint8_t scan_status;
+
+#define I2C_COLUMN_NO_DROPZONE -1
+       int8_t dropzone_h;
+       int16_t dropzone_x;
+       int16_t dropzone_y;
+};
+
+#endif /* _I2C_PROTOCOL_H_ */
diff --git a/projects/microb2010/mainboard/.config b/projects/microb2010/mainboard/.config
new file mode 100644 (file)
index 0000000..18a0d9a
--- /dev/null
@@ -0,0 +1,252 @@
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+# CONFIG_MCU_ATMEGA8 is not set
+# CONFIG_MCU_ATMEGA48 is not set
+# CONFIG_MCU_ATMEGA88 is not set
+# CONFIG_MCU_ATMEGA8515 is not set
+# CONFIG_MCU_ATMEGA8535 is not set
+# CONFIG_MCU_ATTINY13 is not set
+# CONFIG_MCU_ATTINY2313 is not set
+# CONFIG_MCU_ATMEGA16 is not set
+# CONFIG_MCU_ATMEGA161 is not set
+# CONFIG_MCU_ATMEGA162 is not set
+# CONFIG_MCU_ATMEGA163 is not set
+# CONFIG_MCU_ATMEGA165 is not set
+# CONFIG_MCU_ATMEGA168 is not set
+# CONFIG_MCU_ATMEGA169 is not set
+# CONFIG_MCU_ATMEGA32 is not set
+# CONFIG_MCU_ATMEGA323 is not set
+# CONFIG_MCU_ATMEGA325 is not set
+# CONFIG_MCU_ATMEGA3250 is not set
+# CONFIG_MCU_ATMEGA64 is not set
+# CONFIG_MCU_ATMEGA645 is not set
+# CONFIG_MCU_ATMEGA6450 is not set
+# CONFIG_MCU_ATMEGA128 is not set
+# CONFIG_MCU_ATMEGA1281 is not set
+# CONFIG_MCU_AT90CAN128 is not set
+# CONFIG_MCU_AT94K is not set
+# CONFIG_MCU_AT90S1200 is not set
+CONFIG_MCU_ATMEGA2560=y
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+CONFIG_MATH_LIB=y
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+# CONFIG_STANDARD_PRINTF is not set
+CONFIG_ADVANCED_PRINTF=y
+# CONFIG_FORMAT_IHEX is not set
+# CONFIG_FORMAT_SREC is not set
+CONFIG_FORMAT_BINARY=y
+
+#
+# Base modules
+#
+CONFIG_MODULE_CIRBUF=y
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+CONFIG_MODULE_FIXED_POINT=y
+CONFIG_MODULE_VECT2=y
+CONFIG_MODULE_GEOMETRY=y
+CONFIG_MODULE_SCHEDULER=y
+CONFIG_MODULE_SCHEDULER_STATS=y
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+# CONFIG_MODULE_SCHEDULER_TIMER0 is not set
+CONFIG_MODULE_SCHEDULER_MANUAL=y
+CONFIG_MODULE_TIME=y
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
+CONFIG_MODULE_UART_CREATE_CONFIG=y
+CONFIG_MODULE_SPI=y
+CONFIG_MODULE_SPI_CREATE_CONFIG=y
+CONFIG_MODULE_I2C=y
+CONFIG_MODULE_I2C_MASTER=y
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+CONFIG_MODULE_I2C_CREATE_CONFIG=y
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+CONFIG_MODULE_TIMER=y
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+CONFIG_MODULE_PWM_NG=y
+CONFIG_MODULE_ADC=y
+CONFIG_MODULE_ADC_CREATE_CONFIG=y
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+CONFIG_MODULE_VT100=y
+CONFIG_MODULE_RDLINE=y
+CONFIG_MODULE_RDLINE_CREATE_CONFIG=y
+CONFIG_MODULE_RDLINE_KILL_BUF=y
+CONFIG_MODULE_RDLINE_HISTORY=y
+CONFIG_MODULE_PARSE=y
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+CONFIG_MODULE_AX12=y
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+CONFIG_MODULE_ENCODERS_SPI=y
+CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y
+
+#
+# Robot specific modules
+#
+CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+CONFIG_MODULE_POSITION_MANAGER=y
+CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
+CONFIG_MODULE_TRAJECTORY_MANAGER=y
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y
+
+#
+# Control system modules
+#
+CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+CONFIG_MODULE_PID=y
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+CONFIG_MODULE_QUADRAMP=y
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+CONFIG_MODULE_DIAGNOSTIC=y
+CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
+CONFIG_MODULE_ERROR=y
+CONFIG_MODULE_ERROR_CREATE_CONFIG=y
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
diff --git a/projects/microb2010/mainboard/.config.~1.5.~ b/projects/microb2010/mainboard/.config.~1.5.~
new file mode 100644 (file)
index 0000000..860d979
--- /dev/null
@@ -0,0 +1,251 @@
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+# CONFIG_MCU_ATMEGA8 is not set
+# CONFIG_MCU_ATMEGA48 is not set
+# CONFIG_MCU_ATMEGA88 is not set
+# CONFIG_MCU_ATMEGA8515 is not set
+# CONFIG_MCU_ATMEGA8535 is not set
+# CONFIG_MCU_ATTINY13 is not set
+# CONFIG_MCU_ATTINY2313 is not set
+# CONFIG_MCU_ATMEGA16 is not set
+# CONFIG_MCU_ATMEGA161 is not set
+# CONFIG_MCU_ATMEGA162 is not set
+# CONFIG_MCU_ATMEGA163 is not set
+# CONFIG_MCU_ATMEGA165 is not set
+# CONFIG_MCU_ATMEGA168 is not set
+# CONFIG_MCU_ATMEGA169 is not set
+# CONFIG_MCU_ATMEGA32 is not set
+# CONFIG_MCU_ATMEGA323 is not set
+# CONFIG_MCU_ATMEGA325 is not set
+# CONFIG_MCU_ATMEGA3250 is not set
+# CONFIG_MCU_ATMEGA64 is not set
+# CONFIG_MCU_ATMEGA645 is not set
+# CONFIG_MCU_ATMEGA6450 is not set
+# CONFIG_MCU_ATMEGA128 is not set
+# CONFIG_MCU_ATMEGA1281 is not set
+# CONFIG_MCU_AT90CAN128 is not set
+# CONFIG_MCU_AT94K is not set
+# CONFIG_MCU_AT90S1200 is not set
+CONFIG_MCU_ATMEGA2560=y
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+CONFIG_MATH_LIB=y
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+# CONFIG_STANDARD_PRINTF is not set
+CONFIG_ADVANCED_PRINTF=y
+# CONFIG_FORMAT_IHEX is not set
+# CONFIG_FORMAT_SREC is not set
+CONFIG_FORMAT_BINARY=y
+
+#
+# Base modules
+#
+CONFIG_MODULE_CIRBUF=y
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+CONFIG_MODULE_FIXED_POINT=y
+CONFIG_MODULE_VECT2=y
+CONFIG_MODULE_GEOMETRY=y
+CONFIG_MODULE_SCHEDULER=y
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+# CONFIG_MODULE_SCHEDULER_TIMER0 is not set
+CONFIG_MODULE_SCHEDULER_MANUAL=y
+CONFIG_MODULE_TIME=y
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
+CONFIG_MODULE_UART_CREATE_CONFIG=y
+CONFIG_MODULE_SPI=y
+CONFIG_MODULE_SPI_CREATE_CONFIG=y
+CONFIG_MODULE_I2C=y
+CONFIG_MODULE_I2C_MASTER=y
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+CONFIG_MODULE_I2C_CREATE_CONFIG=y
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+CONFIG_MODULE_TIMER=y
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+CONFIG_MODULE_PWM_NG=y
+CONFIG_MODULE_ADC=y
+CONFIG_MODULE_ADC_CREATE_CONFIG=y
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+CONFIG_MODULE_VT100=y
+CONFIG_MODULE_RDLINE=y
+CONFIG_MODULE_RDLINE_CREATE_CONFIG=y
+CONFIG_MODULE_RDLINE_KILL_BUF=y
+CONFIG_MODULE_RDLINE_HISTORY=y
+CONFIG_MODULE_PARSE=y
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+CONFIG_MODULE_AX12=y
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+CONFIG_MODULE_ENCODERS_SPI=y
+CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y
+
+#
+# Robot specific modules
+#
+CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+CONFIG_MODULE_POSITION_MANAGER=y
+CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
+CONFIG_MODULE_TRAJECTORY_MANAGER=y
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y
+
+#
+# Control system modules
+#
+CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+CONFIG_MODULE_PID=y
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+CONFIG_MODULE_QUADRAMP=y
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+CONFIG_MODULE_DIAGNOSTIC=y
+CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
+CONFIG_MODULE_ERROR=y
+CONFIG_MODULE_ERROR_CREATE_CONFIG=y
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile
new file mode 100755 (executable)
index 0000000..5867b4e
--- /dev/null
@@ -0,0 +1,30 @@
+TARGET = main
+
+# repertoire des modules
+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
+
+ASRC = 
+
+CFLAGS += -Wall -Werror
+#CFLAGS += -DHOMOLOGATION
+CFLAGS += -DTEST_BEACON
+LDFLAGS = -T ../common/avr6.x
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+AVRDUDE_DELAY=50
+
+program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep
+       echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+       $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+
diff --git a/projects/microb2010/mainboard/Makefile.~1.6.~ b/projects/microb2010/mainboard/Makefile.~1.6.~
new file mode 100755 (executable)
index 0000000..1a53746
--- /dev/null
@@ -0,0 +1,29 @@
+TARGET = main
+
+# repertoire des modules
+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
+
+ASRC = 
+
+CFLAGS += -Wall -Werror
+#CFLAGS += -DHOMOLOGATION
+LDFLAGS = -T ../common/avr6.x
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+AVRDUDE_DELAY=50
+
+program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep
+       echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+       $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+
diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c
new file mode 100644 (file)
index 0000000..19d2fc7
--- /dev/null
@@ -0,0 +1,77 @@
+/*  
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: actuator.c,v 1.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+
+#include "main.h"
+
+void pwm_set_and_save(void *pwm, int32_t val)
+{
+       /* we need to do the saturation here, before saving the
+        * value */
+       if (val > 4095)
+               val = 4095;
+       if (val < -4095)
+               val = -4095;
+       
+       if (pwm == LEFT_PWM)
+               mainboard.pwm_l = val;
+       else if (pwm == RIGHT_PWM)
+               mainboard.pwm_r = val;
+       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;
+}
+
diff --git a/projects/microb2010/mainboard/actuator.h b/projects/microb2010/mainboard/actuator.h
new file mode 100644 (file)
index 0000000..9c7174a
--- /dev/null
@@ -0,0 +1,25 @@
+/*  
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: actuator.h,v 1.2 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+void pwm_set_and_save(void *pwm, int32_t val);
+void pickup_wheels_on(void);
+void pickup_wheels_off(void);
+
diff --git a/projects/microb2010/mainboard/adc_config.h b/projects/microb2010/mainboard/adc_config.h
new file mode 100644 (file)
index 0000000..e69de29
diff --git a/projects/microb2010/mainboard/ax12_config.h b/projects/microb2010/mainboard/ax12_config.h
new file mode 100755 (executable)
index 0000000..072e8fb
--- /dev/null
@@ -0,0 +1,7 @@
+#ifndef _AX12_CONFIG_H_
+#define _AX12_CONFIG_H_
+
+#define AX12_MAX_PARAMS 32
+
+
+#endif/*_AX12_CONFIG_H_*/
diff --git a/projects/microb2010/mainboard/ax12_user.c b/projects/microb2010/mainboard/ax12_user.c
new file mode 100644 (file)
index 0000000..575f5bc
--- /dev/null
@@ -0,0 +1,176 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: ax12_user.c,v 1.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/error.h>
+
+#include <i2c.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 <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+
+/*
+ * Cmdline interface for AX12. Use the PC to command a daisy-chain of
+ * AX12 actuators with a nice command line interface.
+ * 
+ * The circuit should be as following:
+ *
+ *    |----------|
+ *    |            uart3|------->--- PC (baudrate=57600)
+ *    |                 |-------<---
+ *    |        atmega128|
+ *    |                 |
+ *    |            uart0|---->---+-- AX12 (baudrate 115200)
+ *    |                 |----<---| 
+ *    |----------|
+ *
+ * Note that RX and TX pins of UART1 are connected together to provide
+ * a half-duplex UART emulation.
+ *
+ */
+
+#define UART_AX12_NUM 0
+#define UCSRxB UCSR0B
+#define AX12_TIMEOUT 5000UL /* in us */
+
+/********************************* AX12 commands */
+
+/*
+ * We use synchronous access (not interrupt driven) to the hardware
+ * UART, because we have to be sure that the transmission/reception is
+ * really finished when we return from the functions.
+ *
+ * We don't use the CM-5 circuit as described in the AX12
+ * documentation, we simply connect TX and RX and use TXEN + RXEN +
+ * DDR to manage the port directions.
+ */
+
+static volatile uint8_t ax12_state = AX12_STATE_READ;
+extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
+static volatile uint8_t ax12_nsent = 0;
+
+/* Called by ax12 module to send a character on serial line. Count the
+ * number of transmitted bytes. It will be used in ax12_recv_char() to
+ * drop the bytes that we transmitted. */
+static int8_t ax12_send_char(uint8_t c)
+{
+       uart_send(UART_AX12_NUM, c);
+       ax12_nsent++;
+       return 0;
+}
+
+/* for atmega256 */
+#ifndef TXEN
+#define TXEN TXEN0
+#endif
+
+/* called by uart module when the character has been written in
+ * UDR. It does not mean that the byte is physically transmitted. */
+static void ax12_send_callback(char c)
+{
+       if (ax12_state == AX12_STATE_READ) {
+               /* disable TX when last byte is pushed. */
+               if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+                       UCSRxB &= ~(1<<TXEN);
+       }
+}
+
+/* Called by ax12 module when we want to receive a char. Note that we
+ * also receive the bytes we sent ! So we need to drop them. */
+static int16_t ax12_recv_char(void)
+{
+       microseconds t = time_get_us2();
+       int c;
+       while (1) {
+               c = uart_recv_nowait(UART_AX12_NUM);
+               if (c != -1) {
+                       if (ax12_nsent == 0)
+                               return c;
+                       ax12_nsent --;
+               }
+
+               /* 5 ms timeout */
+               if ((time_get_us2() - t) > AX12_TIMEOUT)
+                       return -1;
+       }
+       return c;
+}
+
+/* called by ax12 module when we want to switch serial line. As we
+ * work in interruption mode, this function can be called to switch
+ * back in read mode even if the bytes are not really transmitted on
+ * the line. That's why in this case we do nothing, we will fall back
+ * in read mode in any case when xmit is finished -- see in
+ * ax12_send_callback() -- */
+static void ax12_switch_uart(uint8_t state)
+{
+       uint8_t flags;
+
+       if (state == AX12_STATE_WRITE) {
+               IRQ_LOCK(flags);
+               ax12_nsent=0;
+               while (uart_recv_nowait(UART_AX12_NUM) != -1);
+               UCSRxB |= (1<<TXEN);
+               ax12_state = AX12_STATE_WRITE;
+               IRQ_UNLOCK(flags);
+       }
+       else {
+               IRQ_LOCK(flags);
+               if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+                       UCSRxB &= ~(1<<TXEN);
+               ax12_state = AX12_STATE_READ;
+               IRQ_UNLOCK(flags);
+       }
+}
+
+
+void ax12_user_init(void)
+{
+       /* AX12 */
+       AX12_init(&gen.ax12);
+       AX12_set_hardware_send(&gen.ax12, ax12_send_char);
+       AX12_set_hardware_recv(&gen.ax12, ax12_recv_char);
+       AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart);
+       uart_register_tx_event(UART_AX12_NUM, ax12_send_callback);
+}
diff --git a/projects/microb2010/mainboard/ax12_user.h b/projects/microb2010/mainboard/ax12_user.h
new file mode 100644 (file)
index 0000000..4091709
--- /dev/null
@@ -0,0 +1,36 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: ax12_user.h,v 1.2 2009-04-07 20:03:48 zer0 Exp $
+ *
+ */
+
+/* This is the ax12 user interface. It initializes the aversive AX12
+ * module so that it works in background, using interrupt driver uart.
+ *
+ * Be carreful, a call to AX12 module is synchronous and uses
+ * interruptions, so interrupts must be enabled. On the other side, a
+ * call _must not_ interrupt another one. That's why all calls to the
+ * module are done either in init() functions or in a scheduler event
+ * with prio=ARM_PRIO.
+ */
+
+/* XXX do a safe_ax12() function that will retry once or twice if we
+ * see some problems. */
+
+void ax12_user_init(void);
diff --git a/projects/microb2010/mainboard/beacon_test.png b/projects/microb2010/mainboard/beacon_test.png
new file mode 100644 (file)
index 0000000..c669f2e
Binary files /dev/null and b/projects/microb2010/mainboard/beacon_test.png differ
diff --git a/projects/microb2010/mainboard/cmdline.c b/projects/microb2010/mainboard/cmdline.c
new file mode 100644 (file)
index 0000000..7c3ec9e
--- /dev/null
@@ -0,0 +1,172 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: cmdline.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "strat_base.h"
+
+
+/******** See in commands.c for the list of commands. */
+extern parse_pgm_ctx_t main_ctx[];
+
+static void write_char(char c) 
+{
+       uart_send(CMDLINE_UART, c);
+}
+
+static void 
+valid_buffer(const char *buf, uint8_t size) 
+{
+       int8_t ret;
+
+       /* reset CTRL-C for trajectory interruption each time we
+        * receive a new command */
+       interrupt_traj_reset();
+
+       ret = parse(main_ctx, buf);
+       if (ret == PARSE_AMBIGUOUS)
+               printf_P(PSTR("Ambiguous command\r\n"));
+       else if (ret == PARSE_NOMATCH)
+               printf_P(PSTR("Command not found\r\n"));
+       else if (ret == PARSE_BAD_ARGS)
+               printf_P(PSTR("Bad arguments\r\n"));
+}
+
+static int8_t 
+complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
+               int16_t *state)
+{
+       return complete(main_ctx, buf, state, dstbuf, dstsize);
+}
+
+
+/* sending "pop" on cmdline uart resets the robot */
+void emergency(char c)
+{
+       static uint8_t i = 0;
+       
+       /* interrupt traj here */
+       if (c == '\003')
+               interrupt_traj();
+       
+       if ((i == 0 && c == 'p') ||
+           (i == 1 && c == 'o') ||
+           (i == 2 && c == 'p')) 
+               i++;
+       else if ( !(i == 1 && c == 'p') )
+               i = 0;
+       if (i == 3)
+               reset();
+}
+
+/* log function, add a command to configure
+ * it dynamically */
+void mylog(struct error * e, ...) 
+{
+       va_list ap;
+       u16 stream_flags = stdout->flags;
+       uint8_t i;
+       time_h tv;
+
+       if (e->severity > ERROR_SEVERITY_ERROR) {
+               if (gen.log_level < e->severity)
+                       return;
+               
+               for (i=0; i<NB_LOGS+1; i++)
+                       if (gen.logs[i] == e->err_num)
+                               break;
+               if (i == NB_LOGS+1)
+                       return;
+       }
+
+       va_start(ap, e);
+       tv = time_get_time();
+       printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
+       
+       printf_P(PSTR("(%d,%d,%d) "),
+                position_get_x_s16(&mainboard.pos),
+                position_get_y_s16(&mainboard.pos),
+                position_get_a_deg_s16(&mainboard.pos));
+       
+       vfprintf_P(stdout, e->text, ap);
+       printf_P(PSTR("\r\n"));
+       va_end(ap);
+       stdout->flags = stream_flags;
+}
+
+int cmdline_interact(void)
+{
+       const char *history, *buffer;
+       int8_t ret, same = 0;
+       int16_t c;
+       
+       rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
+       snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > ");       
+       rdline_newline(&gen.rdl, gen.prompt);
+
+       while (1) {
+               c = uart_recv_nowait(CMDLINE_UART);
+               if (c == -1) 
+                       continue;
+               ret = rdline_char_in(&gen.rdl, c);
+               if (ret != 2 && ret != 0) {
+                       buffer = rdline_get_buffer(&gen.rdl);
+                       history = rdline_get_history_item(&gen.rdl, 0);
+                       if (history) {
+                               same = !memcmp(buffer, history, strlen(history)) &&
+                                       buffer[strlen(history)] == '\n';
+                       }
+                       else
+                               same = 0;
+                       if (strlen(buffer) > 1 && !same)
+                               rdline_add_history(&gen.rdl, buffer);
+                       rdline_newline(&gen.rdl, gen.prompt);
+               }
+       }
+
+       return 0;
+}
diff --git a/projects/microb2010/mainboard/cmdline.h b/projects/microb2010/mainboard/cmdline.h
new file mode 100644 (file)
index 0000000..f80b9b3
--- /dev/null
@@ -0,0 +1,44 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: cmdline.h,v 1.4 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#define CMDLINE_UART 1
+
+/* uart rx callback for reset() */
+void emergency(char c);
+
+/* log function */
+void mylog(struct error * e, ...);
+
+/* launch cmdline */
+int cmdline_interact(void);
+
+static inline uint8_t cmdline_keypressed(void) {
+       return (uart_recv_nowait(CMDLINE_UART) != -1);
+}
+
+static inline int16_t cmdline_getchar(void) {
+       return uart_recv_nowait(CMDLINE_UART);
+}
+
+static inline uint8_t cmdline_getchar_wait(void) {
+       return uart_recv(CMDLINE_UART);
+}
diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
new file mode 100644 (file)
index 0000000..107606e
--- /dev/null
@@ -0,0 +1,219 @@
+/*
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: commands.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org> 
+ */
+
+#include <stdlib.h>
+#include <aversive/pgmspace.h>
+#include <parse.h>
+
+/* commands_gen.c */
+extern parse_pgm_inst_t cmd_reset;
+extern parse_pgm_inst_t cmd_bootloader;
+extern parse_pgm_inst_t cmd_encoders;
+extern parse_pgm_inst_t cmd_pwm;
+extern parse_pgm_inst_t cmd_adc;
+extern parse_pgm_inst_t cmd_sensor;
+extern parse_pgm_inst_t cmd_log;
+extern parse_pgm_inst_t cmd_log_show;
+extern parse_pgm_inst_t cmd_log_type;
+extern parse_pgm_inst_t cmd_stack_space;
+extern parse_pgm_inst_t cmd_scheduler;
+
+/* commands_ax12.c */
+extern parse_pgm_inst_t cmd_baudrate;
+extern parse_pgm_inst_t cmd_uint16_read;
+extern parse_pgm_inst_t cmd_uint16_write;
+extern parse_pgm_inst_t cmd_uint8_read;
+extern parse_pgm_inst_t cmd_uint8_write;
+
+/* commands_cs.c */
+extern parse_pgm_inst_t cmd_gain;
+extern parse_pgm_inst_t cmd_gain_show;
+extern parse_pgm_inst_t cmd_speed;
+extern parse_pgm_inst_t cmd_speed_show;
+extern parse_pgm_inst_t cmd_derivate_filter;
+extern parse_pgm_inst_t cmd_derivate_filter_show;
+extern parse_pgm_inst_t cmd_consign;
+extern parse_pgm_inst_t cmd_maximum;
+extern parse_pgm_inst_t cmd_maximum_show;
+extern parse_pgm_inst_t cmd_quadramp;
+extern parse_pgm_inst_t cmd_quadramp_show;
+extern parse_pgm_inst_t cmd_cs_status;
+extern parse_pgm_inst_t cmd_blocking_i;
+extern parse_pgm_inst_t cmd_blocking_i_show;
+
+/* commands_mainboard.c */
+extern parse_pgm_inst_t cmd_event;
+extern parse_pgm_inst_t cmd_spi_test;
+extern parse_pgm_inst_t cmd_opponent;
+extern parse_pgm_inst_t cmd_opponent_set;
+extern parse_pgm_inst_t cmd_start;
+extern parse_pgm_inst_t cmd_interact;
+extern parse_pgm_inst_t cmd_color;
+extern parse_pgm_inst_t cmd_rs;
+extern parse_pgm_inst_t cmd_i2cdebug;
+extern parse_pgm_inst_t cmd_mechboard_show;
+extern parse_pgm_inst_t cmd_mechboard_setmode1;
+extern parse_pgm_inst_t cmd_mechboard_setmode2;
+extern parse_pgm_inst_t cmd_mechboard_setmode3;
+extern parse_pgm_inst_t cmd_mechboard_setmode4;
+extern parse_pgm_inst_t cmd_mechboard_setmode5;
+extern parse_pgm_inst_t cmd_pickup_wheels;
+extern parse_pgm_inst_t cmd_beacon_start;
+extern parse_pgm_inst_t cmd_pump_current;
+extern parse_pgm_inst_t cmd_build_test;
+extern parse_pgm_inst_t cmd_column_test;
+extern parse_pgm_inst_t cmd_column_test2;
+extern parse_pgm_inst_t cmd_lintel_test;
+extern parse_pgm_inst_t cmd_pickup_test;
+extern parse_pgm_inst_t cmd_scan_test;
+extern parse_pgm_inst_t cmd_scan_test2;
+extern parse_pgm_inst_t cmd_time_monitor;
+extern parse_pgm_inst_t cmd_scanner;
+extern parse_pgm_inst_t cmd_build_z1;
+#ifdef TEST_BEACON
+extern parse_pgm_inst_t cmd_beacon_opp_dump;
+#endif
+extern parse_pgm_inst_t cmd_test;
+
+/* commands_traj.c */
+extern parse_pgm_inst_t cmd_traj_speed;
+extern parse_pgm_inst_t cmd_traj_speed_show;
+extern parse_pgm_inst_t cmd_trajectory;
+extern parse_pgm_inst_t cmd_trajectory_show;
+extern parse_pgm_inst_t cmd_rs_gains;
+extern parse_pgm_inst_t cmd_rs_gains_show;
+extern parse_pgm_inst_t cmd_track;
+extern parse_pgm_inst_t cmd_track_show;
+extern parse_pgm_inst_t cmd_pt_list;
+extern parse_pgm_inst_t cmd_pt_list_append;
+extern parse_pgm_inst_t cmd_pt_list_del;
+extern parse_pgm_inst_t cmd_pt_list_show;
+extern parse_pgm_inst_t cmd_goto1;
+extern parse_pgm_inst_t cmd_goto2;
+extern parse_pgm_inst_t cmd_goto3;
+extern parse_pgm_inst_t cmd_position;
+extern parse_pgm_inst_t cmd_position_set;
+extern parse_pgm_inst_t cmd_strat_infos;
+extern parse_pgm_inst_t cmd_strat_conf;
+extern parse_pgm_inst_t cmd_strat_conf2;
+extern parse_pgm_inst_t cmd_strat_conf3;
+extern parse_pgm_inst_t cmd_strat_conf4;
+extern parse_pgm_inst_t cmd_subtraj;
+
+/* in progmem */
+parse_pgm_ctx_t main_ctx[] = {
+
+       /* commands_gen.c */
+       (parse_pgm_inst_t *)&cmd_reset,
+       (parse_pgm_inst_t *)&cmd_bootloader,
+       (parse_pgm_inst_t *)&cmd_encoders,
+       (parse_pgm_inst_t *)&cmd_pwm,
+       (parse_pgm_inst_t *)&cmd_adc,
+       (parse_pgm_inst_t *)&cmd_sensor,
+       (parse_pgm_inst_t *)&cmd_log,
+       (parse_pgm_inst_t *)&cmd_log_show,
+       (parse_pgm_inst_t *)&cmd_log_type,
+       (parse_pgm_inst_t *)&cmd_stack_space,
+       (parse_pgm_inst_t *)&cmd_scheduler,
+
+       /* commands_ax12.c */
+       (parse_pgm_inst_t *)&cmd_baudrate,
+       (parse_pgm_inst_t *)&cmd_uint16_read,
+       (parse_pgm_inst_t *)&cmd_uint16_write,
+       (parse_pgm_inst_t *)&cmd_uint8_read,
+       (parse_pgm_inst_t *)&cmd_uint8_write,
+
+       /* commands_cs.c */
+       (parse_pgm_inst_t *)&cmd_gain,
+       (parse_pgm_inst_t *)&cmd_gain_show,
+       (parse_pgm_inst_t *)&cmd_speed,
+       (parse_pgm_inst_t *)&cmd_speed_show,
+       (parse_pgm_inst_t *)&cmd_consign,
+       (parse_pgm_inst_t *)&cmd_derivate_filter,
+       (parse_pgm_inst_t *)&cmd_derivate_filter_show,
+       (parse_pgm_inst_t *)&cmd_maximum,
+       (parse_pgm_inst_t *)&cmd_maximum_show,
+       (parse_pgm_inst_t *)&cmd_quadramp,
+       (parse_pgm_inst_t *)&cmd_quadramp_show,
+       (parse_pgm_inst_t *)&cmd_cs_status,
+       (parse_pgm_inst_t *)&cmd_blocking_i,
+       (parse_pgm_inst_t *)&cmd_blocking_i_show,
+
+       /* commands_mainboard.c */
+       (parse_pgm_inst_t *)&cmd_event,
+       (parse_pgm_inst_t *)&cmd_spi_test,
+       (parse_pgm_inst_t *)&cmd_opponent,
+       (parse_pgm_inst_t *)&cmd_opponent_set,
+       (parse_pgm_inst_t *)&cmd_start,
+       (parse_pgm_inst_t *)&cmd_interact,
+       (parse_pgm_inst_t *)&cmd_color,
+       (parse_pgm_inst_t *)&cmd_rs,
+       (parse_pgm_inst_t *)&cmd_i2cdebug,
+       (parse_pgm_inst_t *)&cmd_mechboard_show,
+       (parse_pgm_inst_t *)&cmd_mechboard_setmode1,
+       (parse_pgm_inst_t *)&cmd_mechboard_setmode2,
+       (parse_pgm_inst_t *)&cmd_mechboard_setmode3,
+       (parse_pgm_inst_t *)&cmd_mechboard_setmode4,
+       (parse_pgm_inst_t *)&cmd_mechboard_setmode5,
+       (parse_pgm_inst_t *)&cmd_pickup_wheels,
+       (parse_pgm_inst_t *)&cmd_beacon_start,
+       (parse_pgm_inst_t *)&cmd_pump_current,
+       (parse_pgm_inst_t *)&cmd_build_test,
+       (parse_pgm_inst_t *)&cmd_column_test,
+       (parse_pgm_inst_t *)&cmd_column_test2,
+       (parse_pgm_inst_t *)&cmd_lintel_test,
+       (parse_pgm_inst_t *)&cmd_pickup_test,
+       (parse_pgm_inst_t *)&cmd_scan_test,
+       (parse_pgm_inst_t *)&cmd_scan_test2,
+       (parse_pgm_inst_t *)&cmd_time_monitor,
+       (parse_pgm_inst_t *)&cmd_scanner,
+       (parse_pgm_inst_t *)&cmd_build_z1,
+#ifdef TEST_BEACON
+       (parse_pgm_inst_t *)&cmd_beacon_opp_dump,
+#endif
+       (parse_pgm_inst_t *)&cmd_test,
+
+       /* commands_traj.c */
+       (parse_pgm_inst_t *)&cmd_traj_speed,
+       (parse_pgm_inst_t *)&cmd_traj_speed_show,
+       (parse_pgm_inst_t *)&cmd_trajectory,
+       (parse_pgm_inst_t *)&cmd_trajectory_show,
+       (parse_pgm_inst_t *)&cmd_rs_gains,
+       (parse_pgm_inst_t *)&cmd_rs_gains_show,
+       (parse_pgm_inst_t *)&cmd_track,
+       (parse_pgm_inst_t *)&cmd_track_show,
+       (parse_pgm_inst_t *)&cmd_pt_list,
+       (parse_pgm_inst_t *)&cmd_pt_list_append,
+       (parse_pgm_inst_t *)&cmd_pt_list_del,
+       (parse_pgm_inst_t *)&cmd_pt_list_show,
+       (parse_pgm_inst_t *)&cmd_goto1,
+       (parse_pgm_inst_t *)&cmd_goto2,
+       (parse_pgm_inst_t *)&cmd_position,
+       (parse_pgm_inst_t *)&cmd_position_set,
+       (parse_pgm_inst_t *)&cmd_strat_infos,
+       (parse_pgm_inst_t *)&cmd_strat_conf,
+       (parse_pgm_inst_t *)&cmd_strat_conf2,
+       (parse_pgm_inst_t *)&cmd_strat_conf3,
+       (parse_pgm_inst_t *)&cmd_strat_conf4,
+       (parse_pgm_inst_t *)&cmd_subtraj,
+       NULL,
+};
diff --git a/projects/microb2010/mainboard/commands_ax12.c b/projects/microb2010/mainboard/commands_ax12.c
new file mode 100644 (file)
index 0000000..804e084
--- /dev/null
@@ -0,0 +1,375 @@
+/*
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: commands_ax12.c,v 1.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+
+uint8_t addr_from_string(const char *s)
+{
+       /* 16 bits */
+       if (!strcmp_P(s, PSTR("cw_angle_limit")))
+               return AA_CW_ANGLE_LIMIT_L;
+       if (!strcmp_P(s, PSTR("ccw_angle_limit")))
+               return AA_CCW_ANGLE_LIMIT_L;
+       if (!strcmp_P(s, PSTR("max_torque")))
+               return AA_MAX_TORQUE_L;
+       if (!strcmp_P(s, PSTR("down_calibration")))
+               return AA_DOWN_CALIBRATION_L;
+       if (!strcmp_P(s, PSTR("up_calibration")))
+               return AA_UP_CALIBRATION_L;
+       if (!strcmp_P(s, PSTR("torque_limit")))
+               return AA_TORQUE_LIMIT_L;
+       if (!strcmp_P(s, PSTR("position")))
+               return AA_PRESENT_POSITION_L;
+       if (!strcmp_P(s, PSTR("speed")))
+               return AA_PRESENT_SPEED_L;
+       if (!strcmp_P(s, PSTR("load")))
+               return AA_PRESENT_LOAD_L;
+       if (!strcmp_P(s, PSTR("moving_speed")))
+               return AA_MOVING_SPEED_L;
+       if (!strcmp_P(s, PSTR("model")))
+               return AA_MODEL_NUMBER_L;
+       if (!strcmp_P(s, PSTR("goal_pos")))
+               return AA_GOAL_POSITION_L;
+       if (!strcmp_P(s, PSTR("punch")))
+               return AA_PUNCH_L;
+
+       /* 8 bits */
+       if (!strcmp_P(s, PSTR("firmware")))
+               return AA_FIRMWARE;
+       if (!strcmp_P(s, PSTR("id")))
+               return AA_ID;
+       if (!strcmp_P(s, PSTR("baudrate")))
+               return AA_BAUD_RATE;
+       if (!strcmp_P(s, PSTR("delay")))
+               return AA_DELAY_TIME;
+       if (!strcmp_P(s, PSTR("high_lim_temp")))
+               return AA_HIGHEST_LIMIT_TEMP;
+       if (!strcmp_P(s, PSTR("low_lim_volt")))
+               return AA_LOWEST_LIMIT_VOLTAGE;
+       if (!strcmp_P(s, PSTR("high_lim_volt")))
+               return AA_HIGHEST_LIMIT_VOLTAGE;
+       if (!strcmp_P(s, PSTR("status_return")))
+               return AA_STATUS_RETURN_LEVEL;
+       if (!strcmp_P(s, PSTR("alarm_led")))
+               return AA_ALARM_LED;
+       if (!strcmp_P(s, PSTR("alarm_shutdown")))
+               return AA_ALARM_SHUTDOWN;
+       if (!strcmp_P(s, PSTR("torque_enable")))
+               return AA_TORQUE_ENABLE;
+       if (!strcmp_P(s, PSTR("led")))
+               return AA_LED;
+       if (!strcmp_P(s, PSTR("cw_comp_margin")))
+               return AA_CW_COMPLIANCE_MARGIN;
+       if (!strcmp_P(s, PSTR("ccw_comp_margin")))
+               return AA_CCW_COMPLIANCE_MARGIN;
+       if (!strcmp_P(s, PSTR("cw_comp_slope")))
+               return AA_CW_COMPLIANCE_SLOPE;
+       if (!strcmp_P(s, PSTR("ccw_comp_slope")))
+               return AA_CCW_COMPLIANCE_SLOPE;
+       if (!strcmp_P(s, PSTR("voltage")))
+               return AA_PRESENT_VOLTAGE;
+       if (!strcmp_P(s, PSTR("temp")))
+               return AA_PRESENT_TEMP;
+       if (!strcmp_P(s, PSTR("reginst")))
+               return AA_PRESENT_REGINST;
+       if (!strcmp_P(s, PSTR("moving")))
+               return AA_MOVING;
+       if (!strcmp_P(s, PSTR("lock")))
+               return AA_LOCK;
+       
+       return 0;
+}
+
+/**********************************************************/
+/* Ax12_Stress */
+
+/* this structure is filled when cmd_ax12_stress is parsed successfully */
+struct cmd_ax12_stress_result {
+       fixed_string_t arg0;
+       uint8_t id;
+};
+
+/* function called when cmd_ax12_stress is parsed successfully */
+static void cmd_ax12_stress_parsed(void *parsed_result, void *data)
+{
+       struct cmd_ax12_stress_result *res = parsed_result;
+       int i, nb_errs = 0;
+       uint8_t val;
+       microseconds t = time_get_us2();
+
+       for (i=0; i<1000; i++) {
+               if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
+                       nb_errs ++;
+       }
+
+       printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
+       t = (time_get_us2() - t) / 1000;
+       printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
+}
+
+prog_char str_ax12_stress_arg0[] = "ax12_stress";
+parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
+parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
+
+prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands";
+parse_pgm_inst_t cmd_ax12_stress = {
+       .f = cmd_ax12_stress_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_ax12_stress,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_ax12_stress_arg0, 
+               (prog_void *)&cmd_ax12_stress_id, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_baudrate is parsed successfully */
+struct cmd_baudrate_result {
+       fixed_string_t arg0;
+       uint32_t arg1;
+};
+
+/* function called when cmd_baudrate is parsed successfully */
+static void cmd_baudrate_parsed(void * parsed_result, void * data)
+{
+       struct cmd_baudrate_result *res = parsed_result;
+       struct uart_config c;
+
+       printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+       uart_getconf(1, &c);
+       c.baudrate = res->arg1;
+       uart_setconf(1, &c);
+       printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+}
+
+prog_char str_baudrate_arg0[] = "baudrate";
+parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
+parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
+
+prog_char help_baudrate[] = "Change ax12 baudrate";
+parse_pgm_inst_t cmd_baudrate = {
+       .f = cmd_baudrate_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_baudrate,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_baudrate_arg0, 
+               (prog_void *)&cmd_baudrate_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Uint16 */
+
+
+/* this structure is filled when cmd_uint16_read is parsed successfully */
+struct cmd_uint16_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint8_t num;
+       uint16_t val;
+};
+
+/* function called when cmd_uint16_read is parsed successfully */
+static void cmd_uint16_read_parsed(void * parsed_result, void * data)
+{
+       struct cmd_uint16_result *res = parsed_result;
+       uint8_t ret;
+       uint16_t val;
+       uint8_t addr = addr_from_string(res->arg1);
+       ret = AX12_read_int(&gen.ax12, res->num, addr, &val);
+       if (ret)
+               printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+       printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint16_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
+prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
+               "max_torque#down_calibration#up_calibration#torque_limit#"
+               "position#speed#load#punch";
+parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
+parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
+
+prog_char help_uint16_read[] = "Read uint16 value (type, num)";
+parse_pgm_inst_t cmd_uint16_read = {
+       .f = cmd_uint16_read_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_uint16_read,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_uint16_arg0,
+               (prog_void *)&cmd_uint16_arg1,
+               (prog_void *)&cmd_uint16_num,
+               NULL,
+       },
+};
+
+/* function called when cmd_uint16_write is parsed successfully */
+static void cmd_uint16_write_parsed(void * parsed_result, void * data)
+{
+       struct cmd_uint16_result *res = parsed_result;
+       uint8_t ret;
+       uint8_t addr = addr_from_string(res->arg1);
+       printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
+                res->val, res->val);
+       ret = AX12_write_int(&gen.ax12, res->num, addr, res->val);
+       if (ret)
+               printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint16_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
+prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
+               "max_torque#torque_limit#punch";
+parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
+parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
+
+prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
+parse_pgm_inst_t cmd_uint16_write = {
+       .f = cmd_uint16_write_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_uint16_write,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_uint16_arg0_w,
+               (prog_void *)&cmd_uint16_arg1_w,
+               (prog_void *)&cmd_uint16_num,
+               (prog_void *)&cmd_uint16_val,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Uint8 */
+
+
+/* this structure is filled when cmd_uint8_read is parsed successfully */
+struct cmd_uint8_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint8_t num;
+       uint8_t val;
+};
+
+/* function called when cmd_uint8_read is parsed successfully */
+static void cmd_uint8_read_parsed(void * parsed_result, void * data)
+{
+       struct cmd_uint8_result *res = parsed_result;
+       uint8_t ret;
+       uint8_t val;
+       uint8_t addr = addr_from_string(res->arg1);
+
+       ret = AX12_read_byte(&gen.ax12, res->num, addr, &val);
+       if (ret)
+               printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+       printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint8_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
+prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
+               "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+               "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+               "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+               "voltage#temp#reginst#moving#lock";
+parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
+parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
+
+prog_char help_uint8_read[] = "Read uint8 value (type, num)";
+parse_pgm_inst_t cmd_uint8_read = {
+       .f = cmd_uint8_read_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_uint8_read,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_uint8_arg0,
+               (prog_void *)&cmd_uint8_arg1,
+               (prog_void *)&cmd_uint8_num,
+               NULL,
+       },
+};
+
+/* function called when cmd_uint8_write is parsed successfully */
+static void cmd_uint8_write_parsed(void * parsed_result, void * data)
+{
+       struct cmd_uint8_result *res = parsed_result;
+       uint8_t addr = addr_from_string(res->arg1);
+       uint8_t ret;
+       printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1, 
+                res->val, res->val);
+       ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val);
+       if (ret)
+               printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint8_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
+prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
+               "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+               "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+               "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+               "reginst#lock";
+parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
+parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
+
+prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
+parse_pgm_inst_t cmd_uint8_write = {
+       .f = cmd_uint8_write_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_uint8_write,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_uint8_arg0_w,
+               (prog_void *)&cmd_uint8_arg1_w,
+               (prog_void *)&cmd_uint8_num,
+               (prog_void *)&cmd_uint8_val,
+               NULL,
+       },
+};
diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c
new file mode 100644 (file)
index 0000000..a39ab07
--- /dev/null
@@ -0,0 +1,677 @@
+/*
+ *  Copyright Droids Corporation (2008)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cs.h"
+#include "cmdline.h"
+
+struct csb_list {
+       const prog_char *name;
+       struct cs_block *csb;
+};
+
+prog_char csb_angle_str[] = "angle";
+prog_char csb_distance_str[] = "distance";
+struct csb_list csb_list[] = {
+       { .name = csb_angle_str, .csb = &mainboard.angle },
+       { .name = csb_distance_str, .csb = &mainboard.distance },
+};
+
+struct cmd_cs_result {
+       fixed_string_t cmdname;
+       fixed_string_t csname;
+};
+
+/* token to be used for all cs-related commands */
+prog_char str_csb_name[] = "angle#distance";
+parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
+
+struct cs_block *cs_from_name(const char *name)
+{
+       int i;
+
+       for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
+               if (!strcmp_P(name, csb_list[i].name))
+                       return csb_list[i].csb;
+       }
+       return NULL;
+}
+               
+/**********************************************************/
+/* Gains for control system */
+
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_result {
+       struct cmd_cs_result cs;
+       int16_t p;
+       int16_t i;
+       int16_t d;
+};
+
+/* function called when cmd_gain is parsed successfully */
+static void cmd_gain_parsed(void * parsed_result, void *show)
+{
+       struct cmd_gain_result *res = parsed_result;
+       struct cs_block *csb;
+
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+
+       if (!show) 
+               pid_set_gains(&csb->pid, res->p, res->i, res->d);
+
+       printf_P(PSTR("%s %s %d %d %d\r\n"),
+                res->cs.cmdname,
+                res->cs.csname,
+                pid_get_gain_P(&csb->pid),
+                pid_get_gain_I(&csb->pid),
+                pid_get_gain_D(&csb->pid));
+}
+
+prog_char str_gain_arg0[] = "gain";
+parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
+parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
+parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
+parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
+
+prog_char help_gain[] = "Set gain values for PID";
+parse_pgm_inst_t cmd_gain = {
+       .f = cmd_gain_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_gain,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_gain_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_gain_p, 
+               (prog_void *)&cmd_gain_i, 
+               (prog_void *)&cmd_gain_d, 
+               NULL,
+       },
+};
+
+/* show */
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_show_result {
+       struct cmd_cs_result cs;
+       fixed_string_t show;
+};
+
+prog_char str_gain_show_arg[] = "show";
+parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
+
+prog_char help_gain_show[] = "Show gain values for PID";
+parse_pgm_inst_t cmd_gain_show = {
+       .f = cmd_gain_parsed,  /* function to call */
+       .data = (void *)1,      /* 2nd arg of func */
+       .help_str = help_gain_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_gain_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_gain_show_arg,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Speeds for control system */
+
+/* this structure is filled when cmd_speed is parsed successfully */
+struct cmd_speed_result {
+       struct cmd_cs_result cs;
+       uint16_t s;
+};
+
+/* function called when cmd_speed is parsed successfully */
+static void cmd_speed_parsed(void *parsed_result, void *show)
+{
+       struct cmd_speed_result * res = parsed_result;
+       
+       struct cs_block *csb;
+
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+
+#if notyet
+       if (!show) 
+               ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
+
+       printf_P(PSTR("%s %lu\r\n"), 
+                res->cs.csname,
+                ext.r_b.var_pos);
+#else
+       printf_P(PSTR("TODO\r\n"));
+#endif
+}
+
+prog_char str_speed_arg0[] = "speed";
+parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
+parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
+
+prog_char help_speed[] = "Set speed values for ramp filter";
+parse_pgm_inst_t cmd_speed = {
+       .f = cmd_speed_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_speed,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_speed_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_speed_s, 
+               NULL,
+       },
+};
+
+/* show */
+struct cmd_speed_show_result {
+       struct cmd_cs_result cs;
+       fixed_string_t show;
+};
+
+prog_char str_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
+
+prog_char help_speed_show[] = "Show speed values for ramp filter";
+parse_pgm_inst_t cmd_speed_show = {
+       .f = cmd_speed_parsed,  /* function to call */
+       .data = (void *)1,      /* 2nd arg of func */
+       .help_str = help_speed_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_speed_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_speed_show_arg,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Derivate_Filters for control system */
+
+/* this structure is filled when cmd_derivate_filter is parsed successfully */
+struct cmd_derivate_filter_result {
+       struct cmd_cs_result cs;
+       uint8_t size;
+};
+
+/* function called when cmd_derivate_filter is parsed successfully */
+static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
+{
+       struct cmd_derivate_filter_result * res = parsed_result;
+       struct cs_block *csb;
+
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+
+       if (!show) 
+               pid_set_derivate_filter(&csb->pid, res->size);
+
+       printf_P(PSTR("%s %s %u\r\n"), 
+                res->cs.cmdname,
+                res->cs.csname,
+                pid_get_derivate_filter(&csb->pid));
+}
+
+prog_char str_derivate_filter_arg0[] = "derivate_filter";
+parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
+parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
+
+prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
+parse_pgm_inst_t cmd_derivate_filter = {
+       .f = cmd_derivate_filter_parsed,  /* function to call */
+       .data = (void *)1,      /* 2nd arg of func */
+       .help_str = help_derivate_filter,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_derivate_filter_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_derivate_filter_size, 
+               NULL,
+       },
+};
+
+/* show */
+
+struct cmd_derivate_filter_show_result {
+       struct cmd_cs_result cs;
+       fixed_string_t show;
+};
+
+prog_char str_derivate_filter_show_arg[] = "show";
+parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
+
+prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
+parse_pgm_inst_t cmd_derivate_filter_show = {
+       .f = cmd_derivate_filter_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_derivate_filter_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_derivate_filter_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_derivate_filter_show_arg,
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Consign for control system */
+
+/* this structure is filled when cmd_consign is parsed successfully */
+struct cmd_consign_result {
+       struct cmd_cs_result cs;
+       int32_t p;
+};
+
+/* function called when cmd_consign is parsed successfully */
+static void cmd_consign_parsed(void * parsed_result, void *data)
+{
+       struct cmd_consign_result * res = parsed_result;
+       struct cs_block *csb;
+
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+
+       cs_set_consign(&csb->cs, res->p);
+}
+
+prog_char str_consign_arg0[] = "consign";
+parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
+parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
+
+prog_char help_consign[] = "Set consign value";
+parse_pgm_inst_t cmd_consign = {
+       .f = cmd_consign_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_consign,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_consign_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_consign_p, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Maximums for control system */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_result {
+       struct cmd_cs_result cs;
+       uint32_t in;
+       uint32_t i;
+       uint32_t out;
+};
+
+/* function called when cmd_maximum is parsed successfully */
+static void cmd_maximum_parsed(void *parsed_result, void *show)
+{
+       struct cmd_maximum_result * res = parsed_result;
+       
+       struct cs_block *csb;
+
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+
+       if (!show)
+               pid_set_maximums(&csb->pid, res->in, res->i, res->out);
+
+       printf_P(PSTR("maximum %s %lu %lu %lu\r\n"), 
+                res->cs.csname,
+                pid_get_max_in(&csb->pid),
+                pid_get_max_I(&csb->pid),
+                pid_get_max_out(&csb->pid));
+}
+
+prog_char str_maximum_arg0[] = "maximum";
+parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
+parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
+parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
+parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
+
+prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
+parse_pgm_inst_t cmd_maximum = {
+       .f = cmd_maximum_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_maximum,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_maximum_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_maximum_in, 
+               (prog_void *)&cmd_maximum_i, 
+               (prog_void *)&cmd_maximum_out, 
+               NULL,
+       },
+};
+
+/* show */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_show_result {
+       struct cmd_cs_result cs;
+       fixed_string_t show;
+};
+prog_char str_maximum_show_arg[] = "show";
+parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
+
+prog_char help_maximum_show[] = "Show maximum values for PID";
+parse_pgm_inst_t cmd_maximum_show = {
+       .f = cmd_maximum_parsed,  /* function to call */
+       .data = (void *)1,      /* 2nd arg of func */
+       .help_str = help_maximum_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_maximum_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_maximum_show_arg,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Quadramp for control system */
+
+/* this structure is filled when cmd_quadramp is parsed successfully */
+struct cmd_quadramp_result {
+       struct cmd_cs_result cs;
+       uint32_t ap;
+       uint32_t an;
+       uint32_t sp;
+       uint32_t sn;
+};
+
+/* function called when cmd_quadramp is parsed successfully */
+static void cmd_quadramp_parsed(void *parsed_result, void *show)
+{
+       struct cmd_quadramp_result * res = parsed_result;
+       
+       struct cs_block *csb;
+
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+
+       if (!show)  {
+               quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
+               quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
+       }
+
+       printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), 
+                res->cs.csname,
+                csb->qr.var_2nd_ord_pos,
+                csb->qr.var_2nd_ord_neg,
+                csb->qr.var_1st_ord_pos,
+                csb->qr.var_1st_ord_neg);
+}
+
+prog_char str_quadramp_arg0[] = "quadramp";
+parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+
+prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
+parse_pgm_inst_t cmd_quadramp = {
+       .f = cmd_quadramp_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_quadramp,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_quadramp_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_quadramp_ap, 
+               (prog_void *)&cmd_quadramp_an, 
+               (prog_void *)&cmd_quadramp_sp, 
+               (prog_void *)&cmd_quadramp_sn, 
+               
+               NULL,
+       },
+};
+
+/* show */
+
+struct cmd_quadramp_show_result {
+       struct cmd_cs_result cs;
+       fixed_string_t show;
+};
+
+prog_char str_quadramp_show_arg[] = "show";
+parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
+
+prog_char help_quadramp_show[] = "Get quadramp values for control system";
+parse_pgm_inst_t cmd_quadramp_show = {
+       .f = cmd_quadramp_parsed,  /* function to call */
+       .data = (void *)1,      /* 2nd arg of func */
+       .help_str = help_quadramp_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_quadramp_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_quadramp_show_arg, 
+               NULL,
+       },
+};
+
+
+
+/**********************************************************/
+/* cs_status show for control system */
+
+/* this structure is filled when cmd_cs_status is parsed successfully */
+struct cmd_cs_status_result {
+       struct cmd_cs_result cs;
+       fixed_string_t arg;
+};
+
+/* function called when cmd_cs_status is parsed successfully */
+static void cmd_cs_status_parsed(void *parsed_result, void *data)
+{
+       struct cmd_cs_status_result *res = parsed_result;
+       struct cs_block *csb;
+       uint8_t loop = 0;
+       uint8_t print_pid = 0, print_cs = 0;
+       
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+       if (strcmp_P(res->arg, PSTR("on")) == 0) {
+               csb->on = 1;
+               printf_P(PSTR("%s is on\r\n"), res->cs.csname);
+               return;
+       }
+       else if (strcmp_P(res->arg, PSTR("off")) == 0) {
+               csb->on = 0;
+               printf_P(PSTR("%s is off\r\n"), res->cs.csname);
+               return;
+       }
+       else if (strcmp_P(res->arg, PSTR("show")) == 0) {
+               print_cs = 1;
+       }
+       else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
+               loop = 1;
+               print_cs = 1;
+       }
+       else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
+               print_pid = 1;
+       }
+       else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
+               print_pid = 1;
+               loop = 1;
+       }
+
+       printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
+       do {
+               if (print_cs)
+                       dump_cs(res->cs.csname, &csb->cs);
+               if (print_pid)
+                       dump_pid(res->cs.csname, &csb->pid);
+               wait_ms(100);
+       } while(loop && !cmdline_keypressed());
+}
+
+prog_char str_cs_status_arg0[] = "cs_status";
+parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
+prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
+parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
+
+prog_char help_cs_status[] = "Show cs status";
+parse_pgm_inst_t cmd_cs_status = {
+       .f = cmd_cs_status_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_cs_status,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_cs_status_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_cs_status_arg, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Blocking_I for control system */
+
+/* this structure is filled when cmd_blocking_i is parsed successfully */
+struct cmd_blocking_i_result {
+       struct cmd_cs_result cs;
+       int32_t k1;
+       int32_t k2;
+       uint32_t i;
+       uint16_t cpt;
+};
+
+/* function called when cmd_blocking_i is parsed successfully */
+static void cmd_blocking_i_parsed(void *parsed_result, void *show)
+{
+       struct cmd_blocking_i_result * res = parsed_result;
+       
+       struct cs_block *csb;
+
+       csb = cs_from_name(res->cs.csname);
+       if (csb == NULL) {
+               printf_P(PSTR("null csb\r\n"));
+               return;
+       }
+
+       if (!show)
+               bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
+                                         res->i, res->cpt);
+
+       printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"), 
+                res->cs.cmdname,
+                res->cs.csname,
+                csb->bd.k1,
+                csb->bd.k2,
+                csb->bd.i_thres,
+                csb->bd.cpt_thres);
+}
+
+prog_char str_blocking_i_arg0[] = "blocking";
+parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
+parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
+parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
+parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
+parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
+
+prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
+parse_pgm_inst_t cmd_blocking_i = {
+       .f = cmd_blocking_i_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_blocking_i,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_blocking_i_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_blocking_i_k1, 
+               (prog_void *)&cmd_blocking_i_k2, 
+               (prog_void *)&cmd_blocking_i_i, 
+               (prog_void *)&cmd_blocking_i_cpt,
+               NULL,
+       },
+};
+
+/* show */
+
+struct cmd_blocking_i_show_result {
+       struct cmd_cs_result cs;
+       fixed_string_t show;
+};
+
+prog_char str_blocking_i_show_arg[] = "show";
+parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg);
+
+prog_char help_blocking_i_show[] = "Show blocking detection values";
+parse_pgm_inst_t cmd_blocking_i_show = {
+       .f = cmd_blocking_i_parsed,  /* function to call */
+       .data = (void *)1,      /* 2nd arg of func */
+       .help_str = help_blocking_i_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_blocking_i_arg0, 
+               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_blocking_i_show_arg, 
+               NULL,
+       },
+};
+
+
diff --git a/projects/microb2010/mainboard/commands_gen.c b/projects/microb2010/mainboard/commands_gen.c
new file mode 100644 (file)
index 0000000..b20dcc8
--- /dev/null
@@ -0,0 +1,586 @@
+/*
+ *  Copyright Droids Corporation (2008)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: commands_gen.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <encoders_spi.h>
+#include <adc.h>
+
+#include <scheduler.h>
+#include <scheduler_stats.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include <diagnostic.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "sensor.h"
+
+/**********************************************************/
+/* Reset */
+
+/* this structure is filled when cmd_reset is parsed successfully */
+struct cmd_reset_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_reset is parsed successfully */
+static void cmd_reset_parsed(void * parsed_result, void * data)
+{
+       reset();
+}
+
+prog_char str_reset_arg0[] = "reset";
+parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
+
+prog_char help_reset[] = "Reset the board";
+parse_pgm_inst_t cmd_reset = {
+       .f = cmd_reset_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_reset,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_reset_arg0, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Bootloader */
+
+/* this structure is filled when cmd_bootloader is parsed successfully */
+struct cmd_bootloader_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_bootloader is parsed successfully */
+static void cmd_bootloader_parsed(void *parsed_result, void *data)
+{
+       bootloader();
+}
+
+prog_char str_bootloader_arg0[] = "bootloader";
+parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
+
+prog_char help_bootloader[] = "Launch the bootloader";
+parse_pgm_inst_t cmd_bootloader = {
+       .f = cmd_bootloader_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_bootloader,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_bootloader_arg0, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Encoders tests */
+
+/* this structure is filled when cmd_encoders is parsed successfully */
+struct cmd_encoders_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_encoders is parsed successfully */
+static void cmd_encoders_parsed(void *parsed_result, void *data)
+{
+       struct cmd_encoders_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("reset"))) {
+               encoders_spi_set_value((void *)0, 0);
+               encoders_spi_set_value((void *)1, 0);
+               encoders_spi_set_value((void *)2, 0);
+               encoders_spi_set_value((void *)3, 0);
+               return;
+       }
+
+       /* show */
+       while(!cmdline_keypressed()) {
+               printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"), 
+                        encoders_spi_get_value((void *)0),
+                        encoders_spi_get_value((void *)1),
+                        encoders_spi_get_value((void *)2),
+                        encoders_spi_get_value((void *)3));
+               wait_ms(100);
+       }
+}
+
+prog_char str_encoders_arg0[] = "encoders";
+parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
+prog_char str_encoders_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
+
+prog_char help_encoders[] = "Show encoders values";
+parse_pgm_inst_t cmd_encoders = {
+       .f = cmd_encoders_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_encoders,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_encoders_arg0, 
+               (prog_void *)&cmd_encoders_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Scheduler show */
+
+/* this structure is filled when cmd_scheduler is parsed successfully */
+struct cmd_scheduler_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_scheduler is parsed successfully */
+static void cmd_scheduler_parsed(void *parsed_result, void *data)
+{
+       scheduler_dump_events();
+       scheduler_stats_dump();
+}
+
+prog_char str_scheduler_arg0[] = "scheduler";
+parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
+prog_char str_scheduler_arg1[] = "show";
+parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
+
+prog_char help_scheduler[] = "Show scheduler events";
+parse_pgm_inst_t cmd_scheduler = {
+       .f = cmd_scheduler_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_scheduler,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_scheduler_arg0, 
+               (prog_void *)&cmd_scheduler_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Pwms tests */
+
+/* this structure is filled when cmd_pwm is parsed successfully */
+struct cmd_pwm_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int16_t arg2;
+};
+
+/* function called when cmd_pwm is parsed successfully */
+static void cmd_pwm_parsed(void * parsed_result, void * data)
+{
+       void * pwm_ptr = NULL;
+       struct cmd_pwm_result * res = parsed_result;
+       
+       if (!strcmp_P(res->arg1, PSTR("1(4A)")))
+               pwm_ptr = &gen.pwm1_4A;
+       else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
+               pwm_ptr = &gen.pwm2_4B;
+       else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
+               pwm_ptr = &gen.pwm3_1A;
+       else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
+               pwm_ptr = &gen.pwm4_1B;
+
+       else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
+               pwm_ptr = &gen.servo1;
+       else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
+               pwm_ptr = &gen.servo2;
+       else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
+               pwm_ptr = &gen.servo3;
+       else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
+               pwm_ptr = &gen.servo4;
+       
+       if (pwm_ptr)
+               pwm_ng_set(pwm_ptr, res->arg2);
+
+       printf_P(PSTR("done\r\n"));
+}
+
+prog_char str_pwm_arg0[] = "pwm";
+parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
+prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)";
+parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
+parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
+
+prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
+parse_pgm_inst_t cmd_pwm = {
+       .f = cmd_pwm_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_pwm,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_pwm_arg0, 
+               (prog_void *)&cmd_pwm_arg1, 
+               (prog_void *)&cmd_pwm_arg2, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Adcs tests */
+
+/* this structure is filled when cmd_adc is parsed successfully */
+struct cmd_adc_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_adc is parsed successfully */
+static void cmd_adc_parsed(void *parsed_result, void *data)
+{
+       struct cmd_adc_result *res = parsed_result;
+       uint8_t i, loop = 0;
+
+       if (!strcmp_P(res->arg1, PSTR("loop_show")))
+               loop = 1;
+       
+       do {
+               printf_P(PSTR("ADC values: "));
+               for (i=0; i<ADC_MAX; i++) {
+                       printf_P(PSTR("%.4d "), sensor_get_adc(i));
+               }
+               printf_P(PSTR("\r\n"));
+               wait_ms(100);
+       } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_adc_arg0[] = "adc";
+parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
+prog_char str_adc_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
+
+prog_char help_adc[] = "Show adc values";
+parse_pgm_inst_t cmd_adc = {
+       .f = cmd_adc_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_adc,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_adc_arg0, 
+               (prog_void *)&cmd_adc_arg1, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Sensors tests */
+
+/* this structure is filled when cmd_sensor is parsed successfully */
+struct cmd_sensor_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_sensor is parsed successfully */
+static void cmd_sensor_parsed(void *parsed_result, void *data)
+{
+       struct cmd_sensor_result *res = parsed_result;
+       uint8_t i, loop = 0;
+
+       if (!strcmp_P(res->arg1, PSTR("loop_show")))
+               loop = 1;
+       
+       do {
+               printf_P(PSTR("SENSOR values: "));
+               for (i=0; i<SENSOR_MAX; i++) {
+                       printf_P(PSTR("%d "), !!sensor_get(i));
+               }
+               printf_P(PSTR("\r\n"));
+               wait_ms(100);
+       } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_sensor_arg0[] = "sensor";
+parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
+prog_char str_sensor_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
+
+prog_char help_sensor[] = "Show sensor values";
+parse_pgm_inst_t cmd_sensor = {
+       .f = cmd_sensor_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_sensor,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_sensor_arg0, 
+               (prog_void *)&cmd_sensor_arg1, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint8_t arg2;
+       fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const prog_char uart_log[] = "uart";
+static const prog_char rs_log[] = "rs";
+static const prog_char traj_log[] = "traj";
+static const prog_char i2c_log[] = "i2c";
+static const prog_char oa_log[] = "oa";
+static const prog_char strat_log[] = "strat";
+static const prog_char i2cproto_log[] = "i2cproto";
+static const prog_char sensor_log[] = "sensor";
+static const prog_char block_log[] = "bd";
+static const prog_char cs_log[] = "cs";
+
+struct log_name_and_num {
+       const prog_char * name;
+       uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+       { uart_log, E_UART },
+       { rs_log, E_ROBOT_SYSTEM },
+       { traj_log, E_TRAJECTORY },
+       { i2c_log, E_I2C },
+       { oa_log, E_OA },
+       { strat_log, E_USER_STRAT },
+       { i2cproto_log, E_USER_I2C_PROTO },
+       { sensor_log, E_USER_SENSOR },
+       { block_log, E_BLOCKING_DETECTION_MANAGER },
+       { cs_log, E_USER_CS },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+       uint8_t i;
+       
+       for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+               if (!strcmp_P(s, log_name_and_num[i].name)) {
+                       return log_name_and_num[i].num;
+               }
+       }
+       return 0;
+}
+
+const prog_char *
+log_num2name(uint8_t num)
+{
+       uint8_t i;
+       
+       for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+               if (num ==  log_name_and_num[i].num) {
+                       return log_name_and_num[i].name;
+               }
+       }
+       return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+       uint8_t i, empty=1;
+       const prog_char * name;
+
+       printf_P(PSTR("log level is %d\r\n"), gen.log_level);
+       for (i=0; i<NB_LOGS; i++) {
+               name = log_num2name(gen.logs[i]);
+               if (name) {
+                       printf_P(PSTR("log type %S is on\r\n"), name);
+                       empty = 0;
+               }
+       }
+       if (empty)
+               printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, void * data)
+{
+       struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("level"))) {
+               gen.log_level = res->arg2;
+       }
+
+       /* else it is a show */
+       cmd_log_do_show();
+}
+
+prog_char str_log_arg0[] = "log";
+parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
+prog_char str_log_arg1[] = "level";
+parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
+parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
+
+prog_char help_log[] = "Set log options: level (0 -> 5)";
+parse_pgm_inst_t cmd_log = {
+       .f = cmd_log_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_log,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_log_arg0, 
+               (prog_void *)&cmd_log_arg1, 
+               (prog_void *)&cmd_log_arg2, 
+               NULL,
+       },
+};
+
+prog_char str_log_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
+
+prog_char help_log_show[] = "Show configured logs";
+parse_pgm_inst_t cmd_log_show = {
+       .f = cmd_log_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_log_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_log_arg0, 
+               (prog_void *)&cmd_log_arg1_show, 
+               NULL,
+       },
+};
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_type_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       fixed_string_t arg2;
+       fixed_string_t arg3;
+};
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_type_parsed(void * parsed_result, void * data)
+{
+       struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
+       uint8_t lognum;
+       uint8_t i;
+       
+       lognum = log_name2num(res->arg2);
+       if (lognum == 0) {
+               printf_P(PSTR("Cannot find log num\r\n"));
+               return;
+       }
+
+       if (!strcmp_P(res->arg3, PSTR("on"))) {
+               for (i=0; i<NB_LOGS; i++) {
+                       if (gen.logs[i] == lognum) {
+                               printf_P(PSTR("Already on\r\n"));
+                               return;
+                       }
+               }
+               for (i=0; i<NB_LOGS; i++) {
+                       if (gen.logs[i] == 0) {
+                               gen.logs[i] = lognum;
+                               break;
+                       }
+               }
+               if (i==NB_LOGS) {
+                       printf_P(PSTR("no more room\r\n"));
+               }
+       }
+       else if (!strcmp_P(res->arg3, PSTR("off"))) {
+               for (i=0; i<NB_LOGS; i++) {
+                       if (gen.logs[i] == lognum) {
+                               gen.logs[i] = 0;
+                               break;
+                       }
+               }
+               if (i==NB_LOGS) {
+                       printf_P(PSTR("already off\r\n"));
+               }
+       }
+       cmd_log_do_show();
+}
+
+prog_char str_log_arg1_type[] = "type";
+parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+prog_char str_log_arg2_type[] = "uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor#bd#cs";
+parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+prog_char str_log_arg3[] = "on#off";
+parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+prog_char help_log_type[] = "Set log type";
+parse_pgm_inst_t cmd_log_type = {
+       .f = cmd_log_type_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_log_type,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_log_arg0,
+               (prog_void *)&cmd_log_arg1_type,
+               (prog_void *)&cmd_log_arg2_type,
+               (prog_void *)&cmd_log_arg3,
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(void *parsed_result, void *data)
+{
+       printf("res stack: %d\r\n", min_stack_space_available());
+       
+}
+
+prog_char str_stack_space_arg0[] = "stack_space";
+parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+prog_char help_stack_space[] = "Display remaining stack space";
+parse_pgm_inst_t cmd_stack_space = {
+       .f = cmd_stack_space_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_stack_space,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_stack_space_arg0, 
+               NULL,
+       },
+};
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
new file mode 100644 (file)
index 0000000..b379dc1
--- /dev/null
@@ -0,0 +1,2154 @@
+/*
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <avr/eeprom.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+#include <i2c.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "../common/eeprom_mapping.h"
+
+#include "main.h"
+#include "sensor.h"
+#include "cmdline.h"
+#include "strat.h"
+#include "strat_utils.h"
+#include "strat_base.h"
+#include "i2c_protocol.h"
+#include "actuator.h"
+
+struct cmd_event_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       fixed_string_t arg2;
+};
+
+
+/* function called when cmd_event is parsed successfully */
+static void cmd_event_parsed(void *parsed_result, void *data)
+{
+       u08 bit=0;
+
+       struct cmd_event_result * res = parsed_result;
+       
+       if (!strcmp_P(res->arg1, PSTR("all"))) {
+               bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
+                       DO_BD | DO_TIMER | DO_POWER;
+               if (!strcmp_P(res->arg2, PSTR("on")))
+                       mainboard.flags |= bit;
+               else if (!strcmp_P(res->arg2, PSTR("off")))
+                       mainboard.flags &= bit;
+               else { /* show */
+                       printf_P(PSTR("encoders is %s\r\n"), 
+                                (DO_ENCODERS & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("cs is %s\r\n"), 
+                                (DO_CS & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("rs is %s\r\n"), 
+                                (DO_RS & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("pos is %s\r\n"), 
+                                (DO_POS & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("bd is %s\r\n"), 
+                                (DO_BD & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("timer is %s\r\n"), 
+                                (DO_TIMER & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("power is %s\r\n"), 
+                                (DO_POWER & mainboard.flags) ? "on":"off");
+               }
+               return;
+       }
+
+       if (!strcmp_P(res->arg1, PSTR("encoders")))
+               bit = DO_ENCODERS;
+       else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+               strat_hardstop();
+               bit = DO_CS;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("rs")))
+               bit = DO_RS;
+       else if (!strcmp_P(res->arg1, PSTR("pos")))
+               bit = DO_POS;
+       else if (!strcmp_P(res->arg1, PSTR("bd")))
+               bit = DO_BD;
+       else if (!strcmp_P(res->arg1, PSTR("timer"))) {
+               time_reset();
+               bit = DO_TIMER;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("power")))
+               bit = DO_POWER;
+
+       if (!strcmp_P(res->arg2, PSTR("on")))
+               mainboard.flags |= bit;
+       else if (!strcmp_P(res->arg2, PSTR("off"))) {
+               if (!strcmp_P(res->arg1, PSTR("cs"))) {
+                       pwm_ng_set(LEFT_PWM, 0);
+                       pwm_ng_set(RIGHT_PWM, 0);
+               }
+               mainboard.flags &= (~bit);
+       }
+       printf_P(PSTR("%s is %s\r\n"), res->arg1, 
+                     (bit & mainboard.flags) ? "on":"off");
+}
+
+prog_char str_event_arg0[] = "event";
+parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
+prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
+parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
+prog_char str_event_arg2[] = "on#off#show";
+parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
+
+prog_char help_event[] = "Enable/disable events";
+parse_pgm_inst_t cmd_event = {
+       .f = cmd_event_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_event,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_event_arg0, 
+               (prog_void *)&cmd_event_arg1, 
+               (prog_void *)&cmd_event_arg2, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Spi_Test */
+
+/* this structure is filled when cmd_spi_test is parsed successfully */
+struct cmd_spi_test_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_spi_test is parsed successfully */
+static void cmd_spi_test_parsed(void * parsed_result, void * data)
+{
+       uint16_t i = 0, ret = 0, ret2 = 0;
+       
+       if (mainboard.flags & DO_ENCODERS) {
+               printf_P(PSTR("Disable encoder event first\r\n"));
+               return;
+       }
+
+       do {
+               spi_slave_select(0);
+               ret = spi_send_and_receive_byte(i);
+               ret2 = spi_send_and_receive_byte(i);
+               spi_slave_deselect(0);
+
+               if ((i & 0x7ff) == 0)
+                       printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
+                                i, ret, ret2);
+
+               i++;
+       } while(!cmdline_keypressed());
+}
+
+prog_char str_spi_test_arg0[] = "spi_test";
+parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
+
+prog_char help_spi_test[] = "Test the SPI";
+parse_pgm_inst_t cmd_spi_test = {
+       .f = cmd_spi_test_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_spi_test,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_spi_test_arg0, 
+               NULL,
+       },
+};
+
+
+
+/**********************************************************/
+/* Opponent tests */
+
+/* this structure is filled when cmd_opponent is parsed successfully */
+struct cmd_opponent_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int32_t arg2;
+       int32_t arg3;
+};
+
+/* function called when cmd_opponent is parsed successfully */
+static void cmd_opponent_parsed(void *parsed_result, void *data)
+{
+       int16_t x,y,d,a;
+
+       if (get_opponent_xyda(&x, &y, &d, &a))
+               printf_P(PSTR("No opponent\r\n"));
+       else
+               printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
+}
+
+prog_char str_opponent_arg0[] = "opponent";
+parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
+prog_char str_opponent_arg1[] = "show";
+parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
+
+prog_char help_opponent[] = "Show (x,y) opponent";
+parse_pgm_inst_t cmd_opponent = {
+       .f = cmd_opponent_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_opponent,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_opponent_arg0, 
+               (prog_void *)&cmd_opponent_arg1, 
+               NULL,
+       },
+};
+
+
+prog_char str_opponent_arg1_set[] = "set";
+parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
+parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
+parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
+
+prog_char help_opponent_set[] = "Set (x,y) opponent";
+parse_pgm_inst_t cmd_opponent_set = {
+       .f = cmd_opponent_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_opponent_set,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_opponent_arg0, 
+               (prog_void *)&cmd_opponent_arg1_set,
+               (prog_void *)&cmd_opponent_arg2, 
+               (prog_void *)&cmd_opponent_arg3, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Start */
+
+/* this structure is filled when cmd_start is parsed successfully */
+struct cmd_start_result {
+       fixed_string_t arg0;
+       fixed_string_t color;
+       fixed_string_t debug;
+};
+
+/* function called when cmd_start is parsed successfully */
+static void cmd_start_parsed(void *parsed_result, void *data)
+{
+       struct cmd_start_result *res = parsed_result;
+       uint8_t old_level = gen.log_level;
+
+       gen.logs[NB_LOGS] = E_USER_STRAT;
+       if (!strcmp_P(res->debug, PSTR("debug"))) {
+               strat_infos.dump_enabled = 1;
+               gen.log_level = 5;
+       }
+       else {
+               strat_infos.dump_enabled = 0;
+               gen.log_level = 0;
+       }
+
+       if (!strcmp_P(res->color, PSTR("red"))) {
+               mainboard.our_color = I2C_COLOR_RED;
+               i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
+               i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
+       }
+       else if (!strcmp_P(res->color, PSTR("green"))) {
+               mainboard.our_color = I2C_COLOR_GREEN;
+               i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
+               i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
+       }
+
+       printf_P(PSTR("Check that lintel is loaded\r\n"));
+       while(!cmdline_keypressed());
+
+       printf_P(PSTR("Press a key when beacon ready\r\n"));
+       i2c_sensorboard_set_beacon(0);
+       while(!cmdline_keypressed());
+       i2c_sensorboard_set_beacon(1);
+
+       strat_start();
+
+       gen.logs[NB_LOGS] = 0;
+       gen.log_level = old_level;
+}
+
+prog_char str_start_arg0[] = "start";
+parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
+prog_char str_start_color[] = "green#red";
+parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
+prog_char str_start_debug[] = "debug#match";
+parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
+
+prog_char help_start[] = "Start the robot";
+parse_pgm_inst_t cmd_start = {
+       .f = cmd_start_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_start,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_start_arg0, 
+               (prog_void *)&cmd_start_color, 
+               (prog_void *)&cmd_start_debug, 
+               NULL,
+       },
+};
+
+
+
+/**********************************************************/
+/* Interact */
+
+/* this structure is filled when cmd_interact is parsed successfully */
+struct cmd_interact_result {
+       fixed_string_t arg0;
+};
+
+static void print_cs(void)
+{
+       printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
+                     "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"), 
+                cs_get_consign(&mainboard.distance.cs),
+                cs_get_consign(&mainboard.angle.cs),
+                cs_get_filtered_consign(&mainboard.distance.cs),
+                cs_get_filtered_consign(&mainboard.angle.cs),
+                cs_get_error(&mainboard.distance.cs),
+                cs_get_error(&mainboard.angle.cs),
+                cs_get_out(&mainboard.distance.cs),
+                cs_get_out(&mainboard.angle.cs));
+}
+
+static void print_pos(void)
+{
+       printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), 
+                position_get_x_s16(&mainboard.pos),
+                position_get_y_s16(&mainboard.pos),
+                position_get_a_deg_s16(&mainboard.pos));
+}
+
+static void print_time(void)
+{
+       printf_P(PSTR("time %d\r\n"),time_get_s());
+}
+
+
+static void print_sensors(void)
+{
+#ifdef notyet
+       if (sensor_start_switch())
+               printf_P(PSTR("Start switch | "));
+       else
+               printf_P(PSTR("             | "));
+
+       if (IR_DISP_SENSOR())
+               printf_P(PSTR("IR disp | "));
+       else
+               printf_P(PSTR("        | "));
+
+       printf_P(PSTR("\r\n"));
+#endif
+}
+
+static void print_pid(void)
+{
+       printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
+                     "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+                pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
+                pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
+                pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
+                pid_get_value_out(&mainboard.distance.pid),
+                pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
+                pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
+                pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
+                pid_get_value_out(&mainboard.angle.pid));
+}
+
+#define PRINT_POS       (1<<0)
+#define PRINT_PID       (1<<1)
+#define PRINT_CS        (1<<2)
+#define PRINT_SENSORS   (1<<3)
+#define PRINT_TIME      (1<<4)
+#define PRINT_BLOCKING  (1<<5)
+
+static void cmd_interact_parsed(void * parsed_result, void * data)
+{
+       int c;
+       int8_t cmd;
+       uint8_t print = 0;
+       struct vt100 vt100;
+
+       vt100_init(&vt100);
+
+       printf_P(PSTR("Display debugs:\r\n"
+                     "  1:pos\r\n"
+                     "  2:pid\r\n"
+                     "  3:cs\r\n"
+                     "  4:sensors\r\n"
+                     "  5:time\r\n"
+                     /* "  6:blocking\r\n" */
+                     "Commands:\r\n"
+                     "  arrows:move\r\n"
+                     "  space:stop\r\n"
+                     "  q:quit\r\n"));
+
+       /* stop motors */
+       mainboard.flags &= (~DO_CS);
+       pwm_set_and_save(LEFT_PWM, 0);
+       pwm_set_and_save(RIGHT_PWM, 0);
+
+       while(1) {
+               if (print & PRINT_POS) {
+                       print_pos();
+               }
+
+               if (print & PRINT_PID) {
+                       print_pid();
+               }
+
+               if (print & PRINT_CS) {
+                       print_cs();
+               }
+
+               if (print & PRINT_SENSORS) {
+                       print_sensors();
+               }
+
+               if (print & PRINT_TIME) {
+                       print_time();
+               }
+/*             if (print & PRINT_BLOCKING) { */
+/*                     printf_P(PSTR("%s %s blocking=%d\r\n"),  */
+/*                              mainboard.blocking ? "BLOCK1":"      ", */
+/*                              rs_is_blocked(&mainboard.rs) ? "BLOCK2":"      ", */
+/*                              rs_get_blocking(&mainboard.rs)); */
+/*             } */
+
+               c = cmdline_getchar();
+               if (c == -1) {
+                       wait_ms(10);
+                       continue;
+               }
+               cmd = vt100_parser(&vt100, c);
+               if (cmd == -2) {
+                       wait_ms(10);
+                       continue;
+               }
+               
+               if (cmd == -1) {
+                       switch(c) {
+                       case '1': print ^= PRINT_POS; break;
+                       case '2': print ^= PRINT_PID; break;
+                       case '3': print ^= PRINT_CS; break;
+                       case '4': print ^= PRINT_SENSORS; break;
+                       case '5': print ^= PRINT_TIME; break;
+                       case '6': print ^= PRINT_BLOCKING; break;
+
+                       case 'q': 
+                               if (mainboard.flags & DO_CS)
+                                       strat_hardstop();
+                               pwm_set_and_save(LEFT_PWM, 0);
+                               pwm_set_and_save(RIGHT_PWM, 0);
+                               return;
+                       case ' ':
+                               pwm_set_and_save(LEFT_PWM, 0);
+                               pwm_set_and_save(RIGHT_PWM, 0);
+                               break;
+                       default: 
+                               break;
+                       }
+               }
+               else {
+                       switch(cmd) {
+                       case KEY_UP_ARR: 
+                               pwm_set_and_save(LEFT_PWM, 1200);
+                               pwm_set_and_save(RIGHT_PWM, 1200);
+                               break;
+                       case KEY_LEFT_ARR: 
+                               pwm_set_and_save(LEFT_PWM, -1200);
+                               pwm_set_and_save(RIGHT_PWM, 1200);
+                               break;
+                       case KEY_DOWN_ARR: 
+                               pwm_set_and_save(LEFT_PWM, -1200);
+                               pwm_set_and_save(RIGHT_PWM, -1200);
+                               break;
+                       case KEY_RIGHT_ARR:
+                               pwm_set_and_save(LEFT_PWM, 1200);
+                               pwm_set_and_save(RIGHT_PWM, -1200);
+                               break;
+                       }
+               }
+               wait_ms(10);
+       }
+}
+
+prog_char str_interact_arg0[] = "interact";
+parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
+
+prog_char help_interact[] = "Interactive mode";
+parse_pgm_inst_t cmd_interact = {
+       .f = cmd_interact_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_interact,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_interact_arg0, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Color */
+
+/* this structure is filled when cmd_color is parsed successfully */
+struct cmd_color_result {
+       fixed_string_t arg0;
+       fixed_string_t color;
+};
+
+/* function called when cmd_color is parsed successfully */
+static void cmd_color_parsed(void *parsed_result, void *data)
+{
+       struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
+       if (!strcmp_P(res->color, PSTR("red"))) {
+               mainboard.our_color = I2C_COLOR_RED;
+               i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
+               i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
+       }
+       else if (!strcmp_P(res->color, PSTR("green"))) {
+               mainboard.our_color = I2C_COLOR_GREEN;
+               i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
+               i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
+       }
+       printf_P(PSTR("Done\r\n"));
+}
+
+prog_char str_color_arg0[] = "color";
+parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
+prog_char str_color_color[] = "green#red";
+parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
+
+prog_char help_color[] = "Set our color";
+parse_pgm_inst_t cmd_color = {
+       .f = cmd_color_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_color,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_color_arg0, 
+               (prog_void *)&cmd_color_color, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Rs tests */
+
+/* this structure is filled when cmd_rs is parsed successfully */
+struct cmd_rs_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_rs is parsed successfully */
+static void cmd_rs_parsed(void *parsed_result, void *data)
+{
+       //      struct cmd_rs_result *res = parsed_result;
+       do {
+               printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "), 
+                        cs_get_consign(&mainboard.angle.cs),
+                        cs_get_filtered_feedback(&mainboard.angle.cs),
+                        cs_get_out(&mainboard.angle.cs));
+               printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "), 
+                        cs_get_consign(&mainboard.distance.cs),
+                        cs_get_filtered_feedback(&mainboard.distance.cs),
+                        cs_get_out(&mainboard.distance.cs));
+               printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
+                        mainboard.pwm_r);
+               wait_ms(100);
+       } while(!cmdline_keypressed());
+}
+
+prog_char str_rs_arg0[] = "rs";
+parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
+prog_char str_rs_arg1[] = "show";
+parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
+
+prog_char help_rs[] = "Show rs (robot system) values";
+parse_pgm_inst_t cmd_rs = {
+       .f = cmd_rs_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_rs,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_rs_arg0, 
+               (prog_void *)&cmd_rs_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* I2cdebug */
+
+/* this structure is filled when cmd_i2cdebug is parsed successfully */
+struct cmd_i2cdebug_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_i2cdebug is parsed successfully */
+static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
+{
+       i2c_debug();
+       i2c_protocol_debug();
+}
+
+prog_char str_i2cdebug_arg0[] = "i2cdebug";
+parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
+
+prog_char help_i2cdebug[] = "I2c debug infos";
+parse_pgm_inst_t cmd_i2cdebug = {
+       .f = cmd_i2cdebug_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_i2cdebug,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_i2cdebug_arg0, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Mechboard_Show */
+
+/* this structure is filled when cmd_mechboard_show is parsed successfully */
+struct cmd_mechboard_show_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_mechboard_show is parsed successfully */
+static void cmd_mechboard_show_parsed(void * parsed_result, void * data)
+{
+       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);
+
+}
+
+prog_char str_mechboard_show_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0);
+prog_char str_mechboard_show_arg1[] = "show";
+parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1);
+
+prog_char help_mechboard_show[] = "show mechboard status";
+parse_pgm_inst_t cmd_mechboard_show = {
+       .f = cmd_mechboard_show_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_mechboard_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_mechboard_show_arg0, 
+               (prog_void *)&cmd_mechboard_show_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Mechboard_Setmode1 */
+
+/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
+struct cmd_mechboard_setmode1_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_mechboard_setmode1 is parsed successfully */
+static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data)
+{
+       struct cmd_mechboard_setmode1_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("init")))
+               i2c_mechboard_mode_init();
+       else if (!strcmp_P(res->arg1, PSTR("manual")))
+               i2c_mechboard_mode_manual();
+       else if (!strcmp_P(res->arg1, PSTR("pickup")))
+               i2c_mechboard_mode_pickup();
+       else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
+               i2c_mechboard_mode_lazy_harvest();
+       else if (!strcmp_P(res->arg1, PSTR("harvest")))
+               i2c_mechboard_mode_harvest();
+       else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
+               i2c_mechboard_mode_prepare_get_lintel();
+       else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
+               i2c_mechboard_mode_get_lintel();
+       else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
+               i2c_mechboard_mode_put_lintel();
+       else if (!strcmp_P(res->arg1, PSTR("init")))
+               i2c_mechboard_mode_init();
+       else if (!strcmp_P(res->arg1, PSTR("eject")))
+               i2c_mechboard_mode_init();
+       else if (!strcmp_P(res->arg1, PSTR("clear")))
+               i2c_mechboard_mode_clear();
+       else if (!strcmp_P(res->arg1, PSTR("loaded")))
+               i2c_mechboard_mode_loaded();
+       else if (!strcmp_P(res->arg1, PSTR("store")))
+               i2c_mechboard_mode_store();
+       else if (!strcmp_P(res->arg1, PSTR("manivelle")))
+               i2c_mechboard_mode_manivelle();
+       else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
+               i2c_mechboard_mode_lazy_pickup();
+}
+
+prog_char str_mechboard_setmode1_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0);
+prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup";
+parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1);
+
+prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)";
+parse_pgm_inst_t cmd_mechboard_setmode1 = {
+       .f = cmd_mechboard_setmode1_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_mechboard_setmode1,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_mechboard_setmode1_arg0, 
+               (prog_void *)&cmd_mechboard_setmode1_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Mechboard_Setmode2 */
+
+/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
+struct cmd_mechboard_setmode2_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       fixed_string_t arg2;
+};
+
+/* function called when cmd_mechboard_setmode2 is parsed successfully */
+static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data)
+{
+       struct cmd_mechboard_setmode2_result *res = parsed_result;
+       uint8_t side = I2C_LEFT_SIDE;
+
+       if (!strcmp_P(res->arg2, PSTR("left")))
+               side = I2C_LEFT_SIDE;
+       else if (!strcmp_P(res->arg2, PSTR("right")))
+               side = I2C_RIGHT_SIDE;
+       else if (!strcmp_P(res->arg2, PSTR("center")))
+               side = I2C_CENTER_SIDE;
+       else if (!strcmp_P(res->arg2, PSTR("auto")))
+               side = I2C_AUTO_SIDE;
+
+       if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
+               i2c_mechboard_mode_prepare_pickup(side);
+       else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
+               i2c_mechboard_mode_push_temple_disc(side);
+}
+
+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();
+}
+
+prog_char str_pickup_test_arg0[] = "pickup_test";
+parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0);
+prog_char str_pickup_test_arg1[] = "left#right";
+parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1);
+parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16);
+
+prog_char help_pickup_test[] = "Pickup_Test function";
+parse_pgm_inst_t cmd_pickup_test = {
+       .f = cmd_pickup_test_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_pickup_test,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_pickup_test_arg0,
+               (prog_void *)&cmd_pickup_test_arg1,
+               (prog_void *)&cmd_pickup_test_arg2,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Lintel_Test */
+
+/* this structure is filled when cmd_lintel_test is parsed successfully */
+struct cmd_lintel_test_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_lintel_test is parsed successfully */
+static void cmd_lintel_test_parsed(void *parsed_result, void *data)
+{
+       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 {
+       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)
+{
+       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();
+       }
+}
+
+prog_char str_scanner_arg0[] = "scanner";
+parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0);
+prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib";
+parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1);
+
+prog_char help_scanner[] = "send commands to scanner";
+parse_pgm_inst_t cmd_scanner = {
+       .f = cmd_scanner_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_scanner,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_scanner_arg0, 
+               (prog_void *)&cmd_scanner_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Build_Z1 */
+
+/* this structure is filled when cmd_build_z1 is parsed successfully */
+struct cmd_build_z1_result {
+       fixed_string_t arg0;
+       uint8_t level;
+       int16_t d1;
+       int16_t d2;
+       int16_t d3;
+};
+
+/* function called when cmd_build_z1 is parsed successfully */
+static void cmd_build_z1_parsed(void *parsed_result, void *data)
+{
+       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 */
+struct cmd_test_result {
+       fixed_string_t arg0;
+       int32_t radius;
+};
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+                               double radius_mm,
+                               double *speed_d,
+                               double *speed_a);
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(void *parsed_result, void *data)
+{
+       struct cmd_test_result *res = parsed_result;
+       double d,a;
+       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+       circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
+       printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
+}
+
+prog_char str_test_arg0[] = "test";
+parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
+parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
+
+prog_char help_test[] = "Test function";
+parse_pgm_inst_t cmd_test = {
+       .f = cmd_test_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_test,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_test_arg0,
+               (prog_void *)&cmd_test_arg1,
+               NULL,
+       },
+};
diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
new file mode 100644 (file)
index 0000000..f3b38fd
--- /dev/null
@@ -0,0 +1,1146 @@
+/*
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org> 
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <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 <encoders_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 <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cs.h"
+#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"
+
+/**********************************************************/
+/* Traj_Speeds for trajectory_manager */
+
+/* this structure is filled when cmd_traj_speed is parsed successfully */
+struct cmd_traj_speed_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint16_t s;
+};
+
+/* function called when cmd_traj_speed is parsed successfully */
+static void cmd_traj_speed_parsed(void *parsed_result, void *data)
+{
+       struct cmd_traj_speed_result * res = parsed_result;
+       
+       if (!strcmp_P(res->arg1, PSTR("angle"))) {
+               trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+               trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
+       }
+       /* else it is a "show" */
+
+       printf_P(PSTR("angle %u, distance %u\r\n"), 
+                mainboard.traj.a_speed,
+                mainboard.traj.d_speed);
+}
+
+prog_char str_traj_speed_arg0[] = "traj_speed";
+parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
+prog_char str_traj_speed_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
+parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
+
+prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
+parse_pgm_inst_t cmd_traj_speed = {
+       .f = cmd_traj_speed_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_traj_speed,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_traj_speed_arg0, 
+               (prog_void *)&cmd_traj_speed_arg1, 
+               (prog_void *)&cmd_traj_speed_s, 
+               NULL,
+       },
+};
+
+/* show */
+
+prog_char str_traj_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
+
+prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
+parse_pgm_inst_t cmd_traj_speed_show = {
+       .f = cmd_traj_speed_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_traj_speed_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_traj_speed_arg0, 
+               (prog_void *)&cmd_traj_speed_show_arg,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* trajectory window configuration */
+
+/* this structure is filled when cmd_trajectory is parsed successfully */
+struct cmd_trajectory_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       float d_win;
+       float a_win;
+       float a_start;
+};
+
+
+/* function called when cmd_trajectory is parsed successfully */
+static void cmd_trajectory_parsed(void * parsed_result, void * data)
+{
+       struct cmd_trajectory_result * res = parsed_result;
+       
+       if (!strcmp_P(res->arg1, PSTR("set"))) {
+               trajectory_set_windows(&mainboard.traj, res->d_win,
+                                      res->a_win, res->a_start);
+       }
+
+       printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
+                DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
+}
+
+prog_char str_trajectory_arg0[] = "trajectory";
+parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
+prog_char str_trajectory_arg1[] = "set";
+parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
+parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
+parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
+parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
+
+prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
+parse_pgm_inst_t cmd_trajectory = {
+       .f = cmd_trajectory_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_trajectory,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_trajectory_arg0, 
+               (prog_void *)&cmd_trajectory_arg1, 
+               (prog_void *)&cmd_trajectory_d, 
+               (prog_void *)&cmd_trajectory_a, 
+               (prog_void *)&cmd_trajectory_as, 
+               NULL,
+       },
+};
+
+/* show */
+
+prog_char str_trajectory_show_arg[] = "show";
+parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
+
+prog_char help_trajectory_show[] = "Show trajectory window configuration";
+parse_pgm_inst_t cmd_trajectory_show = {
+       .f = cmd_trajectory_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_trajectory_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_trajectory_arg0, 
+               (prog_void *)&cmd_trajectory_show_arg,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* rs_gains configuration */
+
+/* this structure is filled when cmd_rs_gains is parsed successfully */
+struct cmd_rs_gains_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       float left;
+       float right;
+};
+
+/* function called when cmd_rs_gains is parsed successfully */
+static void cmd_rs_gains_parsed(void * parsed_result, void * data)
+{
+       struct cmd_rs_gains_result * res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("set"))) {
+               rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+                                       LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
+               rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+                                        RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
+       }
+       printf_P(PSTR("rs_gains set "));
+       f64_print(mainboard.rs.left_ext_gain);
+       printf_P(PSTR(" "));
+       f64_print(mainboard.rs.right_ext_gain);
+       printf_P(PSTR("\r\n"));
+}
+
+prog_char str_rs_gains_arg0[] = "rs_gains";
+parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
+prog_char str_rs_gains_arg1[] = "set";
+parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
+parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
+parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
+
+prog_char help_rs_gains[] = "Set rs_gains (left, right)";
+parse_pgm_inst_t cmd_rs_gains = {
+       .f = cmd_rs_gains_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_rs_gains,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_rs_gains_arg0, 
+               (prog_void *)&cmd_rs_gains_arg1, 
+               (prog_void *)&cmd_rs_gains_l, 
+               (prog_void *)&cmd_rs_gains_r, 
+               NULL,
+       },
+};
+
+/* show */
+
+prog_char str_rs_gains_show_arg[] = "show";
+parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
+
+prog_char help_rs_gains_show[] = "Show rs_gains";
+parse_pgm_inst_t cmd_rs_gains_show = {
+       .f = cmd_rs_gains_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_rs_gains_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_rs_gains_arg0, 
+               (prog_void *)&cmd_rs_gains_show_arg,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* track configuration */
+
+/* this structure is filled when cmd_track is parsed successfully */
+struct cmd_track_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       float val;
+};
+
+/* function called when cmd_track is parsed successfully */
+static void cmd_track_parsed(void * parsed_result, void * data)
+{
+       struct cmd_track_result * res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("set"))) {
+               position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
+       }
+       printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
+}
+
+prog_char str_track_arg0[] = "track";
+parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
+prog_char str_track_arg1[] = "set";
+parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
+parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
+
+prog_char help_track[] = "Set track in mm";
+parse_pgm_inst_t cmd_track = {
+       .f = cmd_track_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_track,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_track_arg0, 
+               (prog_void *)&cmd_track_arg1, 
+               (prog_void *)&cmd_track_val, 
+               NULL,
+       },
+};
+
+/* show */
+
+prog_char str_track_show_arg[] = "show";
+parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
+
+prog_char help_track_show[] = "Show track";
+parse_pgm_inst_t cmd_track_show = {
+       .f = cmd_track_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_track_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_track_arg0, 
+               (prog_void *)&cmd_track_show_arg,
+               NULL,
+       },
+};
+
+
+
+/**********************************************************/
+/* Pt_Lists for testing traj */
+
+#define PT_LIST_SIZE 10
+static struct xy_point pt_list[PT_LIST_SIZE];
+static uint16_t pt_list_len = 0;
+
+/* this structure is filled when cmd_pt_list is parsed successfully */
+struct cmd_pt_list_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint16_t arg2;
+       int16_t arg3;
+       int16_t arg4;
+};
+
+/* function called when cmd_pt_list is parsed successfully */
+static void cmd_pt_list_parsed(void * parsed_result, void * data)
+{
+       struct cmd_pt_list_result * res = parsed_result;
+       uint8_t i, why=0;
+       
+       if (!strcmp_P(res->arg1, PSTR("append"))) {
+               res->arg2 = pt_list_len;
+       }
+
+       if (!strcmp_P(res->arg1, PSTR("insert")) ||
+           !strcmp_P(res->arg1, PSTR("append"))) {
+               if (res->arg2 > pt_list_len) {
+                       printf_P(PSTR("Index too large\r\n"));
+                       return;
+               }
+               if (pt_list_len >= PT_LIST_SIZE) {
+                       printf_P(PSTR("List is too large\r\n"));
+                       return;
+               }
+               memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], 
+                      PT_LIST_SIZE-1-res->arg2);
+               pt_list[res->arg2].x = res->arg3;
+               pt_list[res->arg2].y = res->arg4;
+               pt_list_len++;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("del"))) {
+               if (pt_list_len <= 0) {
+                       printf_P(PSTR("Error: list empty\r\n"));
+                       return;
+               }
+               if (res->arg2 > pt_list_len) {
+                       printf_P(PSTR("Index too large\r\n"));
+                       return;
+               }
+               memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], 
+                      (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
+               pt_list_len--;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("reset"))) {
+               pt_list_len = 0;
+       }
+       
+       /* else it is a "show" or a "start" */
+       if (pt_list_len == 0) {
+               printf_P(PSTR("List empty\r\n"));
+               return;
+       }
+       for (i=0 ; i<pt_list_len ; i++) {
+               printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
+               if (!strcmp_P(res->arg1, PSTR("start"))) {
+                       trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
+                       why = wait_traj_end(0xFF); /* all */
+               }
+               else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
+                       while (1) {
+                               why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
+                               printf("next point\r\n");
+                               if (why != END_OBSTACLE)
+                                       break;
+                       }
+               }
+               if (why & (~(END_TRAJ | END_NEAR)))
+                       trajectory_stop(&mainboard.traj);
+       }
+}
+
+prog_char str_pt_list_arg0[] = "pt_list";
+parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
+prog_char str_pt_list_arg1[] = "insert";
+parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
+parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
+parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
+parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
+
+prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
+parse_pgm_inst_t cmd_pt_list = {
+       .f = cmd_pt_list_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_pt_list,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_pt_list_arg0, 
+               (prog_void *)&cmd_pt_list_arg1, 
+               (prog_void *)&cmd_pt_list_arg2, 
+               (prog_void *)&cmd_pt_list_arg3, 
+               (prog_void *)&cmd_pt_list_arg4, 
+               NULL,
+       },
+};
+
+/* append */
+
+prog_char str_pt_list_arg1_append[] = "append";
+parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
+
+prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
+parse_pgm_inst_t cmd_pt_list_append = {
+       .f = cmd_pt_list_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_pt_list_append,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_pt_list_arg0, 
+               (prog_void *)&cmd_pt_list_arg1_append, 
+               (prog_void *)&cmd_pt_list_arg3, 
+               (prog_void *)&cmd_pt_list_arg4, 
+               NULL,
+       },
+};
+
+/* del */
+
+prog_char str_pt_list_del_arg[] = "del";
+parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
+
+prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
+parse_pgm_inst_t cmd_pt_list_del = {
+       .f = cmd_pt_list_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_pt_list_del,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_pt_list_arg0, 
+               (prog_void *)&cmd_pt_list_del_arg, 
+               (prog_void *)&cmd_pt_list_arg2,
+               NULL,
+       },
+};
+/* show */
+
+prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
+parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
+
+prog_char help_pt_list_show[] = "Show, start or reset pt_list";
+parse_pgm_inst_t cmd_pt_list_show = {
+       .f = cmd_pt_list_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_pt_list_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_pt_list_arg0, 
+               (prog_void *)&cmd_pt_list_show_arg,
+               NULL,
+       },
+};
+
+
+
+/**********************************************************/
+/* Goto function */
+
+/* this structure is filled when cmd_goto is parsed successfully */
+struct cmd_goto_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int32_t arg2;
+       int32_t arg3;
+       int32_t arg4;
+};
+
+/* function called when cmd_goto is parsed successfully */
+static void cmd_goto_parsed(void * parsed_result, void * data)
+{
+       struct cmd_goto_result * res = parsed_result;
+       uint8_t err;
+       microseconds t1, t2;
+
+       interrupt_traj_reset();
+       if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
+               trajectory_a_rel(&mainboard.traj, res->arg2);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
+               trajectory_d_rel(&mainboard.traj, res->arg2);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
+               trajectory_a_abs(&mainboard.traj, res->arg2);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
+               trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
+               trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
+               trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
+               trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
+               err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
+               if (err != END_TRAJ && err != END_NEAR)
+                       strat_hardstop();
+       }
+       else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
+               err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
+               if (err != END_TRAJ && err != END_NEAR)
+                       strat_hardstop();
+       }
+       else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
+               trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
+               trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
+               trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
+       }
+       t1 = time_get_us2();
+       while ((err = test_traj_end(0xFF)) == 0) {
+               t2 = time_get_us2();
+               if (t2 - t1 > 200000) {
+                       dump_cs_debug("angle", &mainboard.angle.cs);
+                       dump_cs_debug("distance", &mainboard.distance.cs);
+                       t1 = t2;
+               }
+       }
+       if (err != END_TRAJ && err != END_NEAR)
+               strat_hardstop();
+       printf_P(PSTR("returned %s\r\n"), get_err(err));
+}
+
+prog_char str_goto_arg0[] = "goto";
+parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
+prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
+parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
+parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
+
+/* 1 params */
+prog_char help_goto1[] = "Change orientation of the mainboard";
+parse_pgm_inst_t cmd_goto1 = {
+       .f = cmd_goto_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_goto1,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_goto_arg0, 
+               (prog_void *)&cmd_goto_arg1_a, 
+               (prog_void *)&cmd_goto_arg2, 
+               NULL,
+       },
+};
+
+prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy";
+parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
+parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
+
+/* 2 params */
+prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
+parse_pgm_inst_t cmd_goto2 = {
+       .f = cmd_goto_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_goto2,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_goto_arg0, 
+               (prog_void *)&cmd_goto_arg1_b, 
+               (prog_void *)&cmd_goto_arg2,
+               (prog_void *)&cmd_goto_arg3, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Position tests */
+
+/* this structure is filled when cmd_position is parsed successfully */
+struct cmd_position_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int32_t arg2;
+       int32_t arg3;
+       int32_t arg4;
+};
+
+#define AUTOPOS_SPEED_FAST 200
+static void auto_position(void)
+{
+       uint8_t err;
+       uint16_t old_spdd, old_spda;
+
+       interrupt_traj_reset();
+       strat_get_speed(&old_spdd, &old_spda);
+       strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
+
+       trajectory_d_rel(&mainboard.traj, -300);
+       err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
+       if (err == END_INTR)
+               goto intr;
+       wait_ms(100);
+       strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
+
+       trajectory_d_rel(&mainboard.traj, 120);
+       err = wait_traj_end(END_INTR|END_TRAJ);
+       if (err == END_INTR)
+               goto intr;
+
+       trajectory_a_rel(&mainboard.traj, COLOR_A(90));
+       err = wait_traj_end(END_INTR|END_TRAJ);
+       if (err == END_INTR)
+               goto intr;
+
+       trajectory_d_rel(&mainboard.traj, -300);
+       err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
+       if (err == END_INTR)
+               goto intr;
+       wait_ms(100);
+       strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
+                       COLOR_A(90));
+
+       trajectory_d_rel(&mainboard.traj, 120);
+       err = wait_traj_end(END_INTR|END_TRAJ);
+       if (err == END_INTR)
+               goto intr;
+       wait_ms(100);
+       
+       trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
+       err = wait_traj_end(END_INTR|END_TRAJ);
+       if (err == END_INTR)
+               goto intr;
+       wait_ms(100);
+       
+       strat_set_speed(old_spdd, old_spda);
+       return;
+
+intr:
+       strat_hardstop();
+       strat_set_speed(old_spdd, old_spda);
+}
+
+/* function called when cmd_position is parsed successfully */
+static void cmd_position_parsed(void * parsed_result, void * data)
+{
+       struct cmd_position_result * res = parsed_result;
+       
+       /* display raw position values */
+       if (!strcmp_P(res->arg1, PSTR("reset"))) {
+               position_set(&mainboard.pos, 0, 0, 0);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("set"))) {
+               position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
+               mainboard.our_color = I2C_COLOR_GREEN;
+               i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
+               i2c_set_color(I2C_SENSORBOARD_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);
+               auto_position();
+       }
+
+       /* else it's just a "show" */
+       printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), 
+                position_get_x_double(&mainboard.pos),
+                position_get_y_double(&mainboard.pos),
+                DEG(position_get_a_rad_double(&mainboard.pos)));
+}
+
+prog_char str_position_arg0[] = "position";
+parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
+prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
+parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
+
+prog_char help_position[] = "Show/reset (x,y,a) position";
+parse_pgm_inst_t cmd_position = {
+       .f = cmd_position_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_position,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_position_arg0, 
+               (prog_void *)&cmd_position_arg1, 
+               NULL,
+       },
+};
+
+
+prog_char str_position_arg1_set[] = "set";
+parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
+parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
+parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
+parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
+
+prog_char help_position_set[] = "Set (x,y,a) position";
+parse_pgm_inst_t cmd_position_set = {
+       .f = cmd_position_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_position_set,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_position_arg0, 
+               (prog_void *)&cmd_position_arg1_set, 
+               (prog_void *)&cmd_position_arg2, 
+               (prog_void *)&cmd_position_arg3, 
+               (prog_void *)&cmd_position_arg4, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_infos is parsed successfully */
+struct cmd_strat_infos_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_infos is parsed successfully */
+static void cmd_strat_infos_parsed(void *parsed_result, void *data)
+{
+       struct cmd_strat_infos_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("reset"))) {
+               strat_reset_infos();
+       }
+       strat_infos.dump_enabled = 1;
+       strat_dump_infos(__FUNCTION__);
+}
+
+prog_char str_strat_infos_arg0[] = "strat_infos";
+parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
+prog_char str_strat_infos_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
+
+prog_char help_strat_infos[] = "reset/show strat_infos";
+parse_pgm_inst_t cmd_strat_infos = {
+       .f = cmd_strat_infos_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_strat_infos,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_strat_infos_arg0, 
+               (prog_void *)&cmd_strat_infos_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf is parsed successfully */
+struct cmd_strat_conf_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_conf is parsed successfully */
+static void cmd_strat_conf_parsed(void *parsed_result, void *data)
+{
+       struct cmd_strat_conf_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("base"))) {
+               strat_infos.conf.flags = 0;
+               strat_infos.conf.scan_our_min_time = 90;
+               strat_infos.conf.delay_between_our_scan = 90;
+               strat_infos.conf.scan_opp_min_time = 90;
+               strat_infos.conf.delay_between_opp_scan = 90;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("big3"))) {
+               strat_infos.conf.flags = 
+                       STRAT_CONF_STORE_STATIC2 |
+                       STRAT_CONF_BIG_3_TEMPLE;
+               strat_infos.conf.scan_our_min_time = 90;
+               strat_infos.conf.delay_between_our_scan = 90;
+               strat_infos.conf.scan_opp_min_time = 90;
+               strat_infos.conf.delay_between_opp_scan = 90;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
+               strat_infos.conf.flags = 0;
+               strat_infos.conf.scan_our_min_time = 35;
+               strat_infos.conf.delay_between_our_scan = 90;
+               strat_infos.conf.scan_opp_min_time = 90;
+               strat_infos.conf.delay_between_opp_scan = 90;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
+               strat_infos.conf.flags = 
+                       STRAT_CONF_STORE_STATIC2 |
+                       STRAT_CONF_BIG_3_TEMPLE;
+               strat_infos.conf.scan_our_min_time = 35;
+               strat_infos.conf.delay_between_our_scan = 90;
+               strat_infos.conf.scan_opp_min_time = 90;
+               strat_infos.conf.delay_between_opp_scan = 90;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
+               strat_infos.conf.flags = 
+                       STRAT_CONF_TAKE_ONE_LINTEL |
+                       STRAT_CONF_STORE_STATIC2 |
+                       STRAT_CONF_EARLY_SCAN |
+                       STRAT_CONF_PUSH_OPP_COLS;
+               strat_infos.conf.scan_our_min_time = 50;
+               strat_infos.conf.delay_between_our_scan = 90;
+               strat_infos.conf.scan_opp_min_time = 15;
+               strat_infos.conf.delay_between_opp_scan = 90;
+               strat_infos.conf.wait_opponent = 5;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
+               strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
+               strat_infos.conf.scan_our_min_time = 90;
+               strat_infos.conf.delay_between_our_scan = 90;
+               strat_infos.conf.scan_opp_min_time = 30;
+               strat_infos.conf.delay_between_opp_scan = 90;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
+               strat_infos.conf.flags = 
+                       STRAT_CONF_ONLY_ONE_ON_DISC;
+               strat_infos.conf.scan_our_min_time = 90;
+               strat_infos.conf.delay_between_our_scan = 90;
+               strat_infos.conf.scan_opp_min_time = 90;
+               strat_infos.conf.delay_between_opp_scan = 90;
+       }
+       strat_infos.dump_enabled = 1;
+       strat_dump_conf();
+}
+
+prog_char str_strat_conf_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
+prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
+parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
+
+prog_char help_strat_conf[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf = {
+       .f = cmd_strat_conf_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_strat_conf,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_strat_conf_arg0, 
+               (prog_void *)&cmd_strat_conf_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf2 is parsed successfully */
+struct cmd_strat_conf2_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       fixed_string_t arg2;
+};
+
+/* function called when cmd_strat_conf2 is parsed successfully */
+static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
+{
+       struct cmd_strat_conf2_result *res = parsed_result;
+       uint8_t on, bit = 0;
+
+       if (!strcmp_P(res->arg2, PSTR("on")))
+               on = 1;
+       else
+               on = 0;
+       
+       if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
+               bit = STRAT_CONF_ONLY_ONE_ON_DISC;
+       else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
+               bit = STRAT_CONF_BYPASS_STATIC2;
+       else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
+               bit = STRAT_CONF_TAKE_ONE_LINTEL;
+       else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
+               bit = STRAT_CONF_TAKE_ONE_LINTEL;
+       else if (!strcmp_P(res->arg1, PSTR("store_static2")))
+               bit = STRAT_CONF_STORE_STATIC2;
+       else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
+               bit = STRAT_CONF_BIG_3_TEMPLE;
+       else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
+               bit = STRAT_CONF_EARLY_SCAN;
+       else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
+               bit = STRAT_CONF_PUSH_OPP_COLS;
+
+       if (on)
+               strat_infos.conf.flags |= bit;
+       else
+               strat_infos.conf.flags &= (~bit);
+
+       strat_infos.dump_enabled = 1;
+       strat_dump_conf();
+}
+
+prog_char str_strat_conf2_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
+prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan";
+parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
+prog_char str_strat_conf2_arg2[] = "on#off";
+parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
+
+
+prog_char help_strat_conf2[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf2 = {
+       .f = cmd_strat_conf2_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_strat_conf2,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_strat_conf2_arg0, 
+               (prog_void *)&cmd_strat_conf2_arg1, 
+               (prog_void *)&cmd_strat_conf2_arg2, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf3 is parsed successfully */
+struct cmd_strat_conf3_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint8_t arg2;
+};
+
+/* function called when cmd_strat_conf3 is parsed successfully */
+static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
+{
+       struct cmd_strat_conf3_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
+               if (res->arg2 > 90)
+                       res->arg2 = 90;
+               strat_infos.conf.scan_opp_min_time = res->arg2;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
+               if (res->arg2 > 90)
+                       res->arg2 = 90;
+               strat_infos.conf.delay_between_opp_scan = res->arg2;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
+               if (res->arg2 > 90)
+                       res->arg2 = 90;
+               strat_infos.conf.scan_our_min_time = res->arg2;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
+               if (res->arg2 > 90)
+                       res->arg2 = 90;
+               strat_infos.conf.delay_between_our_scan = res->arg2;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
+               strat_infos.conf.wait_opponent = res->arg2;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
+               strat_infos.conf.lintel_min_time = res->arg2;
+       }
+       strat_infos.dump_enabled = 1;
+       strat_dump_conf();
+}
+
+prog_char str_strat_conf3_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
+prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent";
+parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
+parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
+
+prog_char help_strat_conf3[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf3 = {
+       .f = cmd_strat_conf3_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_strat_conf3,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_strat_conf3_arg0, 
+               (prog_void *)&cmd_strat_conf3_arg1, 
+               (prog_void *)&cmd_strat_conf3_arg2, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf4 is parsed successfully */
+struct cmd_strat_conf4_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int16_t arg2;
+};
+
+/* function called when cmd_strat_conf4 is parsed successfully */
+static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
+{
+       struct cmd_strat_conf4_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
+               strat_infos.conf.scan_opp_angle = res->arg2;
+       }
+       strat_infos.dump_enabled = 1;
+       strat_dump_conf();
+}
+
+prog_char str_strat_conf4_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
+prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
+parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
+parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
+
+prog_char help_strat_conf4[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf4 = {
+       .f = cmd_strat_conf4_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_strat_conf4,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_strat_conf4_arg0, 
+               (prog_void *)&cmd_strat_conf4_arg1, 
+               (prog_void *)&cmd_strat_conf4_arg2, 
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Subtraj */
+
+/* this structure is filled when cmd_subtraj is parsed successfully */
+struct cmd_subtraj_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int32_t arg2;
+       int32_t arg3;
+       int32_t arg4;
+       int32_t arg5;
+};
+
+/* function called when cmd_subtraj is parsed successfully */
+static void cmd_subtraj_parsed(void *parsed_result, void *data)
+{
+       struct cmd_subtraj_result *res = parsed_result;
+       uint8_t err = 0;
+       struct column_dispenser *disp;
+
+       if (strcmp_P(res->arg1, PSTR("static")) == 0) {
+               err = strat_static_columns(res->arg2);
+       }
+       else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
+               strat_infos.s_cols.configuration = res->arg2;
+               switch (res->arg2) {
+               case 1:
+                       position_set(&mainboard.pos, 1398, 
+                                    COLOR_Y(1297), COLOR_A(-66));
+                       break;
+               case 2:
+                       position_set(&mainboard.pos, 1232, 
+                                    COLOR_Y(1051), COLOR_A(4));
+                       break;
+               case 3:
+                       position_set(&mainboard.pos, 1232, 
+                                    COLOR_Y(1043), COLOR_A(5));
+                       break;
+               case 4:
+                       position_set(&mainboard.pos, 1346,
+                                    COLOR_Y(852), COLOR_A(57));
+                       break;
+               default:
+                       return;
+               }
+               if (res->arg2 == 1 && res->arg3 == 1) {
+                       strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
+               }
+               if (res->arg2 == 1 && res->arg3 == 2) {
+                       strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
+               }
+               err = strat_static_columns_pass2();
+       }
+       else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
+               err = strat_goto_lintel_disp(&strat_infos.l1);
+       }
+       else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
+               err = strat_goto_lintel_disp(&strat_infos.l2);
+       }
+       else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
+               disp = &strat_infos.c1;
+               err = strat_goto_col_disp(&disp);
+       }
+       else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
+               disp = &strat_infos.c2;
+               err = strat_goto_col_disp(&disp);
+       }
+       else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
+               disp = &strat_infos.c3;
+               err = strat_goto_col_disp(&disp);
+       }
+       else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
+               if (res->arg2 == 0) {
+                       printf_P(PSTR("bad level\r\n"));
+                       return;
+               }
+               err = strat_goto_disc(res->arg2);
+       }
+
+       printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
+       trajectory_hardstop(&mainboard.traj);
+}
+
+prog_char str_subtraj_arg0[] = "subtraj";
+parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
+prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
+parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
+parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
+parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
+parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
+parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
+
+prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
+parse_pgm_inst_t cmd_subtraj = {
+       .f = cmd_subtraj_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_subtraj,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_subtraj_arg0, 
+               (prog_void *)&cmd_subtraj_arg1, 
+               (prog_void *)&cmd_subtraj_arg2, 
+               (prog_void *)&cmd_subtraj_arg3, 
+               (prog_void *)&cmd_subtraj_arg4, 
+               (prog_void *)&cmd_subtraj_arg5, 
+               NULL,
+       },
+};
diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c
new file mode 100644 (file)
index 0000000..ec70f9a
--- /dev/null
@@ -0,0 +1,239 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: cs.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+#include <adc.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 <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "strat.h"
+#include "actuator.h"
+
+/* called every 5 ms */
+static void do_cs(void *dummy) 
+{
+       static uint16_t cpt = 0;
+       static int32_t old_a = 0, old_d = 0;
+
+       /* read encoders */
+       if (mainboard.flags & DO_ENCODERS) {
+               encoders_spi_manage(NULL);
+       }
+
+       /* XXX there is an issue which is probably related to avr-libc
+        * 1.6.2 (debian): this code using fixed_point lib does not
+        * work with it */
+       /* robot system, conversion to angle,distance */
+       if (mainboard.flags & DO_RS) {
+               int16_t a,d;
+               rs_update(&mainboard.rs); /* takes about 0.5 ms */
+               /* process and store current speed */
+               a = rs_get_angle(&mainboard.rs);
+               d = rs_get_distance(&mainboard.rs);
+               mainboard.speed_a = a - old_a;
+               mainboard.speed_d = d - old_d;
+               old_a = a;
+               old_d = d;
+       }
+
+       /* control system */
+       if (mainboard.flags & DO_CS) {
+               if (mainboard.angle.on)
+                       cs_manage(&mainboard.angle.cs);
+               if (mainboard.distance.on)
+                       cs_manage(&mainboard.distance.cs);
+       }
+       if ((cpt & 1) && (mainboard.flags & DO_POS)) {
+               /* about 1.5ms (worst case without centrifugal force
+                * compensation) */
+               position_manage(&mainboard.pos);
+       }
+       if (mainboard.flags & DO_BD) {
+               bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
+               bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
+       }
+       if (mainboard.flags & DO_TIMER) {
+               uint8_t second;
+               /* the robot should stop correctly in the strat, but
+                * in some cases, we must force the stop from an
+                * interrupt */
+               second = time_get_s();
+               if (second >= MATCH_TIME + 2) {
+                       pwm_ng_set(LEFT_PWM, 0);
+                       pwm_ng_set(RIGHT_PWM, 0);
+                       printf_P(PSTR("END OF TIME\r\n"));
+                       while(1);
+               }
+       }
+       /* brakes */
+       if (mainboard.flags & DO_POWER)
+               BRAKE_OFF();
+       else
+               BRAKE_ON();
+       cpt++;
+}
+
+void dump_cs_debug(const char *name, struct cs *cs)
+{
+       DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "
+             "in=% .5ld out=% .5ld", 
+             name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+             cs_get_error(cs), cs_get_filtered_feedback(cs),
+             cs_get_out(cs));
+}
+
+void dump_cs(const char *name, struct cs *cs)
+{
+       printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
+                     "in=% .5ld out=% .5ld\r\n"), 
+                name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+                cs_get_error(cs), cs_get_filtered_feedback(cs),
+                cs_get_out(cs));
+}
+
+void dump_pid(const char *name, struct pid_filter *pid)
+{
+       printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+                name,
+                pid_get_value_in(pid) * pid_get_gain_P(pid),
+                pid_get_value_I(pid) * pid_get_gain_I(pid),
+                pid_get_value_D(pid) * pid_get_gain_D(pid),
+                pid_get_value_out(pid));
+}
+
+void microb_cs_init(void)
+{
+       /* ROBOT_SYSTEM */
+       rs_init(&mainboard.rs);
+       rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
+       rs_set_right_pwm(&mainboard.rs,  pwm_set_and_save, RIGHT_PWM);
+       /* increase gain to decrease dist, increase left and it will turn more left */
+       rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+                               LEFT_ENCODER, IMP_COEF * 1.0015);
+       rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+                                RIGHT_ENCODER, IMP_COEF * -1.006);
+       /* rs will use external encoders */
+       rs_set_flags(&mainboard.rs, RS_USE_EXT);
+
+       /* POSITION MANAGER */
+       position_init(&mainboard.pos);
+       position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
+       position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
+       position_set_centrifugal_coef(&mainboard.pos, 0.000016);
+       position_use_ext(&mainboard.pos);
+
+       /* TRAJECTORY MANAGER */
+       trajectory_init(&mainboard.traj);
+       trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
+                         &mainboard.angle.cs);
+       trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
+       trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */
+       /* distance window, angle window, angle start */
+       trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
+
+       /* ---- CS angle */
+       /* PID */
+       pid_init(&mainboard.angle.pid);
+       pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
+       pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095);
+       pid_set_out_shift(&mainboard.angle.pid, 10);
+       pid_set_derivate_filter(&mainboard.angle.pid, 4);
+
+       /* QUADRAMP */
+       quadramp_init(&mainboard.angle.qr);
+       quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */
+       quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */
+
+       /* CS */
+       cs_init(&mainboard.angle.cs);
+       cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr);
+       cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid);
+       cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs);
+       cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs);
+       cs_set_consign(&mainboard.angle.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&mainboard.angle.bd);
+       bd_set_speed_threshold(&mainboard.angle.bd, 80);
+       bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50);
+
+       /* ---- CS distance */
+       /* PID */
+       pid_init(&mainboard.distance.pid);
+       pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
+       pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
+       pid_set_out_shift(&mainboard.distance.pid, 10);
+       pid_set_derivate_filter(&mainboard.distance.pid, 6);
+
+       /* QUADRAMP */
+       quadramp_init(&mainboard.distance.qr);
+       quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */
+       quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */
+
+       /* CS */
+       cs_init(&mainboard.distance.cs);
+       cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr);
+       cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid);
+       cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs);
+       cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs);
+       cs_set_consign(&mainboard.distance.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&mainboard.distance.bd);
+       bd_set_speed_threshold(&mainboard.distance.bd, 60);
+       bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
+
+       /* set them on !! */
+       mainboard.angle.on = 1;
+       mainboard.distance.on = 1;
+
+
+       scheduler_add_periodical_event_priority(do_cs, NULL,
+                                               5000L / SCHEDULER_UNIT,
+                                               CS_PRIO);
+}
diff --git a/projects/microb2010/mainboard/cs.h b/projects/microb2010/mainboard/cs.h
new file mode 100644 (file)
index 0000000..d3d1fa9
--- /dev/null
@@ -0,0 +1,26 @@
+/*  
+ *  Copyright Droids Corporation
+ *  Olivier Matz <zer0@droids-corp.org>
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: cs.h,v 1.3 2009-03-29 18:42:41 zer0 Exp $
+ *
+ */
+
+void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_cs_debug(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
diff --git a/projects/microb2010/mainboard/diagnostic_config.h b/projects/microb2010/mainboard/diagnostic_config.h
new file mode 100644 (file)
index 0000000..9d9c3a5
--- /dev/null
@@ -0,0 +1,44 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: diagnostic_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _DEBUG_CONFIG_
+#define _DEBUG_CONFIG_ 1.0 // version
+
+
+/** port line definition for the show_int_loop() function */
+/* undefine it to disable this functionnality */
+#define INTERRUPT_SHOW_PORT PORTA
+#define INTERRUPT_SHOW_BIT  3
+
+
+
+/** memory mark for the min_stack_space_available() function
+    the ram is filled with this value after a reset ! */
+#define MARK 0x55
+
+/** the mark is inserted in whole RAM if this is enabled 
+    (could lead to problems if you need to hold values through a reset...)
+    so it's better to disable it.
+    stack counting is not affected */
+//#define DIAG_FILL_ENTIRE_RAM
+
+
+#endif //_DEBUG_CONFIG_
diff --git a/projects/microb2010/mainboard/encoders_spi_config.h b/projects/microb2010/mainboard/encoders_spi_config.h
new file mode 100644 (file)
index 0000000..6528244
--- /dev/null
@@ -0,0 +1,33 @@
+/*  
+ *  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: encoders_spi_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $
+ *
+ */
+#ifndef _ENCODERS_SPI_CONFIG_H_
+#define _ENCODERS_SPI_CONFIG_H_
+
+#define ENCODERS_SPI_NUMBER  4
+#define ENCODERS_SPI_SS_PORT SS_PORT /* PORTB on atmega2560 */
+#define ENCODERS_SPI_SS_BIT  SS_BIT  /* 0 on atmega2560 */
+
+/* see spi configuration */
+#define ENCODERS_SPI_CLK_RATE     SPI_CLK_RATE_16
+#define ENCODERS_SPI_FORMAT       SPI_FORMAT_3
+#define ENCODERS_SPI_DATA_ORDER   SPI_LSB_FIRST
+
+#endif
diff --git a/projects/microb2010/mainboard/error_config.h b/projects/microb2010/mainboard/error_config.h
new file mode 100644 (file)
index 0000000..7aad86a
--- /dev/null
@@ -0,0 +1,31 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: error_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _ERROR_CONFIG_
+#define _ERROR_CONFIG_
+
+/** enable the dump of the comment */
+#define ERROR_DUMP_TEXTLOG 
+
+/** enable the dump of filename and line number */
+#define ERROR_DUMP_FILE_LINE
+
+#endif
diff --git a/projects/microb2010/mainboard/i2c_config.h b/projects/microb2010/mainboard/i2c_config.h
new file mode 100644 (file)
index 0000000..1617810
--- /dev/null
@@ -0,0 +1,30 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: i2c_config.h,v 1.2 2009-03-05 23:01:32 zer0 Exp $
+ *
+ */
+
+
+#define I2C_BITRATE 1 // divider dor i2c baudrate, see TWBR in doc 
+#define I2C_PRESCALER 3 // prescaler config, rate = 2^(n*2)
+
+/* Size of transmission buffer */
+#define I2C_SEND_BUFFER_SIZE 32
+
+/* Size of reception buffer */
+#define I2C_RECV_BUFFER_SIZE 32
diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
new file mode 100644 (file)
index 0000000..a503f6d
--- /dev/null
@@ -0,0 +1,687 @@
+/*
+ *  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: i2c_protocol.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <i2c.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include &q