prepare cobboard and ballboard
authorOlivier Matz <zer0@droids-corp.org>
Wed, 17 Mar 2010 22:38:35 +0000 (23:38 +0100)
committerOlivier Matz <zer0@droids-corp.org>
Wed, 17 Mar 2010 22:38:35 +0000 (23:38 +0100)
161 files changed:
modules/comm/uart/uart_defs.h
projects/microb2010/ballboard/.config [new file with mode: 0644]
projects/microb2010/ballboard/Makefile [new file with mode: 0644]
projects/microb2010/ballboard/actuator.c [new file with mode: 0644]
projects/microb2010/ballboard/actuator.h [new file with mode: 0644]
projects/microb2010/ballboard/adc_config.h [new file with mode: 0644]
projects/microb2010/ballboard/ax12_config.h [new file with mode: 0755]
projects/microb2010/ballboard/ax12_user.c [new file with mode: 0644]
projects/microb2010/ballboard/ax12_user.h [new file with mode: 0644]
projects/microb2010/ballboard/cmdline.c [new file with mode: 0644]
projects/microb2010/ballboard/cmdline.h [new file with mode: 0644]
projects/microb2010/ballboard/commands.c [new file with mode: 0644]
projects/microb2010/ballboard/commands_ax12.c [new file with mode: 0644]
projects/microb2010/ballboard/commands_ballboard.c [new file with mode: 0644]
projects/microb2010/ballboard/commands_cs.c [new file with mode: 0644]
projects/microb2010/ballboard/commands_gen.c [new file with mode: 0644]
projects/microb2010/ballboard/cs.c [new file with mode: 0644]
projects/microb2010/ballboard/cs.h [new file with mode: 0644]
projects/microb2010/ballboard/diagnostic_config.h [new file with mode: 0644]
projects/microb2010/ballboard/encoders_spi_config.h [new file with mode: 0644]
projects/microb2010/ballboard/error_config.h [new file with mode: 0644]
projects/microb2010/ballboard/i2c_config.h [new file with mode: 0644]
projects/microb2010/ballboard/i2c_protocol.c [new file with mode: 0644]
projects/microb2010/ballboard/i2c_protocol.h [new file with mode: 0644]
projects/microb2010/ballboard/main.c [new file with mode: 0755]
projects/microb2010/ballboard/main.h [new file with mode: 0755]
projects/microb2010/ballboard/pid_config.h [new file with mode: 0755]
projects/microb2010/ballboard/rdline_config.h [new file with mode: 0644]
projects/microb2010/ballboard/scheduler_config.h [new file with mode: 0755]
projects/microb2010/ballboard/sensor.c [new file with mode: 0644]
projects/microb2010/ballboard/sensor.h [new file with mode: 0644]
projects/microb2010/ballboard/spi_config.h [new file with mode: 0644]
projects/microb2010/ballboard/time_config.h [new file with mode: 0755]
projects/microb2010/ballboard/timer_config.h [new file with mode: 0755]
projects/microb2010/ballboard/uart_config.h [new file with mode: 0644]
projects/microb2010/cobboard/.config [new file with mode: 0644]
projects/microb2010/cobboard/Makefile [new file with mode: 0644]
projects/microb2010/cobboard/actuator.c [new file with mode: 0644]
projects/microb2010/cobboard/actuator.h [new file with mode: 0644]
projects/microb2010/cobboard/adc_config.h [new file with mode: 0644]
projects/microb2010/cobboard/ax12_config.h [new file with mode: 0755]
projects/microb2010/cobboard/ax12_user.c [new file with mode: 0644]
projects/microb2010/cobboard/ax12_user.h [new file with mode: 0644]
projects/microb2010/cobboard/cmdline.c [new file with mode: 0644]
projects/microb2010/cobboard/cmdline.h [new file with mode: 0644]
projects/microb2010/cobboard/commands.c [new file with mode: 0644]
projects/microb2010/cobboard/commands_ax12.c [new file with mode: 0644]
projects/microb2010/cobboard/commands_cobboard.c [new file with mode: 0644]
projects/microb2010/cobboard/commands_cs.c [new file with mode: 0644]
projects/microb2010/cobboard/commands_gen.c [new file with mode: 0644]
projects/microb2010/cobboard/cs.c [new file with mode: 0644]
projects/microb2010/cobboard/cs.h [new file with mode: 0644]
projects/microb2010/cobboard/diagnostic_config.h [new file with mode: 0644]
projects/microb2010/cobboard/encoders_spi_config.h [new file with mode: 0644]
projects/microb2010/cobboard/error_config.h [new file with mode: 0644]
projects/microb2010/cobboard/i2c_config.h [new file with mode: 0644]
projects/microb2010/cobboard/i2c_protocol.c [new file with mode: 0644]
projects/microb2010/cobboard/i2c_protocol.h [new file with mode: 0644]
projects/microb2010/cobboard/main.c [new file with mode: 0755]
projects/microb2010/cobboard/main.h [new file with mode: 0755]
projects/microb2010/cobboard/pid_config.h [new file with mode: 0755]
projects/microb2010/cobboard/rdline_config.h [new file with mode: 0644]
projects/microb2010/cobboard/scheduler_config.h [new file with mode: 0755]
projects/microb2010/cobboard/sensor.c [new file with mode: 0644]
projects/microb2010/cobboard/sensor.h [new file with mode: 0644]
projects/microb2010/cobboard/spi_config.h [new file with mode: 0644]
projects/microb2010/cobboard/state.c [new file with mode: 0644]
projects/microb2010/cobboard/state.h [new file with mode: 0644]
projects/microb2010/cobboard/time_config.h [new file with mode: 0755]
projects/microb2010/cobboard/timer_config.h [new file with mode: 0755]
projects/microb2010/cobboard/uart_config.h [new file with mode: 0644]
projects/microb2010/common/eeprom_mapping.h
projects/microb2010/common/i2c_commands.h
projects/microb2010/mechboard/.config [deleted file]
projects/microb2010/mechboard/Makefile [deleted file]
projects/microb2010/mechboard/actuator.c [deleted file]
projects/microb2010/mechboard/actuator.h [deleted file]
projects/microb2010/mechboard/adc_config.h [deleted file]
projects/microb2010/mechboard/arm_highlevel.c [deleted file]
projects/microb2010/mechboard/arm_highlevel.h [deleted file]
projects/microb2010/mechboard/arm_xy.c [deleted file]
projects/microb2010/mechboard/arm_xy.h [deleted file]
projects/microb2010/mechboard/ax12_config.h [deleted file]
projects/microb2010/mechboard/ax12_user.c [deleted file]
projects/microb2010/mechboard/ax12_user.h [deleted file]
projects/microb2010/mechboard/cmdline.c [deleted file]
projects/microb2010/mechboard/cmdline.h [deleted file]
projects/microb2010/mechboard/commands.c [deleted file]
projects/microb2010/mechboard/commands_ax12.c [deleted file]
projects/microb2010/mechboard/commands_cs.c [deleted file]
projects/microb2010/mechboard/commands_gen.c [deleted file]
projects/microb2010/mechboard/commands_mechboard.c [deleted file]
projects/microb2010/mechboard/cs.c [deleted file]
projects/microb2010/mechboard/cs.h [deleted file]
projects/microb2010/mechboard/diagnostic_config.h [deleted file]
projects/microb2010/mechboard/encoders_spi_config.h [deleted file]
projects/microb2010/mechboard/error_config.h [deleted file]
projects/microb2010/mechboard/i2c_config.h [deleted file]
projects/microb2010/mechboard/i2c_protocol.c [deleted file]
projects/microb2010/mechboard/i2c_protocol.h [deleted file]
projects/microb2010/mechboard/main.c [deleted file]
projects/microb2010/mechboard/main.h [deleted file]
projects/microb2010/mechboard/pid_config.h [deleted file]
projects/microb2010/mechboard/rdline_config.h [deleted file]
projects/microb2010/mechboard/scheduler_config.h [deleted file]
projects/microb2010/mechboard/sensor.c [deleted file]
projects/microb2010/mechboard/sensor.h [deleted file]
projects/microb2010/mechboard/spi_config.h [deleted file]
projects/microb2010/mechboard/state.c [deleted file]
projects/microb2010/mechboard/state.h [deleted file]
projects/microb2010/mechboard/time_config.h [deleted file]
projects/microb2010/mechboard/timer_config.h [deleted file]
projects/microb2010/mechboard/uart_config.h [deleted file]
projects/microb2010/sensorboard/.config [deleted file]
projects/microb2010/sensorboard/Makefile [deleted file]
projects/microb2010/sensorboard/actuator.c [deleted file]
projects/microb2010/sensorboard/actuator.h [deleted file]
projects/microb2010/sensorboard/adc_config.h [deleted file]
projects/microb2010/sensorboard/ax12_config.h [deleted file]
projects/microb2010/sensorboard/ax12_user.c [deleted file]
projects/microb2010/sensorboard/ax12_user.h [deleted file]
projects/microb2010/sensorboard/beacon.c [deleted file]
projects/microb2010/sensorboard/beacon.h [deleted file]
projects/microb2010/sensorboard/cmdline.c [deleted file]
projects/microb2010/sensorboard/cmdline.h [deleted file]
projects/microb2010/sensorboard/commands.c [deleted file]
projects/microb2010/sensorboard/commands_ax12.c [deleted file]
projects/microb2010/sensorboard/commands_cs.c [deleted file]
projects/microb2010/sensorboard/commands_gen.c [deleted file]
projects/microb2010/sensorboard/commands_scan.c [deleted file]
projects/microb2010/sensorboard/commands_sensorboard.c [deleted file]
projects/microb2010/sensorboard/cs.c [deleted file]
projects/microb2010/sensorboard/cs.h [deleted file]
projects/microb2010/sensorboard/diagnostic_config.h [deleted file]
projects/microb2010/sensorboard/encoders_spi_config.h [deleted file]
projects/microb2010/sensorboard/error_config.h [deleted file]
projects/microb2010/sensorboard/gen_scan_tab.c [deleted file]
projects/microb2010/sensorboard/i2c_config.h [deleted file]
projects/microb2010/sensorboard/i2c_protocol.c [deleted file]
projects/microb2010/sensorboard/i2c_protocol.h [deleted file]
projects/microb2010/sensorboard/img_processing.c [deleted file]
projects/microb2010/sensorboard/img_processing.h [deleted file]
projects/microb2010/sensorboard/main.c [deleted file]
projects/microb2010/sensorboard/main.h [deleted file]
projects/microb2010/sensorboard/pid_config.h [deleted file]
projects/microb2010/sensorboard/rdline_config.h [deleted file]
projects/microb2010/sensorboard/scanner.c [deleted file]
projects/microb2010/sensorboard/scanner.h [deleted file]
projects/microb2010/sensorboard/scheduler_config.h [deleted file]
projects/microb2010/sensorboard/sensor.c [deleted file]
projects/microb2010/sensorboard/sensor.h [deleted file]
projects/microb2010/sensorboard/spi_config.h [deleted file]
projects/microb2010/sensorboard/time_config.h [deleted file]
projects/microb2010/sensorboard/timer_config.h [deleted file]
projects/microb2010/sensorboard/uart_config.h [deleted file]
projects/microb2010/tests/beacon_tsop/Makefile
projects/microb2010/tests/beacon_tsop/main.c
projects/microb2010/tests/beacon_tsop/main.h
projects/microb2010/tests/beacon_tsop/trigo.c
projects/microb2010/tests/beacon_tsop/trigo.h
projects/microb2010/tests/static_beacon/static_beacon.c

index e37de8c..7d872a1 100644 (file)
 #ifndef TXC \r
 #define TXC TXC0\r
 #endif\r
+#ifndef RXC \r
+#define RXC RXC0\r
+#endif\r
 #ifndef RXB8\r
 #define RXB8 RXB80\r
 #endif\r
diff --git a/projects/microb2010/ballboard/.config b/projects/microb2010/ballboard/.config
new file mode 100644 (file)
index 0000000..0d59101
--- /dev/null
@@ -0,0 +1,282 @@
+#
+# 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=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
+#
+
+#
+# Enable math library in generation options to see all modules
+#
+CONFIG_MODULE_CIRBUF=y
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_HOSTSIM is not set
+CONFIG_MODULE_SCHEDULER=y
+# CONFIG_MODULE_SCHEDULER_STATS is not set
+# 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
+#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
+CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
+CONFIG_MODULE_UART_CREATE_CONFIG=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 (fixed point lib may be needed)
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+
+#
+# Filters
+#
+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
+#
+
+#
+# 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=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/ballboard/Makefile b/projects/microb2010/ballboard/Makefile
new file mode 100644 (file)
index 0000000..afa396b
--- /dev/null
@@ -0,0 +1,43 @@
+TARGET = main
+
+# repertoire des modules
+AVERSIVE_DIR = ../../..
+# VALUE, absolute or relative path : example ../.. #
+
+CFLAGS += -Werror
+LDFLAGS = -T ../common/avr6.x
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c 
+SRC += commands_cs.c commands_ballboard.c commands.c
+SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.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
+
+scan_h_l.h:
+       @gcc gen_scan_tab.c -o gen_scan_tab -lm;                \
+       if ./gen_scan_tab > /dev/null 2>&1; then        \
+               echo ok;                                \
+       else                                            \
+               echo nok;                               \
+       fi
+
+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/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
new file mode 100644 (file)
index 0000000..367e241
--- /dev/null
@@ -0,0 +1,44 @@
+/*  
+ *  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.2 2009-04-24 19:30:42 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+
+#include "main.h"
+
diff --git a/projects/microb2010/ballboard/actuator.h b/projects/microb2010/ballboard/actuator.h
new file mode 100644 (file)
index 0000000..a6320be
--- /dev/null
@@ -0,0 +1,21 @@
+/*  
+ *  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:42 zer0 Exp $
+ *
+ */
+
diff --git a/projects/microb2010/ballboard/adc_config.h b/projects/microb2010/ballboard/adc_config.h
new file mode 100644 (file)
index 0000000..e69de29
diff --git a/projects/microb2010/ballboard/ax12_config.h b/projects/microb2010/ballboard/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/ballboard/ax12_user.c b/projects/microb2010/ballboard/ax12_user.c
new file mode 100644 (file)
index 0000000..e3a5f9f
--- /dev/null
@@ -0,0 +1,169 @@
+/*  
+ *  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.2 2009-04-07 20:03:48 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 <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_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/ballboard/ax12_user.h b/projects/microb2010/ballboard/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/ballboard/cmdline.c b/projects/microb2010/ballboard/cmdline.c
new file mode 100644 (file)
index 0000000..6f8ff46
--- /dev/null
@@ -0,0 +1,149 @@
+/*  
+ *  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.2 2009-04-07 20:03:48 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 <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include "main.h"
+#include "cmdline.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;
+       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;
+       
+       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));
+       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), "ballboard > ");       
+       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/ballboard/cmdline.h b/projects/microb2010/ballboard/cmdline.h
new file mode 100644 (file)
index 0000000..57dfa49
--- /dev/null
@@ -0,0 +1,40 @@
+/*  
+ *  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.2 2009-05-27 20:04:07 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 uint8_t cmdline_getchar(void) {
+       return uart_recv_nowait(CMDLINE_UART);
+}
diff --git a/projects/microb2010/ballboard/commands.c b/projects/microb2010/ballboard/commands.c
new file mode 100644 (file)
index 0000000..9ab33ad
--- /dev/null
@@ -0,0 +1,111 @@
+/*
+ *  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.2 2009-05-27 20:04:07 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_ballboard.c */
+extern parse_pgm_inst_t cmd_event;
+extern parse_pgm_inst_t cmd_test;
+
+/* 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_ballboard.c */
+       (parse_pgm_inst_t *)&cmd_event,
+       (parse_pgm_inst_t *)&cmd_test,
+
+       NULL,
+};
diff --git a/projects/microb2010/ballboard/commands_ax12.c b/projects/microb2010/ballboard/commands_ax12.c
new file mode 100644 (file)
index 0000000..2fda225
--- /dev/null
@@ -0,0 +1,368 @@
+/*
+ *  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.1 2009-03-29 18:44:54 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 <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_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/ballboard/commands_ballboard.c b/projects/microb2010/ballboard/commands_ballboard.c
new file mode 100644 (file)
index 0000000..03dc830
--- /dev/null
@@ -0,0 +1,197 @@
+/*
+ *  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_ballboard.c,v 1.2 2009-04-24 19:30:42 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 <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "../common/i2c_commands.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_BD | DO_POWER;
+               if (!strcmp_P(res->arg2, PSTR("on")))
+                       ballboard.flags |= bit;
+               else if (!strcmp_P(res->arg2, PSTR("off")))
+                       ballboard.flags &= bit;
+               else { /* show */
+                       printf_P(PSTR("encoders is %s\r\n"), 
+                                (DO_ENCODERS & ballboard.flags) ? "on":"off");
+                       printf_P(PSTR("cs is %s\r\n"), 
+                                (DO_CS & ballboard.flags) ? "on":"off");
+                       printf_P(PSTR("bd is %s\r\n"), 
+                                (DO_BD & ballboard.flags) ? "on":"off");
+                       printf_P(PSTR("power is %s\r\n"), 
+                                (DO_POWER & ballboard.flags) ? "on":"off");
+               }
+               return;
+       }
+
+       if (!strcmp_P(res->arg1, PSTR("encoders")))
+               bit = DO_ENCODERS;
+       else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+               bit = DO_CS;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("bd")))
+               bit = DO_BD;
+       else if (!strcmp_P(res->arg1, PSTR("power")))
+               bit = DO_POWER;
+
+
+       if (!strcmp_P(res->arg2, PSTR("on")))
+               ballboard.flags |= bit;
+       else if (!strcmp_P(res->arg2, PSTR("off"))) {
+               if (!strcmp_P(res->arg1, PSTR("cs"))) {
+                       pwm_ng_set(ROLLER_PWM, 0);
+                       pwm_ng_set(FORKTRANS_PWM, 0);
+                       pwm_ng_set(FORKROT_PWM, 0);
+               }
+               ballboard.flags &= (~bit);
+       }
+       printf_P(PSTR("%s is %s\r\n"), res->arg1, 
+                     (bit & ballboard.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#bd#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,
+       },
+};
+
+/**********************************************************/
+/* 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"))) {
+               ballboard.our_color = I2C_COLOR_RED;
+       }
+       else if (!strcmp_P(res->color, PSTR("green"))) {
+               ballboard.our_color = 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,
+       },
+};
+
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+       fixed_string_t arg0;
+};
+
+/* 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;
+       
+}
+
+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);
+
+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, 
+               NULL,
+       },
+};
diff --git a/projects/microb2010/ballboard/commands_cs.c b/projects/microb2010/ballboard/commands_cs.c
new file mode 100644 (file)
index 0000000..098ffb3
--- /dev/null
@@ -0,0 +1,672 @@
+/*
+ *  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.1 2009-03-29 18:44:54 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 <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_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_roller_str[] = "roller";
+prog_char csb_forktrans_str[] = "forktrans";
+prog_char csb_forkrot_str[] = "forkrot";
+struct csb_list csb_list[] = {
+       { .name = csb_roller_str, .csb = &ballboard.roller },
+       { .name = csb_forktrans_str, .csb = &ballboard.forktrans },
+       { .name = csb_forkrot_str, .csb = &ballboard.forkrot },
+};
+
+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[] = "roller#forktrans#forkrot";
+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/ballboard/commands_gen.c b/projects/microb2010/ballboard/commands_gen.c
new file mode 100644 (file)
index 0000000..b2a1e5d
--- /dev/null
@@ -0,0 +1,573 @@
+/*
+ *  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.4 2009-05-27 20:04:07 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 <clock_time.h>
+#include <encoders_spi.h>
+#include <adc.h>
+
+#include <scheduler.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_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();
+}
+
+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 i2c_log[] = "i2c";
+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 beacon_log[] = "beacon";
+static const prog_char scanner_log[] = "scanner";
+static const prog_char imgprocess_log[] = "imgprocess";
+
+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 },
+       { i2c_log, E_I2C },
+       { i2cproto_log, E_USER_I2C_PROTO },
+       { sensor_log, E_USER_SENSOR },
+       { block_log, E_BLOCKING_DETECTION_MANAGER },
+       { beacon_log, E_USER_BEACON },
+       { scanner_log, E_USER_SCANNER },
+       { imgprocess_log, E_USER_IMGPROCESS },
+};
+
+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#i2c#i2cproto#sensor#bd#beacon#scanner#imgprocess";
+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/ballboard/cs.c b/projects/microb2010/ballboard/cs.c
new file mode 100644 (file)
index 0000000..7e87fca
--- /dev/null
@@ -0,0 +1,183 @@
+/*  
+ *  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.4 2009-05-27 20:04:07 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 <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "actuator.h"
+
+int32_t encoders_spi_update_roller_speed(void *number)
+{
+       static volatile int32_t roller_pos;
+       int32_t tmp, speed;
+       tmp = encoders_spi_get_value(number);
+       speed = tmp - roller_pos;
+       roller_pos = tmp;
+       return speed;
+}
+
+/* called every 5 ms */
+static void do_cs(void *dummy) 
+{
+       /* read encoders */
+       if (ballboard.flags & DO_ENCODERS) {
+               encoders_spi_manage(NULL);
+       }
+       /* control system */
+       if (ballboard.flags & DO_CS) {
+               if (ballboard.roller.on)
+                       cs_manage(&ballboard.roller.cs);
+               if (ballboard.forktrans.on)
+                       cs_manage(&ballboard.forktrans.cs);
+               if (ballboard.forkrot.on)
+                       cs_manage(&ballboard.forkrot.cs);
+       }
+       if (ballboard.flags & DO_BD) {
+               bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs);
+               bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
+               bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs);
+       }
+       if (ballboard.flags & DO_POWER)
+               BRAKE_OFF();
+       else
+               BRAKE_ON();
+}
+
+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)
+{
+       /* ---- CS roller */
+       /* PID */
+       pid_init(&ballboard.roller.pid);
+       pid_set_gains(&ballboard.roller.pid, 80, 80, 250);
+       pid_set_maximums(&ballboard.roller.pid, 0, 10000, 2000);
+       pid_set_out_shift(&ballboard.roller.pid, 6);
+       pid_set_derivate_filter(&ballboard.roller.pid, 6);
+
+       /* CS */
+       cs_init(&ballboard.roller.cs);
+       cs_set_correct_filter(&ballboard.roller.cs, pid_do_filter, &ballboard.roller.pid);
+       cs_set_process_in(&ballboard.roller.cs, pwm_ng_set, ROLLER_PWM);
+       cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER);
+       cs_set_consign(&ballboard.roller.cs, 0);
+
+       /* ---- CS forktrans */
+       /* PID */
+       pid_init(&ballboard.forktrans.pid);
+       pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250);
+       pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047);
+       pid_set_out_shift(&ballboard.forktrans.pid, 6);
+       pid_set_derivate_filter(&ballboard.forktrans.pid, 6);
+
+       /* QUADRAMP */
+       quadramp_init(&ballboard.forktrans.qr);
+       quadramp_set_1st_order_vars(&ballboard.forktrans.qr, 200, 200); /* set speed */
+       quadramp_set_2nd_order_vars(&ballboard.forktrans.qr, 20, 20); /* set accel */
+
+       /* CS */
+       cs_init(&ballboard.forktrans.cs);
+       cs_set_consign_filter(&ballboard.forktrans.cs, quadramp_do_filter, &ballboard.forktrans.qr);
+       cs_set_correct_filter(&ballboard.forktrans.cs, pid_do_filter, &ballboard.forktrans.pid);
+       cs_set_process_in(&ballboard.forktrans.cs, pwm_ng_set, FORKTRANS_PWM);
+       cs_set_process_out(&ballboard.forktrans.cs, encoders_spi_get_value, FORKTRANS_ENCODER);
+       cs_set_consign(&ballboard.forktrans.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&ballboard.forktrans.bd);
+       bd_set_speed_threshold(&ballboard.forktrans.bd, 150);
+       bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40);
+
+       /* ---- CS forkrot */
+       /* PID */
+       pid_init(&ballboard.forkrot.pid);
+       pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250);
+       pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047);
+       pid_set_out_shift(&ballboard.forkrot.pid, 6);
+       pid_set_derivate_filter(&ballboard.forkrot.pid, 6);
+
+       /* QUADRAMP */
+       quadramp_init(&ballboard.forkrot.qr);
+       quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */
+       quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */
+
+       /* CS */
+       cs_init(&ballboard.forkrot.cs);
+       cs_set_consign_filter(&ballboard.forkrot.cs, quadramp_do_filter, &ballboard.forkrot.qr);
+       cs_set_correct_filter(&ballboard.forkrot.cs, pid_do_filter, &ballboard.forkrot.pid);
+       cs_set_process_in(&ballboard.forkrot.cs, pwm_ng_set, FORKROT_PWM);
+       cs_set_process_out(&ballboard.forkrot.cs, encoders_spi_get_value, FORKROT_ENCODER);
+       cs_set_consign(&ballboard.forkrot.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&ballboard.forkrot.bd);
+       bd_set_speed_threshold(&ballboard.forkrot.bd, 150);
+       bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40);
+
+       /* set them on !! */
+       ballboard.roller.on = 0;
+       ballboard.forktrans.on = 1;
+       ballboard.forkrot.on = 1;
+
+
+       scheduler_add_periodical_event_priority(do_cs, NULL, 
+                                               5000L / SCHEDULER_UNIT, 
+                                               CS_PRIO);
+}
diff --git a/projects/microb2010/ballboard/cs.h b/projects/microb2010/ballboard/cs.h
new file mode 100644 (file)
index 0000000..fe50c80
--- /dev/null
@@ -0,0 +1,25 @@
+/*  
+ *  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.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
diff --git a/projects/microb2010/ballboard/diagnostic_config.h b/projects/microb2010/ballboard/diagnostic_config.h
new file mode 100644 (file)
index 0000000..e4ebb75
--- /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-03-29 18:44:54 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/ballboard/encoders_spi_config.h b/projects/microb2010/ballboard/encoders_spi_config.h
new file mode 100644 (file)
index 0000000..6a4a68b
--- /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-03-29 18:44:54 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/ballboard/error_config.h b/projects/microb2010/ballboard/error_config.h
new file mode 100644 (file)
index 0000000..15ca2fc
--- /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-03-29 18:44:54 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/ballboard/i2c_config.h b/projects/microb2010/ballboard/i2c_config.h
new file mode 100644 (file)
index 0000000..ebc0678
--- /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.1 2009-03-29 18:44:54 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/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c
new file mode 100644 (file)
index 0000000..8995e55
--- /dev/null
@@ -0,0 +1,147 @@
+/*
+ *  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_protocol.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/error.h>
+
+#include <scheduler.h>
+
+#include <i2c.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "actuator.h"
+
+void i2c_protocol_init(void)
+{
+}
+
+/*** LED CONTROL ***/
+void i2c_led_control(uint8_t l, uint8_t state)
+{
+       switch(l) {
+       case 1:
+               state? LED1_ON():LED1_OFF();
+               break;
+       case 2:
+               state? LED2_ON():LED2_OFF();
+               break;
+       default:
+               break;
+       }
+}
+
+void i2c_send_status(void)
+{
+       struct i2c_ans_ballboard_status ans;
+       i2c_flush();
+       ans.hdr.cmd =  I2C_ANS_BALLBOARD_STATUS;
+       ans.status = 0x55; /* XXX */
+
+       i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
+                sizeof(ans), I2C_CTRL_GENERIC);
+}
+
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+       void *void_cmd = buf;
+       static uint8_t a=0;
+       a=!a;
+       if (a)
+               LED2_ON();
+       else
+               LED2_OFF();
+
+       if (size <= 0) {
+               goto error;
+       }
+       
+       switch (buf[0]) {
+
+       /* Commands (no answer needed) */
+       case I2C_CMD_GENERIC_LED_CONTROL: 
+               {
+                       struct i2c_cmd_led_control *cmd = void_cmd;
+                       if (size != sizeof (*cmd))
+                               goto error;
+                       i2c_led_control(cmd->led_num, cmd->state);
+                       break;
+               }
+               
+       case I2C_CMD_GENERIC_SET_COLOR:
+               {
+                       struct i2c_cmd_generic_color *cmd = void_cmd;
+                       if (size != sizeof (*cmd))
+                               goto error;
+                       ballboard.our_color = cmd->color;
+                       break;
+               }
+
+
+       /* Add other commands here ...*/
+
+
+       case I2C_REQ_BALLBOARD_STATUS:
+               {
+                       struct i2c_req_ballboard_status *cmd = void_cmd;                        
+                       if (size != sizeof (*cmd))
+                               goto error;
+                               
+                       i2c_send_status();
+                       break;
+               }
+
+       default:
+               goto error;
+       }
+
+ error:
+       /* log error on a led ? */
+       return;
+}
+
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
+{
+}
+
+void i2c_sendevent(int8_t size)
+{
+}
+
+
diff --git a/projects/microb2010/ballboard/i2c_protocol.h b/projects/microb2010/ballboard/i2c_protocol.h
new file mode 100644 (file)
index 0000000..6fcac74
--- /dev/null
@@ -0,0 +1,30 @@
+/*
+ *  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_protocol.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+
+void i2c_protocol_init(void);
+
+void i2c_recvevent(uint8_t * buf, int8_t size);
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
+void i2c_sendevent(int8_t size);
+
+int debug_send(char c, FILE* f);
diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c
new file mode 100755 (executable)
index 0000000..46a3b52
--- /dev/null
@@ -0,0 +1,252 @@
+/*  
+ *  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:07 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <avr/eeprom.h>
+
+#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 <i2c.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "../common/eeprom_mapping.h"
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+#include "ax12_user.h"
+#include "cmdline.h"
+#include "sensor.h"
+#include "actuator.h"
+#include "cs.h"
+#include "i2c_protocol.h"
+#include "beacon.h"
+#include "scanner.h"
+
+/* 0 means "programmed"
+ * ---- with 16 Mhz quartz
+ * CKSEL 3-0 : 0111
+ * SUT 1-0 : 10 
+ * CKDIV8 : 1
+ * ---- bootloader
+ * BOOTZ 1-0 : 01 (4K bootloader)
+ * BOOTRST : 0 (reset on bootloader)
+ * ---- jtag
+ * jtagen : 0
+ */
+
+struct genboard gen;
+struct ballboard ballboard;
+
+/***********************/
+
+void bootloader(void)
+{
+#define BOOTLOADER_ADDR 0x3f000
+       if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
+               printf_P(PSTR("Bootloader is not present\r\n"));
+               return;
+       }
+       cli();
+       BRAKE_ON();
+       /* ... very specific :( */
+       TIMSK0 = 0;
+       TIMSK1 = 0;
+       TIMSK2 = 0;
+       TIMSK3 = 0;
+       TIMSK4 = 0;
+       TIMSK5 = 0;
+       EIMSK = 0;
+       UCSR0B = 0;
+       UCSR1B = 0;
+       UCSR2B = 0;
+       UCSR3B = 0;
+       SPCR = 0;
+       TWCR = 0;
+       ACSR = 0;
+       ADCSRA = 0;
+
+       EIND = 1;
+       __asm__ __volatile__ ("ldi r31,0xf8\n");
+       __asm__ __volatile__ ("ldi r30,0x00\n");
+       __asm__ __volatile__ ("eijmp\n");
+       
+       /* never returns */
+}
+
+void do_led_blink(__attribute__((unused)) void *dummy)
+{
+#if 1 /* simple blink */
+       static uint8_t a=0;
+
+       if(a)
+               LED1_ON();
+       else
+               LED1_OFF();
+       
+       a = !a;
+#endif
+}
+
+static void main_timer_interrupt(void)
+{
+       static uint8_t cpt = 0;
+       cpt++;
+       sei();
+       if ((cpt & 0x3) == 0)
+               scheduler_interrupt();
+}
+
+int main(void)
+{
+       /* brake */
+       BRAKE_OFF();
+       BRAKE_DDR();
+
+       /* CPLD reset on PG3 */
+       DDRG |= 1<<3;
+       PORTG &= ~(1<<3); /* implicit */
+
+       /* LEDS */
+       DDRJ |= 0x0c;
+       DDRL = 0xc0;
+       LED1_OFF();
+       memset(&gen, 0, sizeof(gen));
+       memset(&ballboard, 0, sizeof(ballboard));
+       ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD
+       
+       /* UART */
+       uart_init();
+#if CMDLINE_UART == 3
+       fdevopen(uart3_dev_send, uart3_dev_recv);
+       uart_register_rx_event(3, emergency);
+#elif CMDLINE_UART == 1
+       fdevopen(uart1_dev_send, uart1_dev_recv);
+       uart_register_rx_event(1, emergency);
+#else
+#  error not supported
+#endif
+
+       //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD);
+       /* check eeprom to avoid to run the bad program */
+       if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
+           EEPROM_MAGIC_BALLBOARD) {
+               sei();
+               printf_P(PSTR("Bad eeprom value\r\n"));
+               while(1);
+       }
+
+       /* LOGS */
+       error_register_emerg(mylog);
+       error_register_error(mylog);
+       error_register_warning(mylog);
+       error_register_notice(mylog);
+       error_register_debug(mylog);
+
+       /* SPI + ENCODERS */
+       encoders_spi_init(); /* this will also init spi hardware */
+
+       /* I2C */
+       i2c_protocol_init();
+       i2c_init(I2C_MODE_SLAVE, I2C_BALLBOARD_ADDR);
+       i2c_register_recv_event(i2c_recvevent);
+
+       /* TIMER */
+       timer_init();
+       timer0_register_OV_intr(main_timer_interrupt);
+
+       /* PWM */
+       PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, 
+                                TIMER1_PRESCALER_DIV_1);
+       PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, 
+                                TIMER4_PRESCALER_DIV_1);
+       
+       PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | 
+                     PWM_NG_MODE_SIGN_INVERTED,
+                     &PORTD, 4);
+       PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
+                     &PORTD, 5);
+       PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
+                     &PORTD, 6);
+       PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
+                     &PORTD, 7);
+
+
+       /* servos */
+       PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10, 
+                                TIMER1_PRESCALER_DIV_256);
+       PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10, 
+                                TIMER1_PRESCALER_DIV_256);
+       PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       
+       /* SCHEDULER */
+       scheduler_init();
+
+       scheduler_add_periodical_event_priority(do_led_blink, NULL, 
+                                               100000L / SCHEDULER_UNIT, 
+                                               LED_PRIO);
+       /* all cs management */
+       microb_cs_init();
+
+       /* sensors, will also init hardware adc */
+       sensor_init();
+
+       /* TIME */
+       time_init(TIME_PRIO);
+
+       /* ax12 */
+       ax12_user_init();
+
+       sei();
+
+       printf_P(PSTR("\r\n"));
+       printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
+       cmdline_interact();
+
+       return 0;
+}
diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h
new file mode 100755 (executable)
index 0000000..36915bd
--- /dev/null
@@ -0,0 +1,146 @@
+/*  
+ *  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: main.h,v 1.4 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#define LED_TOGGLE(port, bit) do {             \
+               if (port & _BV(bit))            \
+                       port &= ~_BV(bit);      \
+               else                            \
+                       port |= _BV(bit);       \
+       } while(0)
+
+#define LED1_ON()      sbi(PORTJ, 2)
+#define LED1_OFF()     cbi(PORTJ, 2)
+#define LED1_TOGGLE()  LED_TOGGLE(PORTJ, 2)
+
+#define LED2_ON()      sbi(PORTL, 7)
+#define LED2_OFF()     cbi(PORTL, 7)
+#define LED2_TOGGLE()  LED_TOGGLE(PORTL, 7)
+
+#define LED3_ON()      sbi(PORTJ, 3)
+#define LED3_OFF()     cbi(PORTJ, 3)
+#define LED3_TOGGLE()  LED_TOGGLE(PORTJ, 3)
+
+#define LED4_ON()      sbi(PORTL, 6)
+#define LED4_OFF()     cbi(PORTL, 6)
+#define LED4_TOGGLE()  LED_TOGGLE(PORTL, 6)
+
+#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 ROLLER_ENCODER    ((void *)0)
+#define FORKTRANS_ENCODER ((void *)1)
+#define FORKROT_ENCODER   ((void *)2)
+
+#define ROLLER_PWM     ((void *)&gen.pwm1_4A)
+#define FORKTRANS_PWM  ((void *)&gen.pwm2_4B)
+#define FORKROT_PWM    ((void *)&gen.pwm3_1A)
+#define XXX_PWM        ((void *)&gen.pwm4_1B)
+
+#define BALL_PRESENT_SENSOR  2 /* XXX dummy example */
+
+/** ERROR NUMS */
+#define E_USER_I2C_PROTO       195
+#define E_USER_SENSOR          196
+#define E_USER_BEACON          197
+#define E_USER_SCANNER         198
+#define E_USER_IMGPROCESS      199
+
+#define LED_PRIO           170
+#define TIME_PRIO          160
+#define ADC_PRIO           120
+#define CS_PRIO            100
+#define I2C_POLL_PRIO       20
+
+#define CS_PERIOD 5000L
+
+#define NB_LOGS 4
+
+/* generic to all boards */
+struct genboard {
+       /* command line interface */
+       struct rdline rdl;
+       char prompt[RDLINE_PROMPT_SIZE];
+
+       /* motors */
+       struct pwm_ng pwm1_4A;
+       struct pwm_ng pwm2_4B;
+       struct pwm_ng pwm3_1A;
+       struct pwm_ng pwm4_1B;
+
+       /* servos */
+       struct pwm_ng servo1;
+       struct pwm_ng servo2;
+       struct pwm_ng servo3;
+       struct pwm_ng servo4;
+       
+       /* ax12 interface */
+       AX12 ax12;
+
+       /* log */
+       uint8_t logs[NB_LOGS+1];
+       uint8_t log_level;
+       uint8_t debug;
+};
+
+struct cs_block {
+       uint8_t on;
+        struct cs cs;
+        struct pid_filter pid;
+       struct quadramp_filter qr;
+       struct blocking_detection bd;
+};
+
+/* ballboard specific */
+struct ballboard {
+#define DO_ENCODERS  1
+#define DO_CS        2
+#define DO_BD        4
+#define DO_POWER     8
+       uint8_t flags;                /* misc flags */
+
+       /* control systems */
+        struct cs_block roller;
+        struct cs_block forktrans;
+        struct cs_block forkrot;
+
+       /* robot status */
+       uint8_t our_color;
+};
+
+extern struct genboard gen;
+extern struct ballboard ballboard;
+
+/* start the bootloader */
+void bootloader(void);
+
+#define wait_cond_or_timeout(cond, timeout)                   \
+({                                                            \
+        microseconds __us = time_get_us2();                   \
+        uint8_t __ret = 1;                                    \
+        while(! (cond)) {                                     \
+                if (time_get_us2() - __us > (timeout)*1000L) {\
+                        __ret = 0;                            \
+                        break;                                \
+                }                                             \
+        }                                                     \
+        __ret;                                                \
+})
diff --git a/projects/microb2010/ballboard/pid_config.h b/projects/microb2010/ballboard/pid_config.h
new file mode 100755 (executable)
index 0000000..fa95f08
--- /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
+ *
+ * 
+ *
+ */
+
+#ifndef PID_CONFIG_H
+#define PID_CONFIG_H
+
+/** the derivate term can be filtered to remove the noise. This value
+ * is the maxium sample count to keep in memory to do this
+ * filtering. For an instance of pid, this count is defined o*/
+#define PID_DERIVATE_FILTER_MAX_SIZE 4
+
+#endif
diff --git a/projects/microb2010/ballboard/rdline_config.h b/projects/microb2010/ballboard/rdline_config.h
new file mode 100644 (file)
index 0000000..e69de29
diff --git a/projects/microb2010/ballboard/scheduler_config.h b/projects/microb2010/ballboard/scheduler_config.h
new file mode 100755 (executable)
index 0000000..2547a24
--- /dev/null
@@ -0,0 +1,47 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: scheduler_config.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#ifndef _SCHEDULER_CONFIG_H_
+#define _SCHEDULER_CONFIG_H_
+
+#define _SCHEDULER_CONFIG_VERSION_ 4
+
+/** maximum number of allocated events */
+#define SCHEDULER_NB_MAX_EVENT 8
+
+
+#define SCHEDULER_UNIT_FLOAT 512.0
+#define SCHEDULER_UNIT 512L
+
+/** number of allowed imbricated scheduler interrupts. The maximum
+ * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate
+ * more than once per event. If it is less, it can avoid to browse the
+ * event table, events are delayed (we loose precision) but it takes
+ * less CPU */
+#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT
+
+/** define it for debug infos (not recommended, because very slow on
+ *  an AVR, it uses printf in an interrupt). It can be useful if
+ *  prescaler is very high, making the timer interrupt period very
+ *  long in comparison to printf() */
+/* #define SCHEDULER_DEBUG */
+
+#endif // _SCHEDULER_CONFIG_H_
diff --git a/projects/microb2010/ballboard/sensor.c b/projects/microb2010/ballboard/sensor.c
new file mode 100644 (file)
index 0000000..6cfb491
--- /dev/null
@@ -0,0 +1,258 @@
+/*  
+ *  Copyright Droids Corporation (2009)
+ *  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: sensor.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#include <stdlib.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <adc.h>
+#include <scheduler.h>
+#include <ax12.h>
+#include <pwm_ng.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "sensor.h"
+
+/************ ADC */
+
+struct adc_infos {
+       uint16_t config;
+       int16_t value;
+       int16_t prev_val;
+        int16_t (*filter)(struct adc_infos *, int16_t);
+};
+
+/* reach 90% of the value in 4 samples */
+int16_t rii_light(struct adc_infos *adc, int16_t val)
+{
+       adc->prev_val = val + (int32_t)adc->prev_val / 2;
+       return adc->prev_val / 2;
+}
+
+/* reach 90% of the value in 8 samples */
+int16_t rii_medium(struct adc_infos *adc, int16_t val)
+{
+       adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4;
+       return adc->prev_val / 4;
+}
+
+/* reach 90% of the value in 16 samples */
+int16_t rii_strong(struct adc_infos *adc, int16_t val)
+{
+       adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8;
+       return adc->prev_val / 8;
+}
+
+
+#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
+
+/* define which ADC to poll, see in sensor.h */
+static struct adc_infos adc_infos[ADC_MAX] = { 
+  
+       [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
+       [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
+       [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
+       [ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium },
+       /* add adc on "cap" pins if needed */
+/*     [ADC_CAP1] = { .config = ADC_CONF(10) }, */
+/*     [ADC_CAP2] = { .config = ADC_CONF(11) }, */
+/*     [ADC_CAP3] = { .config = ADC_CONF(12) }, */
+/*     [ADC_CAP4] = { .config = ADC_CONF(13) }, */
+};
+
+static void adc_event(int16_t result);
+
+/* called every 10 ms, see init below */
+static void do_adc(void *dummy) 
+{
+       /* launch first conversion */
+       adc_launch(adc_infos[0].config);
+}
+
+static void adc_event(int16_t result)
+{
+       static uint8_t i = 0;
+
+       /* filter value if needed */
+       if (adc_infos[i].filter)
+               adc_infos[i].value = adc_infos[i].filter(&adc_infos[i],
+                                                        result);
+       else
+               adc_infos[i].value = result;
+
+       i ++;
+       if (i >= ADC_MAX)
+               i = 0;
+       else
+               adc_launch(adc_infos[i].config);
+}
+
+int16_t sensor_get_adc(uint8_t i)
+{
+       int16_t tmp;
+       uint8_t flags;
+
+       IRQ_LOCK(flags);
+       tmp = adc_infos[i].value;
+       IRQ_UNLOCK(flags);
+       return tmp;
+}
+
+/************ boolean sensors */
+
+
+struct sensor_filter {
+       uint8_t filter;
+       uint8_t prev;
+       uint8_t thres_off;
+       uint8_t thres_on;
+       uint8_t cpt;
+       uint8_t invert;
+};
+
+/* pullup mapping:
+ * CAP 5,6,7,8
+ * voltage div mapping:
+ * CAP 1
+ */
+static struct sensor_filter sensor_filter[SENSOR_MAX] = {
+       [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */
+       [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
+       [S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */
+       [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
+       [S_CAP5] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
+       [S_CAP6] = { 1, 0, 0, 1, 0, 0 }, /* 5 */
+       [S_CAP7] = { 1, 0, 0, 1, 0, 0 }, /* 6 */
+       [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
+       [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
+       [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
+       [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */
+       [S_RESERVED4] = { 1, 0, 0, 1, 0, 0 }, /* 11 */
+       [S_RESERVED5] = { 1, 0, 0, 1, 0, 0 }, /* 12 */
+       [S_RESERVED6] = { 1, 0, 0, 1, 0, 0 }, /* 13 */
+       [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */
+       [S_RESERVED8] = { 1, 0, 0, 1, 0, 0 }, /* 15 */
+};
+
+/* value of filtered sensors */
+static uint16_t sensor_filtered = 0;
+
+/* sensor mapping : 
+ * 0-3:  PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
+ * 4-5:  PORTL 0->1 (cap5 -> cap6)
+ * 6-7:  PORTE 3->4 (cap7 -> cap8)
+ * 8-15: reserved
+ */
+
+uint16_t sensor_get_all(void)
+{
+       uint16_t tmp;
+       uint8_t flags;
+       IRQ_LOCK(flags);
+       tmp = sensor_filtered;
+       IRQ_UNLOCK(flags);
+       return tmp;
+}
+
+uint8_t sensor_get(uint8_t i)
+{
+       uint16_t tmp = sensor_get_all();
+       return (tmp & _BV(i));
+}
+
+/* get the physical value of pins */
+static uint16_t sensor_read(void)
+{
+       uint16_t tmp = 0;
+       tmp |= (uint16_t)((PINK & (_BV(2)|_BV(3)|_BV(4)|_BV(5))) >> 2) << 0;
+       tmp |= (uint16_t)((PINL & (_BV(0)|_BV(1))) >> 0) << 4;
+       tmp |= (uint16_t)((PINE & (_BV(3)|_BV(4))) >> 3) << 6;
+       /* add reserved sensors here */
+       return tmp;
+}
+
+/* called every 10 ms, see init below */
+static void do_boolean_sensors(void *dummy)
+{
+       uint8_t i;
+       uint8_t flags;
+       uint16_t sensor = sensor_read();
+       uint16_t tmp = 0;
+
+       for (i=0; i<SENSOR_MAX; i++) {
+               if ((1 << i) & sensor) {
+                       if (sensor_filter[i].cpt < sensor_filter[i].filter)
+                               sensor_filter[i].cpt++;
+                       if (sensor_filter[i].cpt >= sensor_filter[i].thres_on)
+                               sensor_filter[i].prev = 1;
+               }
+               else {
+                       if (sensor_filter[i].cpt > 0)
+                               sensor_filter[i].cpt--;
+                       if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
+                               sensor_filter[i].prev = 0;
+               }
+               
+               if (sensor_filter[i].prev) {
+                       tmp |= (1UL << i);
+               }
+       }
+       IRQ_LOCK(flags);
+       sensor_filtered = tmp;
+       IRQ_UNLOCK(flags);
+}
+
+
+
+/************ global sensor init */
+#define BACKGROUND_ADC  0
+
+/* called every 10 ms, see init below */
+static void do_sensors(void *dummy)
+{
+       if (BACKGROUND_ADC)
+         do_adc(NULL);
+       do_boolean_sensors(NULL);
+}
+
+void sensor_init(void)
+{
+       adc_init();
+       if (BACKGROUND_ADC)
+         adc_register_event(adc_event);
+       /* CS EVENT */
+       scheduler_add_periodical_event_priority(do_sensors, NULL, 
+                                               10000L / SCHEDULER_UNIT, 
+                                               ADC_PRIO);
+
+}
+
diff --git a/projects/microb2010/ballboard/sensor.h b/projects/microb2010/ballboard/sensor.h
new file mode 100644 (file)
index 0000000..c2b18a7
--- /dev/null
@@ -0,0 +1,56 @@
+/*  
+ *  Copyright Droids Corporation (2009)
+ *  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: sensor.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+/* synchronize with sensor.c */
+#define ADC_CSENSE1   0
+#define ADC_CSENSE2   1
+#define ADC_CSENSE3   2
+#define ADC_CSENSE4   3
+#define ADC_MAX       4
+
+/* synchronize with sensor.c */
+#define S_CAP1         0
+#define S_CAP2         1
+#define S_CAP3         2
+#define S_CAP4         3
+#define S_CAP5         4
+#define S_CAP6         5
+#define S_CAP7         6
+#define S_CAP8         7
+#define S_RESERVED1    8
+#define S_RESERVED2    9
+#define S_RESERVED3   10
+#define S_RESERVED4   11
+#define S_RESERVED5   12
+#define S_RESERVED6   13
+#define S_RESERVED7   14
+#define S_RESERVED8   15
+#define SENSOR_MAX    16
+
+void sensor_init(void);
+
+/* get filtered values for adc */
+int16_t sensor_get_adc(uint8_t i);
+
+/* get filtered values of boolean sensors */
+uint16_t sensor_get_all(void);
+uint8_t sensor_get(uint8_t i);
diff --git a/projects/microb2010/ballboard/spi_config.h b/projects/microb2010/ballboard/spi_config.h
new file mode 100644 (file)
index 0000000..76697c3
--- /dev/null
@@ -0,0 +1,36 @@
+/*  
+ *  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
+ *
+ */
+
+/*
+ * Author : Julien LE GUEN - jlg@jleguen.info
+ */
+
+
+/*
+ *     Configure HERE your SPI module
+ */
+
+
+
+/* Number of slave devices in your system
+ * Each slave have a dedicated SS line that you have to register
+ * before using the SPI module
+ */
+#define SPI_MAX_SLAVES 1
+
diff --git a/projects/microb2010/ballboard/time_config.h b/projects/microb2010/ballboard/time_config.h
new file mode 100755 (executable)
index 0000000..e0f7f2a
--- /dev/null
@@ -0,0 +1,23 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: time_config.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+/** precision of the time processor, in us */
+#define TIME_PRECISION 25000l
diff --git a/projects/microb2010/ballboard/timer_config.h b/projects/microb2010/ballboard/timer_config.h
new file mode 100755 (executable)
index 0000000..810525c
--- /dev/null
@@ -0,0 +1,36 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2006)
+ * 
+ *  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: timer_config.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+#define TIMER0_ENABLED
+
+/* #define TIMER1_ENABLED */
+/* #define TIMER1A_ENABLED */
+/* #define TIMER1B_ENABLED */
+/* #define TIMER1C_ENABLED */
+
+/* #define TIMER2_ENABLED */
+
+/* #define TIMER3_ENABLED */
+/* #define TIMER3A_ENABLED */
+/* #define TIMER3B_ENABLED */
+/* #define TIMER3C_ENABLED */
+
+#define TIMER0_PRESCALER_DIV 8
diff --git a/projects/microb2010/ballboard/uart_config.h b/projects/microb2010/ballboard/uart_config.h
new file mode 100644 (file)
index 0000000..2a27e07
--- /dev/null
@@ -0,0 +1,102 @@
+/*  \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:07 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
+/*\r
+ * UART1 definitions \r
+ */\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
+/* \r
+ * if you enable this, the maximum baudrate you can reach is \r
+ * higher, but the precision is lower. \r
+ */\r
+#define UART1_USE_DOUBLE_SPEED 1\r
+\r
+#define UART1_RX_FIFO_SIZE 64\r
+#define UART1_TX_FIFO_SIZE 64\r
+#define UART1_NBITS 8\r
+\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+\r
+\r
+/*\r
+ * UART3 definitions \r
+ */\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
+/* \r
+ * if you enable this, the maximum baudrate you can reach is \r
+ * higher, but the precision is lower. \r
+ */\r
+#define UART3_USE_DOUBLE_SPEED 1\r
+//#define UART3_USE_DOUBLE_SPEED 1\r
+\r
+#define UART3_RX_FIFO_SIZE 64\r
+#define UART3_TX_FIFO_SIZE 64\r
+//#define UART3_NBITS 5\r
+//#define UART3_NBITS 6\r
+//#define UART3_NBITS 7\r
+#define UART3_NBITS 8\r
+//#define UART3_NBITS 9\r
+\r
+#define UART3_PARITY UART_PARTITY_NONE\r
+//#define UART3_PARITY UART_PARTITY_ODD\r
+//#define UART3_PARITY UART_PARTITY_EVEN\r
+\r
+#define UART3_STOP_BIT UART_STOP_BITS_1\r
+//#define UART3_STOP_BIT UART_STOP_BITS_2\r
+\r
+\r
+\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+#endif\r
+\r
diff --git a/projects/microb2010/cobboard/.config b/projects/microb2010/cobboard/.config
new file mode 100644 (file)
index 0000000..706895c
--- /dev/null
@@ -0,0 +1,282 @@
+#
+# 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=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
+#
+
+#
+# Enable math library in generation options to see all modules
+#
+CONFIG_MODULE_CIRBUF=y
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+# CONFIG_MODULE_GEOMETRY is not set
+# CONFIG_MODULE_HOSTSIM is not set
+CONFIG_MODULE_SCHEDULER=y
+# CONFIG_MODULE_SCHEDULER_STATS is not set
+# 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
+#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
+CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
+CONFIG_MODULE_UART_CREATE_CONFIG=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 (fixed point lib may be needed)
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+
+#
+# Filters
+#
+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
+#
+
+#
+# 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=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/cobboard/Makefile b/projects/microb2010/cobboard/Makefile
new file mode 100644 (file)
index 0000000..0c93238
--- /dev/null
@@ -0,0 +1,35 @@
+TARGET = main
+
+# repertoire des modules
+AVERSIVE_DIR = ../../..
+# VALUE, absolute or relative path : example ../.. #
+
+CFLAGS += -Werror -Wextra
+LDFLAGS = -T ../common/avr6.x
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c 
+SRC += commands_cs.c commands_cobboard.c commands.c
+SRC += i2c_protocol.c sensor.c actuator.c cs.c
+SRC += state.c ax12_user.c
+
+# 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
+
+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/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c
new file mode 100644 (file)
index 0000000..67598ea
--- /dev/null
@@ -0,0 +1,67 @@
+/*  
+ *  Copyright Droids Corporation (2009)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+
+#include "../common/i2c_commands.h"
+#include "actuator.h"
+#include "ax12_user.h"
+#include "main.h"
+
+void servo_carry_open(void)
+{
+}
+
+void servo_carry_close(void)
+{
+}
+
+void servo_door_open(void)
+{
+}
+
+void servo_door_close(void)
+{
+}
+
+void actuator_init(void)
+{
+
+}
diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h
new file mode 100644 (file)
index 0000000..cd2e28b
--- /dev/null
@@ -0,0 +1,27 @@
+/*  
+ *  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.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+void actuator_init(void);
+void servo_carry_open(void);
+void servo_carry_close(void);
+void servo_door_open(void);
+void servo_door_close(void);
+
diff --git a/projects/microb2010/cobboard/adc_config.h b/projects/microb2010/cobboard/adc_config.h
new file mode 100644 (file)
index 0000000..e69de29
diff --git a/projects/microb2010/cobboard/ax12_config.h b/projects/microb2010/cobboard/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/cobboard/ax12_user.c b/projects/microb2010/cobboard/ax12_user.c
new file mode 100644 (file)
index 0000000..b022ef7
--- /dev/null
@@ -0,0 +1,305 @@
+/*  
+ *  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.4 2009-04-24 19:30:42 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <i2c.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "ax12_user.h"
+
+#define AX12_ERROR(args...) ERROR(E_USER_AX12, args)
+#define AX12_NOTICE(args...) NOTICE(E_USER_AX12, args)
+#define AX12_MAX_TRIES 3
+
+/*
+ * 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 UCSRxB UCSR0B
+#define AX12_TIMEOUT 15000L /* in us */
+
+static uint32_t ax12_stats_ops = 0;   /* total ops */
+static uint32_t ax12_stats_fails = 0; /* number of fails */
+static uint32_t ax12_stats_drops = 0; /* number of drops (3 fails) */
+static uint32_t ax12_dropped_logs = 0; /* error messages that were not displayed */
+static microseconds t_prev_msg = 0;
+
+/********************************* 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(__attribute__((unused)) 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);
+       }
+}
+
+/* ----- */ 
+
+/* log rate limit */
+static void ax12_print_error(uint8_t err, uint16_t line)
+{
+       microseconds t2;
+       
+       /* no more than 1 log per sec */
+       t2 = time_get_us2();
+
+       if (t2 - t_prev_msg < 1000000L) {
+               ax12_dropped_logs++;
+               return;
+       }
+       AX12_ERROR("AX12 error %x at line %d (%ld messages dropped)",
+                  err, line, ax12_dropped_logs);
+       ax12_dropped_logs = 0;
+       t_prev_msg = t2;
+}
+
+uint8_t __ax12_user_write_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                              uint8_t data, uint16_t line)
+{
+       uint8_t err, i;
+
+       ax12_stats_ops++;
+
+       for (i=0; i<AX12_MAX_TRIES ; i++) {
+               err = AX12_write_byte(ax12, id, address, data);
+               if (err == 0)
+                       break;
+               wait_ms(2); /* BAD HACK XXX */
+               ax12_stats_fails++;
+       }
+       if (err == 0)
+               return 0;
+
+       ax12_print_error(err, line);
+       ax12_stats_drops++;
+       return err;
+}
+
+uint8_t __ax12_user_write_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                           uint16_t data, uint16_t line)
+{
+       uint8_t err, i;
+
+       ax12_stats_ops++;
+
+       for (i=0; i<AX12_MAX_TRIES ; i++) {
+               err = AX12_write_int(ax12, id, address, data);
+               if (err == 0)
+                       break;
+               wait_ms(2); /* BAD HACK XXX */
+               ax12_stats_fails++;
+       }
+       if (err == 0)
+               return 0;
+
+       ax12_print_error(err, line);
+       ax12_stats_drops++;
+       return err;
+}
+
+uint8_t __ax12_user_read_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                           uint8_t *val, uint16_t line)
+{
+       uint8_t err, i;
+
+       ax12_stats_ops++;
+
+       for (i=0; i<AX12_MAX_TRIES ; i++) {
+               err = AX12_read_byte(ax12, id, address, val);
+               if (err == 0)
+                       break;
+               wait_ms(2); /* BAD HACK XXX */
+               ax12_stats_fails++;
+       }
+       if (err == 0) {
+               /* XXX hack for broadcast */
+               if (id == AX12_BROADCAST_ID)
+                       wait_ms(1);
+               return 0;
+       }
+
+       ax12_print_error(err, line);
+       ax12_stats_drops++;
+       return err;
+}
+
+uint8_t __ax12_user_read_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                          uint16_t *val, uint16_t line)
+{
+       uint8_t err, i;
+
+       ax12_stats_ops++;
+
+       for (i=0; i<AX12_MAX_TRIES ; i++) {
+               err = AX12_read_int(ax12, id, address, val);
+               if (err == 0)
+                       break;
+               wait_ms(2); /* BAD HACK XXX */
+               ax12_stats_fails++;
+       }
+       if (err == 0) {
+               /* XXX hack for broadcast */
+               if (id == AX12_BROADCAST_ID)
+                       wait_ms(1);
+               return 0;
+       }
+
+       ax12_print_error(err, line);
+       ax12_stats_drops++;
+       return err;
+}
+
+void ax12_dump_stats(void)
+{
+       printf_P(PSTR("AX12 stats:\r\n"));
+       printf_P(PSTR("  total ops:   %ld\r\n"), ax12_stats_ops);
+       printf_P(PSTR("  total fails: %ld\r\n"), ax12_stats_fails);
+       printf_P(PSTR("  total drops: %ld\r\n"), ax12_stats_drops);
+       printf_P(PSTR("  logs dropped since last message: %ld\r\n"), ax12_dropped_logs);
+}
+
+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);
+       t_prev_msg = time_get_us2();
+}
diff --git a/projects/microb2010/cobboard/ax12_user.h b/projects/microb2010/cobboard/ax12_user.h
new file mode 100644 (file)
index 0000000..bbd7283
--- /dev/null
@@ -0,0 +1,76 @@
+/*  
+ *  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.3 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. */
+
+#define UART_AX12_NUM 0
+
+void ax12_user_init(void);
+
+void ax12_dump_stats(void);
+
+#define ax12_user_write_byte(ax12, id, addr, data)             \
+       __ax12_user_write_byte(ax12, id, addr, data, __LINE__)
+
+#define ax12_user_write_int(ax12, id, addr, data)              \
+       __ax12_user_write_int(ax12, id, addr, data, __LINE__)
+
+#define ax12_user_read_byte(ax12, id, addr, data)              \
+       __ax12_user_read_byte(ax12, id, addr, data, __LINE__)
+
+#define ax12_user_read_int(ax12, id, addr, data)               \
+       __ax12_user_read_int(ax12, id, addr, data, __LINE__)
+
+/** @brief Write byte in AX-12 memory 
+ * @return Error code from AX-12 (0 means okay) */
+uint8_t __ax12_user_write_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                              uint8_t data, uint16_t line);
+
+/** @brief Write integer (2 bytes) in AX-12 memory 
+ * @return Error code from AX-12 (0 means okay)
+ *
+ * address   : data low
+ * address+1 : data high
+ */
+uint8_t __ax12_user_write_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                             uint16_t data, uint16_t line);
+
+/** @brief Read byte from AX-12 memory 
+ * @return Error code from AX-12 (0 means okay) */
+uint8_t __ax12_user_read_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                             uint8_t *val, uint16_t line);
+
+/** @brief Write integer (2 bytes) from AX-12 memory
+ * @return Error code from AX-12 (0 means okay) */
+uint8_t __ax12_user_read_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+                            uint16_t *val, uint16_t line);
diff --git a/projects/microb2010/cobboard/cmdline.c b/projects/microb2010/cobboard/cmdline.c
new file mode 100644 (file)
index 0000000..6de7569
--- /dev/null
@@ -0,0 +1,149 @@
+/*  
+ *  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.3 2009-04-24 19:30:42 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 <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include "main.h"
+#include "cmdline.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, __attribute__((unused)) uint8_t size) 
+{
+       int8_t ret;
+       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;
+       
+       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));
+       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), "cobboard > ");        
+       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/cobboard/cmdline.h b/projects/microb2010/cobboard/cmdline.h
new file mode 100644 (file)
index 0000000..e5bb569
--- /dev/null
@@ -0,0 +1,40 @@
+/*  
+ *  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:25:00 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);
+}
diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c
new file mode 100644 (file)
index 0000000..1723f2a
--- /dev/null
@@ -0,0 +1,132 @@
+/*
+ *  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.6 2009-11-08 17:25:00 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;
+extern parse_pgm_inst_t cmd_ax12_stress;
+extern parse_pgm_inst_t cmd_ax12_dump_stats;
+
+/* 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_cobboard.c */
+extern parse_pgm_inst_t cmd_event;
+extern parse_pgm_inst_t cmd_color;
+extern parse_pgm_inst_t cmd_state1;
+extern parse_pgm_inst_t cmd_state2;
+extern parse_pgm_inst_t cmd_state3;
+extern parse_pgm_inst_t cmd_state_debug;
+extern parse_pgm_inst_t cmd_state_machine;
+extern parse_pgm_inst_t cmd_servo_door;
+extern parse_pgm_inst_t cmd_servo_carry;
+extern parse_pgm_inst_t cmd_test;
+
+
+/* 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,
+       (parse_pgm_inst_t *)&cmd_ax12_stress,
+       (parse_pgm_inst_t *)&cmd_ax12_dump_stats,
+
+       /* 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_cobboard.c */
+       (parse_pgm_inst_t *)&cmd_event,
+       (parse_pgm_inst_t *)&cmd_color,
+       (parse_pgm_inst_t *)&cmd_state1,
+       (parse_pgm_inst_t *)&cmd_state2,
+       (parse_pgm_inst_t *)&cmd_state3,
+       (parse_pgm_inst_t *)&cmd_state_debug,
+       (parse_pgm_inst_t *)&cmd_state_machine,
+       (parse_pgm_inst_t *)&cmd_servo_door,
+       (parse_pgm_inst_t *)&cmd_servo_carry,
+       (parse_pgm_inst_t *)&cmd_test,
+
+       NULL,
+};
diff --git a/projects/microb2010/cobboard/commands_ax12.c b/projects/microb2010/cobboard/commands_ax12.c
new file mode 100644 (file)
index 0000000..ebe3834
--- /dev/null
@@ -0,0 +1,427 @@
+/*
+ *  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-04-24 19:30:42 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 <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "ax12_user.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, __attribute__((unused)) void *data)
+{
+       struct cmd_ax12_stress_result *res = parsed_result;
+       int i, nb_errs, id;
+       uint8_t val;
+       microseconds t;
+
+       t = time_get_us2();
+       nb_errs = 0;
+       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);
+
+       t = time_get_us2();
+       nb_errs = 0;
+       for (i=0; i<1000; i++) {
+               if (AX12_write_int(&gen.ax12, res->id, AA_GOAL_POSITION_L, 500))
+                       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);
+
+       /* test 5 servos */
+       t = time_get_us2();
+       nb_errs = 0;
+       id = 1;
+       for (i=0; i<100; i++) {
+               if (AX12_write_int(&gen.ax12, id, AA_GOAL_POSITION_L, 500))
+                       nb_errs ++;
+               id ++;
+               if (id > 5)
+                       id = 1;
+       }
+
+       printf_P(PSTR("%d errors / 100\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 (id)";
+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,
+       },
+};
+
+/**********************************************************/
+/* Ax12_Dump_Stats */
+
+/* this structure is filled when cmd_ax12_dump_stats is parsed successfully */
+struct cmd_ax12_dump_stats_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_ax12_dump_stats is parsed successfully */
+static void cmd_ax12_dump_stats_parsed(__attribute__((unused)) void *parsed_result,
+                                      __attribute__((unused)) void *data)
+{
+       ax12_dump_stats();
+}
+
+prog_char str_ax12_dump_stats_arg0[] = "ax12_dump_stats";
+parse_pgm_token_string_t cmd_ax12_dump_stats_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_dump_stats_result, arg0, str_ax12_dump_stats_arg0);
+
+prog_char help_ax12_dump_stats[] = "Dump AX12 stats";
+parse_pgm_inst_t cmd_ax12_dump_stats = {
+       .f = cmd_ax12_dump_stats_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_ax12_dump_stats,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_ax12_dump_stats_arg0, 
+               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, __attribute__((unused)) void *data)
+{
+       struct cmd_baudrate_result *res = parsed_result;
+       struct uart_config c;
+
+       uart_getconf(UART_AX12_NUM, &c);
+       c.baudrate = res->arg1;
+       uart_setconf(UART_AX12_NUM, &c);
+}
+
+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, __attribute__((unused)) 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_user_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, __attribute__((unused)) 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_user_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, __attribute__((unused)) 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_user_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, __attribute__((unused)) 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_user_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/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
new file mode 100644 (file)
index 0000000..b822e4a
--- /dev/null
@@ -0,0 +1,477 @@
+/*
+ *  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_cobboard.c,v 1.6 2009-11-08 17:25:00 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 <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "sensor.h"
+#include "cmdline.h"
+#include "state.h"
+#include "i2c_protocol.h"
+#include "actuator.h"
+#include "actuator.h"
+
+extern uint16_t state_debug;
+
+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, __attribute__((unused)) 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_BD | DO_POWER;
+               if (!strcmp_P(res->arg2, PSTR("on")))
+                       cobboard.flags |= bit;
+               else if (!strcmp_P(res->arg2, PSTR("off")))
+                       cobboard.flags &= bit;
+               else { /* show */
+                       printf_P(PSTR("encoders is %s\r\n"), 
+                                (DO_ENCODERS & cobboard.flags) ? "on":"off");
+                       printf_P(PSTR("cs is %s\r\n"), 
+                                (DO_CS & cobboard.flags) ? "on":"off");
+                       printf_P(PSTR("bd is %s\r\n"), 
+                                (DO_BD & cobboard.flags) ? "on":"off");
+                       printf_P(PSTR("power is %s\r\n"), 
+                                (DO_POWER & cobboard.flags) ? "on":"off");
+               }
+               return;
+       }
+
+       if (!strcmp_P(res->arg1, PSTR("encoders")))
+               bit = DO_ENCODERS;
+       else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+               bit = DO_CS;
+       }
+       else if (!strcmp_P(res->arg1, PSTR("bd")))
+               bit = DO_BD;
+       else if (!strcmp_P(res->arg1, PSTR("power")))
+               bit = DO_POWER;
+
+
+       if (!strcmp_P(res->arg2, PSTR("on")))
+               cobboard.flags |= bit;
+       else if (!strcmp_P(res->arg2, PSTR("off"))) {
+               if (!strcmp_P(res->arg1, PSTR("cs"))) {
+                       pwm_ng_set(LEFT_SPICKLE_PWM, 0);
+                       pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
+                       pwm_ng_set(SHOVEL_PWM, 0);
+               }
+               cobboard.flags &= (~bit);
+       }
+       printf_P(PSTR("%s is %s\r\n"), res->arg1, 
+                     (bit & cobboard.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#bd#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,
+       },
+};
+
+/**********************************************************/
+/* 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, __attribute__((unused)) void *data)
+{
+       struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
+       if (!strcmp_P(res->color, PSTR("red"))) {
+               cobboard.our_color = I2C_COLOR_RED;
+       }
+       else if (!strcmp_P(res->color, PSTR("green"))) {
+               cobboard.our_color = 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,
+       },
+};
+
+/**********************************************************/
+/* State1 */
+
+/* this structure is filled when cmd_state1 is parsed successfully */
+struct cmd_state1_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_state1 is parsed successfully */
+static void cmd_state1_parsed(void *parsed_result,
+                             __attribute__((unused)) void *data)
+{
+       struct cmd_state1_result *res = parsed_result;
+       struct i2c_cmd_cobboard_set_mode command;
+
+       if (!strcmp_P(res->arg1, PSTR("init"))) {
+               state_init();
+               return;
+       }
+
+       if (!strcmp_P(res->arg1, PSTR("manual")))
+               command.mode = I2C_COBBOARD_MODE_MANUAL;
+       else if (!strcmp_P(res->arg1, PSTR("harvest")))
+               command.mode = I2C_COBBOARD_MODE_HARVEST;
+       state_set_mode(&command);
+}
+
+prog_char str_state1_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
+prog_char str_state1_arg1[] = "init#manual";
+parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
+
+prog_char help_state1[] = "set cobboard mode";
+parse_pgm_inst_t cmd_state1 = {
+       .f = cmd_state1_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_state1,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_state1_arg0, 
+               (prog_void *)&cmd_state1_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* State2 */
+
+/* this structure is filled when cmd_state2 is parsed successfully */
+struct cmd_state2_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       fixed_string_t arg2;
+};
+
+/* function called when cmd_state2 is parsed successfully */
+static void cmd_state2_parsed(void *parsed_result,
+                             __attribute__((unused)) void *data)
+{
+       struct cmd_state2_result *res = parsed_result;
+       struct i2c_cmd_cobboard_set_mode command;
+       uint8_t side;
+
+       if (!strcmp_P(res->arg2, PSTR("left")))
+               side = I2C_LEFT_SIDE;
+       else if (!strcmp_P(res->arg2, PSTR("right")))
+               side = I2C_RIGHT_SIDE;
+
+       if (!strcmp_P(res->arg1, PSTR("yyy"))) {
+       }
+       else if (!strcmp_P(res->arg1, PSTR("xxx"))) {
+       }
+
+
+       state_set_mode(&command);
+}
+
+prog_char str_state2_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
+prog_char str_state2_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
+prog_char str_state2_arg2[] = "left#right";
+parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
+
+prog_char help_state2[] = "set cobboard mode";
+parse_pgm_inst_t cmd_state2 = {
+       .f = cmd_state2_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_state2,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_state2_arg0, 
+               (prog_void *)&cmd_state2_arg1, 
+               (prog_void *)&cmd_state2_arg2, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* State3 */
+
+/* this structure is filled when cmd_state3 is parsed successfully */
+struct cmd_state3_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint8_t arg2;
+};
+
+/* function called when cmd_state3 is parsed successfully */
+static void cmd_state3_parsed(void *parsed_result, 
+                             __attribute__((unused)) void *data)
+{
+       struct cmd_state3_result *res = parsed_result;
+       struct i2c_cmd_cobboard_set_mode command;
+
+       if (!strcmp_P(res->arg1, PSTR("xxx"))) {
+               /* xxx = res->arg2 */
+       }
+       else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
+       }
+       state_set_mode(&command);
+}
+
+prog_char str_state3_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
+prog_char str_state3_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
+parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8);
+
+prog_char help_state3[] = "set cobboard mode";
+parse_pgm_inst_t cmd_state3 = {
+       .f = cmd_state3_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_state3,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_state3_arg0, 
+               (prog_void *)&cmd_state3_arg1, 
+               (prog_void *)&cmd_state3_arg2, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* State_Machine */
+
+/* this structure is filled when cmd_state_machine is parsed successfully */
+struct cmd_state_machine_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_state_machine is parsed successfully */
+static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
+                                    __attribute__((unused)) void *data)
+{
+       state_machine();
+}
+
+prog_char str_state_machine_arg0[] = "state_machine";
+parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
+
+prog_char help_state_machine[] = "launch state machine";
+parse_pgm_inst_t cmd_state_machine = {
+       .f = cmd_state_machine_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_state_machine,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_state_machine_arg0, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* State_Debug */
+
+/* this structure is filled when cmd_state_debug is parsed successfully */
+struct cmd_state_debug_result {
+       fixed_string_t arg0;
+       uint8_t on;
+};
+
+/* function called when cmd_state_debug is parsed successfully */
+static void cmd_state_debug_parsed(void *parsed_result,
+                                  __attribute__((unused)) void *data)
+{
+       struct cmd_state_debug_result *res = parsed_result;
+       state_debug = res->on;
+}
+
+prog_char str_state_debug_arg0[] = "state_debug";
+parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
+parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
+
+prog_char help_state_debug[] = "Set debug timer for state machine";
+parse_pgm_inst_t cmd_state_debug = {
+       .f = cmd_state_debug_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_state_debug,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_state_debug_arg0, 
+               (prog_void *)&cmd_state_debug_on, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Servo_Door */
+
+/* this structure is filled when cmd_servo_door is parsed successfully */
+struct cmd_servo_door_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_door is parsed successfully */
+static void cmd_servo_door_parsed(void *parsed_result,
+                                   __attribute__((unused)) void *data)
+{
+       struct cmd_servo_door_result *res = parsed_result;
+       if (!strcmp_P(res->arg1, PSTR("open")))
+               servo_door_open();
+       else if (!strcmp_P(res->arg1, PSTR("closed")))
+               servo_door_close();
+}
+
+prog_char str_servo_door_arg0[] = "door";
+parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0);
+prog_char str_servo_door_arg1[] = "open#closed";
+parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1);
+
+prog_char help_servo_door[] = "Servo door function";
+parse_pgm_inst_t cmd_servo_door = {
+       .f = cmd_servo_door_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_servo_door,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_servo_door_arg0, 
+               (prog_void *)&cmd_servo_door_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Servo_Carry */
+
+/* this structure is filled when cmd_servo_carry is parsed successfully */
+struct cmd_servo_carry_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_carry is parsed successfully */
+static void cmd_servo_carry_parsed(void *parsed_result,
+                                   __attribute__((unused)) void *data)
+{
+       struct cmd_servo_carry_result *res = parsed_result;
+       if (!strcmp_P(res->arg1, PSTR("open")))
+               servo_carry_open();
+       else if (!strcmp_P(res->arg1, PSTR("closed")))
+               servo_carry_close();
+}
+
+prog_char str_servo_carry_arg0[] = "carry";
+parse_pgm_token_string_t cmd_servo_carry_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg0, str_servo_carry_arg0);
+prog_char str_servo_carry_arg1[] = "open#closed";
+parse_pgm_token_string_t cmd_servo_carry_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg1, str_servo_carry_arg1);
+
+prog_char help_servo_carry[] = "Servo carry function";
+parse_pgm_inst_t cmd_servo_carry = {
+       .f = cmd_servo_carry_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_servo_carry,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_servo_carry_arg0, 
+               (prog_void *)&cmd_servo_carry_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(__attribute__((unused)) void *parsed_result,
+                           __attribute__((unused)) void *data)
+{
+}
+
+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);
+
+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, 
+               NULL,
+       },
+};
diff --git a/projects/microb2010/cobboard/commands_cs.c b/projects/microb2010/cobboard/commands_cs.c
new file mode 100644 (file)
index 0000000..27be783
--- /dev/null
@@ -0,0 +1,672 @@
+/*
+ *  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.2 2009-04-07 20:03:48 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 <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_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_left_spickle_str[] = "left_spickle";
+prog_char csb_right_spickle_str[] = "right_spickle";
+prog_char csb_shovel_str[] = "shovel";
+struct csb_list csb_list[] = {
+       { .name = csb_left_spickle_str, .csb = &cobboard.left_spickle },
+       { .name = csb_right_spickle_str, .csb = &cobboard.right_spickle },
+       { .name = csb_shovel_str, .csb = &cobboard.shovel },
+};
+
+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[] = "left_spickle#right_spickle#shovel";
+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)
+{
+       unsigned 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, __attribute__((unused)) 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, __attribute__((unused)) 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, __attribute__((unused)) 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/cobboard/commands_gen.c b/projects/microb2010/cobboard/commands_gen.c
new file mode 100644 (file)
index 0000000..393f874
--- /dev/null
@@ -0,0 +1,575 @@
+/*
+ *  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.5 2009-11-08 17:25:00 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 <clock_time.h>
+#include <encoders_spi.h>
+#include <adc.h>
+
+#include <scheduler.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_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(__attribute__((unused)) void *parsed_result,
+                            __attribute__((unused)) 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(__attribute__((unused)) void *parsed_result,
+                                 __attribute__((unused)) 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, __attribute__((unused)) 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( __attribute__((unused)) void *parsed_result,
+                                 __attribute__((unused)) void *data)
+{
+       scheduler_dump_events();
+}
+
+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, __attribute__((unused)) 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, __attribute__((unused)) 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, __attribute__((unused)) 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 i2c_log[] = "i2c";
+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 state_log[] = "state";
+static const prog_char ax12_log[] = "ax12";
+
+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 },
+       { i2c_log, E_I2C },
+       { i2cproto_log, E_USER_I2C_PROTO },
+       { sensor_log, E_USER_SENSOR },
+       { block_log, E_BLOCKING_DETECTION_MANAGER },
+       { state_log, E_USER_ST_MACH },
+       { ax12_log, E_USER_AX12 },
+};
+
+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, __attribute__((unused)) 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, __attribute__((unused)) 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#i2c#i2cproto#sensor#bd##state#ax12";
+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(__attribute__((unused)) void *parsed_result,
+                                  __attribute__((unused)) 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/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
new file mode 100644 (file)
index 0000000..4d8ccb0
--- /dev/null
@@ -0,0 +1,192 @@
+/*  
+ *  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.4 2009-04-24 19:30:42 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 <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "actuator.h"
+
+/* called every 5 ms */
+static void do_cs(__attribute__((unused)) void *dummy) 
+{
+       /* read encoders */
+       if (cobboard.flags & DO_ENCODERS) {
+               encoders_spi_manage(NULL);
+       }
+       /* control system */
+       if (cobboard.flags & DO_CS) {
+               if (cobboard.left_spickle.on)
+                       cs_manage(&cobboard.left_spickle.cs);
+               if (cobboard.right_spickle.on)
+                       cs_manage(&cobboard.right_spickle.cs);
+               if (cobboard.shovel.on)
+                       cs_manage(&cobboard.shovel.cs);
+       }
+       if (cobboard.flags & DO_BD) {
+               bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs);
+               bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs);
+               bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs);
+       }
+       if (cobboard.flags & DO_POWER)
+               BRAKE_OFF();
+       else
+               BRAKE_ON();
+}
+
+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)
+{
+       /* ---- CS left_spickle */
+       /* PID */
+       pid_init(&cobboard.left_spickle.pid);
+       pid_set_gains(&cobboard.left_spickle.pid, 500, 40, 5000);
+       pid_set_maximums(&cobboard.left_spickle.pid, 0, 5000, 2400); /* max is 12 V */
+       pid_set_out_shift(&cobboard.left_spickle.pid, 10);
+       pid_set_derivate_filter(&cobboard.left_spickle.pid, 4);
+
+       /* QUADRAMP */
+       quadramp_init(&cobboard.left_spickle.qr);
+       quadramp_set_1st_order_vars(&cobboard.left_spickle.qr, 2000, 2000); /* set speed */
+       quadramp_set_2nd_order_vars(&cobboard.left_spickle.qr, 20, 20); /* set accel */
+
+       /* CS */
+       cs_init(&cobboard.left_spickle.cs);
+       cs_set_consign_filter(&cobboard.left_spickle.cs, quadramp_do_filter, &cobboard.left_spickle.qr);
+       cs_set_correct_filter(&cobboard.left_spickle.cs, pid_do_filter, &cobboard.left_spickle.pid);
+       cs_set_process_in(&cobboard.left_spickle.cs, pwm_ng_set, LEFT_SPICKLE_PWM);
+       cs_set_process_out(&cobboard.left_spickle.cs, encoders_spi_get_value, LEFT_SPICKLE_ENCODER);
+       cs_set_consign(&cobboard.left_spickle.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&cobboard.left_spickle.bd);
+       bd_set_speed_threshold(&cobboard.left_spickle.bd, 150);
+       bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40);
+
+       /* ---- CS right_spickle */
+       /* PID */
+       pid_init(&cobboard.right_spickle.pid);
+       pid_set_gains(&cobboard.right_spickle.pid, 500, 40, 5000);
+       pid_set_maximums(&cobboard.right_spickle.pid, 0, 5000, 2400); /* max is 12 V */
+       pid_set_out_shift(&cobboard.right_spickle.pid, 10);
+       pid_set_derivate_filter(&cobboard.right_spickle.pid, 6);
+
+       /* QUADRAMP */
+       quadramp_init(&cobboard.right_spickle.qr);
+       quadramp_set_1st_order_vars(&cobboard.right_spickle.qr, 1000, 1000); /* set speed */
+       quadramp_set_2nd_order_vars(&cobboard.right_spickle.qr, 20, 20); /* set accel */
+
+       /* CS */
+       cs_init(&cobboard.right_spickle.cs);
+       cs_set_consign_filter(&cobboard.right_spickle.cs, quadramp_do_filter, &cobboard.right_spickle.qr);
+       cs_set_correct_filter(&cobboard.right_spickle.cs, pid_do_filter, &cobboard.right_spickle.pid);
+       cs_set_process_in(&cobboard.right_spickle.cs, pwm_ng_set, RIGHT_SPICKLE_PWM);
+       cs_set_process_out(&cobboard.right_spickle.cs, encoders_spi_get_value, RIGHT_SPICKLE_ENCODER);
+       cs_set_consign(&cobboard.right_spickle.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&cobboard.right_spickle.bd);
+       bd_set_speed_threshold(&cobboard.right_spickle.bd, 150);
+       bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40);
+
+       /* ---- CS shovel */
+       /* PID */
+       pid_init(&cobboard.shovel.pid);
+       pid_set_gains(&cobboard.shovel.pid, 500, 40, 5000);
+       pid_set_maximums(&cobboard.shovel.pid, 0, 5000, 2400); /* max is 12 V */
+       pid_set_out_shift(&cobboard.shovel.pid, 10);
+       pid_set_derivate_filter(&cobboard.shovel.pid, 6);
+
+       /* QUADRAMP */
+       quadramp_init(&cobboard.shovel.qr);
+       quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 1000); /* set speed */
+       quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 20, 20); /* set accel */
+
+       /* CS */
+       cs_init(&cobboard.shovel.cs);
+       cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);
+       cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid);
+       cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM);
+       cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER);
+       cs_set_consign(&cobboard.shovel.cs, 0);
+
+       /* Blocking detection */
+       bd_init(&cobboard.shovel.bd);
+       bd_set_speed_threshold(&cobboard.shovel.bd, 150);
+       bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40);
+
+       /* set them on !! */
+       cobboard.left_spickle.on = 1;
+       cobboard.right_spickle.on = 1;
+       cobboard.shovel.on = 1;
+
+       scheduler_add_periodical_event_priority(do_cs, NULL, 
+                                               CS_PERIOD / SCHEDULER_UNIT, 
+                                               CS_PRIO);
+}
diff --git a/projects/microb2010/cobboard/cs.h b/projects/microb2010/cobboard/cs.h
new file mode 100644 (file)
index 0000000..5b3be83
--- /dev/null
@@ -0,0 +1,25 @@
+/*  
+ *  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.1 2009-03-05 22:52:35 zer0 Exp $
+ *
+ */
+
+void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
diff --git a/projects/microb2010/cobboard/diagnostic_config.h b/projects/microb2010/cobboard/diagnostic_config.h
new file mode 100644 (file)
index 0000000..7302bfa
--- /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-03-05 22:52:35 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/cobboard/encoders_spi_config.h b/projects/microb2010/cobboard/encoders_spi_config.h
new file mode 100644 (file)
index 0000000..e71e662
--- /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-03-05 22:52:35 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/cobboard/error_config.h b/projects/microb2010/cobboard/error_config.h
new file mode 100644 (file)
index 0000000..2df45d0
--- /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-03-05 22:52:35 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/cobboard/i2c_config.h b/projects/microb2010/cobboard/i2c_config.h
new file mode 100644 (file)
index 0000000..93ce0ec
--- /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.1 2009-03-05 22:52:35 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/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c
new file mode 100644 (file)
index 0000000..b790128
--- /dev/null
@@ -0,0 +1,195 @@
+/*
+ *  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.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#include <string.h>
+
+#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 <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "sensor.h"
+#include "state.h"
+
+void i2c_protocol_init(void)
+{
+}
+
+/*** LED CONTROL ***/
+void i2c_led_control(uint8_t l, uint8_t state)
+{
+       switch(l) {
+       case 1:
+               state? LED1_ON():LED1_OFF();
+               break;
+       case 2:
+               state? LED2_ON():LED2_OFF();
+               break;
+       case 3:
+               state? LED3_ON():LED3_OFF();
+               break;
+       case 4:
+               state? LED4_ON():LED4_OFF();
+               break;
+       default:
+               break;
+       }
+}
+
+#ifdef notyet
+static void i2c_test(uint16_t val)
+{
+       static uint16_t prev=0;
+
+       if ( (val-prev) != 1 ) {
+               WARNING(E_USER_I2C_PROTO, "Dupplicated msg %d", val);
+       }
+       prev = val;
+       ext.nb_test_cmd ++;
+}
+#endif
+
+static void i2c_send_status(void)
+{
+       struct i2c_ans_cobboard_status ans;
+       i2c_flush();
+       ans.hdr.cmd =  I2C_ANS_COBBOARD_STATUS;
+
+       /* status */
+       ans.mode = state_get_mode();
+       ans.status = 0x55;
+
+       i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
+                sizeof(ans), I2C_CTRL_GENERIC);
+}
+
+static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+{
+       state_set_mode(cmd);
+       return 0;
+}
+
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+       void *void_cmd = buf;
+       
+       static uint8_t a = 0;
+       
+       a++;
+       if (a & 0x10)
+               LED2_TOGGLE();
+       
+       if (size <= 0) {
+               goto error;
+       }
+       
+       switch (buf[0]) {
+
+       /* Commands (no answer needed) */
+       case I2C_CMD_GENERIC_LED_CONTROL: 
+               {
+                       struct i2c_cmd_led_control *cmd = void_cmd;
+                       if (size != sizeof (*cmd))
+                               goto error;
+                       i2c_led_control(cmd->led_num, cmd->state);
+                       break;
+               }
+               
+       case I2C_CMD_COBBOARD_SET_MODE:
+               {
+                       struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
+                       if (size != sizeof(struct i2c_cmd_cobboard_set_mode))
+                               goto error;
+                       i2c_set_mode(cmd);
+                       break;
+               }
+
+       case I2C_CMD_GENERIC_SET_COLOR:
+               {
+                       struct i2c_cmd_generic_color *cmd = void_cmd;
+                       if (size != sizeof (*cmd))
+                               goto error;
+                       cobboard.our_color = cmd->color;
+                       break;
+               }
+
+#ifdef notyet
+       case I2C_CMD_EXTENSION_TEST: 
+               {
+                       struct i2c_cmd_extension_test *cmd = void_cmd;
+                       if (size != sizeof (*cmd))
+                               goto error;
+                       i2c_test(cmd->val);
+                       break;
+               }
+#endif
+               
+       /* Add other commands here ...*/
+
+
+       case I2C_REQ_COBBOARD_STATUS:
+               {
+                       //struct i2c_req_cobboard_status *cmd = void_cmd;
+                       if (size != sizeof (struct i2c_req_cobboard_status))
+                               goto error;
+                       
+                       i2c_send_status();
+                       break;
+               }
+
+       default:
+               goto error;
+       }
+
+ error:
+       /* log error on a led ? */
+       return;
+}
+
+void i2c_recvbyteevent(__attribute__((unused)) uint8_t hwstatus,
+                      __attribute__((unused)) uint8_t i, 
+                      __attribute__((unused)) int8_t c)
+{
+}
+
+void i2c_sendevent(__attribute__((unused)) int8_t size)
+{
+}
+
+
diff --git a/projects/microb2010/cobboard/i2c_protocol.h b/projects/microb2010/cobboard/i2c_protocol.h
new file mode 100644 (file)
index 0000000..7f022ef
--- /dev/null
@@ -0,0 +1,30 @@
+/*
+ *  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_protocol.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+
+void i2c_protocol_init(void);
+
+void i2c_recvevent(uint8_t * buf, int8_t size);
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
+void i2c_sendevent(int8_t size);
+
+int debug_send(char c, FILE* f);
diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c
new file mode 100755 (executable)
index 0000000..418f5e1
--- /dev/null
@@ -0,0 +1,266 @@
+/*  
+ *  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.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <avr/eeprom.h>
+
+#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 <i2c.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "../common/eeprom_mapping.h"
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+#include "ax12_user.h"
+#include "cmdline.h"
+#include "sensor.h"
+#include "state.h"
+#include "actuator.h"
+#include "arm_xy.h"
+#include "cs.h"
+#include "i2c_protocol.h"
+
+/* 0 means "programmed"
+ * ---- with 16 Mhz quartz
+ * CKSEL 3-0 : 0111
+ * SUT 1-0 : 10 
+ * CKDIV8 : 1
+ * ---- bootloader
+ * BOOTZ 1-0 : 01 (4K bootloader)
+ * BOOTRST : 0 (reset on bootloader)
+ * ---- jtag
+ * jtagen : 0
+ */
+
+struct genboard gen;
+struct cobboard cobboard;
+
+/***********************/
+
+void bootloader(void)
+{
+#define BOOTLOADER_ADDR 0x3f000
+       if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
+               printf_P(PSTR("Bootloader is not present\r\n"));
+               return;
+       }
+       cli();
+       BRAKE_ON();
+       /* ... very specific :( */
+       TIMSK0 = 0;
+       TIMSK1 = 0;
+       TIMSK2 = 0;
+       TIMSK3 = 0;
+       TIMSK4 = 0;
+       TIMSK5 = 0;
+       EIMSK = 0;
+       UCSR0B = 0;
+       UCSR1B = 0;
+       UCSR2B = 0;
+       UCSR3B = 0;
+       SPCR = 0;
+       TWCR = 0;
+       ACSR = 0;
+       ADCSRA = 0;
+
+       EIND = 1;
+       __asm__ __volatile__ ("ldi r31,0xf8\n");
+       __asm__ __volatile__ ("ldi r30,0x00\n");
+       __asm__ __volatile__ ("eijmp\n");
+       
+       /* never returns */
+}
+
+void do_led_blink(__attribute__((unused)) void *dummy)
+{
+#if 1 /* simple blink */
+       static uint8_t a=0;
+
+       if(a)
+               LED1_ON();
+       else
+               LED1_OFF();
+       
+       a = !a;
+#endif
+}
+
+static void main_timer_interrupt(void)
+{
+       static uint8_t cpt = 0;
+       cpt++;
+       sei();
+       if ((cpt & 0x3) == 0)
+               scheduler_interrupt();
+}
+
+int main(void)
+{
+       /* brake */
+       BRAKE_ON();
+       BRAKE_DDR();
+
+       /* CPLD reset on PG3 */
+       DDRG |= 1<<3;
+       PORTG &= ~(1<<3); /* implicit */
+
+       /* LEDS */
+       DDRJ |= 0x0c;
+       DDRL = 0xc0;
+       LED1_OFF();
+       memset(&gen, 0, sizeof(gen));
+       memset(&cobboard, 0, sizeof(cobboard));
+       /* cs is enabled after arm_calibrate() */
+       cobboard.flags = DO_ENCODERS | DO_POWER; // DO_BD
+
+       /* UART */
+       uart_init();
+#if CMDLINE_UART == 3
+       fdevopen(uart3_dev_send, uart3_dev_recv);
+       uart_register_rx_event(3, emergency);
+#elif CMDLINE_UART == 1
+       fdevopen(uart1_dev_send, uart1_dev_recv);
+       uart_register_rx_event(1, emergency);
+#else
+#  error not supported
+#endif
+
+       //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
+       /* check eeprom to avoid to run the bad program */
+       if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
+           EEPROM_MAGIC_COBBOARD) {
+               sei();
+               printf_P(PSTR("Bad eeprom value\r\n"));
+               while(1);
+       }
+
+       /* LOGS */
+       error_register_emerg(mylog);
+       error_register_error(mylog);
+       error_register_warning(mylog);
+       error_register_notice(mylog);
+       error_register_debug(mylog);
+
+       /* SPI + ENCODERS */
+       encoders_spi_init(); /* this will also init spi hardware */
+
+       /* I2C */
+       i2c_protocol_init();
+       i2c_init(I2C_MODE_SLAVE, I2C_COBBOARD_ADDR);
+       i2c_register_recv_event(i2c_recvevent);
+
+       /* TIMER */
+       timer_init();
+       timer0_register_OV_intr(main_timer_interrupt);
+
+       /* PWM */
+       PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, 
+                                TIMER1_PRESCALER_DIV_1);
+       PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, 
+                                TIMER4_PRESCALER_DIV_1);
+       
+       PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
+                     &PORTD, 4);
+       PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
+                     &PORTD, 5);
+       PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED | 
+                     PWM_NG_MODE_SIGN_INVERTED,
+                     &PORTD, 6);
+       PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED |
+                     PWM_NG_MODE_SIGN_INVERTED,
+                     &PORTD, 7);
+
+
+       /* servos */
+       PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10, 
+                                TIMER1_PRESCALER_DIV_256);
+       PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10, 
+                                TIMER1_PRESCALER_DIV_256);
+       PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
+                     NULL, 0);
+       
+       /* SCHEDULER */
+       scheduler_init();
+
+       scheduler_add_periodical_event_priority(do_led_blink, NULL, 
+                                               100000L / SCHEDULER_UNIT, 
+                                               LED_PRIO);
+       /* all cs management */
+       microb_cs_init();
+
+       /* sensors, will also init hardware adc */
+       sensor_init();
+
+       /* TIME */
+       time_init(TIME_PRIO);
+
+       /* ax12 */
+       ax12_user_init();
+
+       sei();
+
+       /* finger + other actuators */
+       actuator_init();
+
+       state_init();
+
+       printf_P(PSTR("\r\n"));
+       printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
+
+       /* arm management */
+       gen.logs[0] = E_USER_ST_MACH;
+       gen.log_level = 5;
+       cobboard.flags |= DO_CS;
+
+       state_machine();
+       cmdline_interact();
+
+       return 0;
+}
diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h
new file mode 100755 (executable)
index 0000000..33fb833
--- /dev/null
@@ -0,0 +1,150 @@
+/*  
+ *  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: main.h,v 1.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#define LED_TOGGLE(port, bit) do {             \
+               if (port & _BV(bit))            \
+                       port &= ~_BV(bit);      \
+               else                            \
+                       port |= _BV(bit);       \
+       } while(0)
+
+#define LED1_ON()      sbi(PORTJ, 2)
+#define LED1_OFF()     cbi(PORTJ, 2)
+#define LED1_TOGGLE()  LED_TOGGLE(PORTJ, 2)
+
+#define LED2_ON()      sbi(PORTL, 7)
+#define LED2_OFF()     cbi(PORTL, 7)
+#define LED2_TOGGLE()  LED_TOGGLE(PORTL, 7)
+
+#define LED3_ON()      sbi(PORTJ, 3)
+#define LED3_OFF()     cbi(PORTJ, 3)
+#define LED3_TOGGLE()  LED_TOGGLE(PORTJ, 3)
+
+#define LED4_ON()      sbi(PORTL, 6)
+#define LED4_OFF()     cbi(PORTL, 6)
+#define LED4_TOGGLE()  LED_TOGGLE(PORTL, 6)
+
+#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 LEFT_SPICKLE_ENCODER   ((void *)0)
+#define RIGHT_SPICKLE_ENCODER  ((void *)1)
+#define SHOVEL_ENCODER         ((void *)1)
+
+#define LEFT_SPICKLE_PWM       ((void *)&gen.pwm1_4A)
+#define RIGHT_SPICKLE_PWM      ((void *)&gen.pwm2_4B)
+#define SHOVEL_PWM             ((void *)&gen.pwm2_4B)
+
+/** ERROR NUMS */
+#define E_USER_I2C_PROTO       195
+#define E_USER_SENSOR          196
+#define E_USER_SPICKLE         197
+#define E_USER_ST_MACH         199
+#define E_USER_CS              200
+#define E_USER_AX12            201
+
+#define LED_PRIO           170
+#define TIME_PRIO          160
+#define ADC_PRIO           120
+#define CS_PRIO            100
+#define I2C_POLL_PRIO       20
+
+#define CS_PERIOD 5000L
+
+#define NB_LOGS 4
+
+/* generic to all boards */
+struct genboard {
+       /* command line interface */
+       struct rdline rdl;
+       char prompt[RDLINE_PROMPT_SIZE];
+
+       /* motors */
+       struct pwm_ng pwm1_4A;
+       struct pwm_ng pwm2_4B;
+       struct pwm_ng pwm3_1A;
+       struct pwm_ng pwm4_1B;
+
+       /* servos */
+       struct pwm_ng servo1;
+       struct pwm_ng servo2;
+       struct pwm_ng servo3;
+       struct pwm_ng servo4;
+       
+       /* ax12 interface */
+       AX12 ax12;
+
+       /* log */
+       uint8_t logs[NB_LOGS+1];
+       uint8_t log_level;
+       uint8_t debug;
+};
+
+struct cs_block {
+       uint8_t on;
+        struct cs cs;
+        struct pid_filter pid;
+       struct quadramp_filter qr;
+       struct blocking_detection bd;
+};
+
+/* cobboard specific */
+struct cobboard {
+#define DO_ENCODERS  1
+#define DO_CS        2
+#define DO_BD        4
+#define DO_POWER     8
+       uint8_t flags;                /* misc flags */
+
+       /* control systems */
+        struct cs_block left_spickle;
+        struct cs_block right_spickle;
+        struct cs_block shovel;
+
+       /* robot status */
+       uint8_t our_color;
+       volatile uint8_t cob_count;
+       volatile uint8_t status;
+};
+
+extern struct genboard gen;
+extern struct cobboard cobboard;
+
+/* start the bootloader */
+void bootloader(void);
+
+#define DEG(x) (((double)(x)) * (180.0 / M_PI))
+#define RAD(x) (((double)(x)) * (M_PI / 180.0))
+#define M_2PI (2*M_PI)
+
+#define WAIT_COND_OR_TIMEOUT(cond, timeout)                   \
+({                                                            \
+        microseconds __us = time_get_us2();                   \
+        uint8_t __ret = 1;                                    \
+        while(! (cond)) {                                     \
+                if (time_get_us2() - __us > (timeout)*1000L) {\
+                        __ret = 0;                            \
+                        break;                                \
+                }                                             \
+        }                                                     \
+        __ret;                                                \
+})
diff --git a/projects/microb2010/cobboard/pid_config.h b/projects/microb2010/cobboard/pid_config.h
new file mode 100755 (executable)
index 0000000..fa95f08
--- /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
+ *
+ * 
+ *
+ */
+
+#ifndef PID_CONFIG_H
+#define P