From: Olivier Matz Date: Wed, 17 Mar 2010 22:38:35 +0000 (+0100) Subject: prepare cobboard and ballboard X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305;hp=821f753c0f88aff895d9feae59c442a6c446f96b prepare cobboard and ballboard --- diff --git a/modules/comm/uart/uart_defs.h b/modules/comm/uart/uart_defs.h index e37de8c..7d872a1 100644 --- a/modules/comm/uart/uart_defs.h +++ b/modules/comm/uart/uart_defs.h @@ -115,6 +115,9 @@ #ifndef TXC #define TXC TXC0 #endif +#ifndef RXC +#define RXC RXC0 +#endif #ifndef RXB8 #define RXB8 RXB80 #endif diff --git a/projects/microb2010/ballboard/.config b/projects/microb2010/ballboard/.config new file mode 100644 index 0000000..0d59101 --- /dev/null +++ b/projects/microb2010/ballboard/.config @@ -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 index 0000000..afa396b --- /dev/null +++ b/projects/microb2010/ballboard/Makefile @@ -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 index 0000000..367e241 --- /dev/null +++ b/projects/microb2010/ballboard/actuator.c @@ -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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" + diff --git a/projects/microb2010/ballboard/actuator.h b/projects/microb2010/ballboard/actuator.h new file mode 100644 index 0000000..a6320be --- /dev/null +++ b/projects/microb2010/ballboard/actuator.h @@ -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 index 0000000..e69de29 diff --git a/projects/microb2010/ballboard/ax12_config.h b/projects/microb2010/ballboard/ax12_config.h new file mode 100755 index 0000000..072e8fb --- /dev/null +++ b/projects/microb2010/ballboard/ax12_config.h @@ -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 index 0000000..e3a5f9f --- /dev/null +++ b/projects/microb2010/ballboard/ax12_user.c @@ -0,0 +1,169 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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< 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< + * + * 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 index 0000000..6f8ff46 --- /dev/null +++ b/projects/microb2010/ballboard/cmdline.c @@ -0,0 +1,149 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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; ierr_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 index 0000000..57dfa49 --- /dev/null +++ b/projects/microb2010/ballboard/cmdline.h @@ -0,0 +1,40 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 index 0000000..9ab33ad --- /dev/null +++ b/projects/microb2010/ballboard/commands.c @@ -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 + */ + +#include +#include +#include + +/* 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 index 0000000..2fda225 --- /dev/null +++ b/projects/microb2010/ballboard/commands_ax12.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..03dc830 --- /dev/null +++ b/projects/microb2010/ballboard/commands_ballboard.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..098ffb3 --- /dev/null +++ b/projects/microb2010/ballboard/commands_cs.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..b2a1e5d --- /dev/null +++ b/projects/microb2010/ballboard/commands_gen.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#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; iarg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("SENSOR values: ")); + for (i=0; iarg1, 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; iarg3, PSTR("off"))) { + for (i=0; i + * + * 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 +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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 index 0000000..fe50c80 --- /dev/null +++ b/projects/microb2010/ballboard/cs.h @@ -0,0 +1,25 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 index 0000000..e4ebb75 --- /dev/null +++ b/projects/microb2010/ballboard/diagnostic_config.h @@ -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 index 0000000..6a4a68b --- /dev/null +++ b/projects/microb2010/ballboard/encoders_spi_config.h @@ -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 index 0000000..15ca2fc --- /dev/null +++ b/projects/microb2010/ballboard/error_config.h @@ -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 index 0000000..ebc0678 --- /dev/null +++ b/projects/microb2010/ballboard/i2c_config.h @@ -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 index 0000000..8995e55 --- /dev/null +++ b/projects/microb2010/ballboard/i2c_protocol.c @@ -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 + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..6fcac74 --- /dev/null +++ b/projects/microb2010/ballboard/i2c_protocol.h @@ -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 + +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 index 0000000..46a3b52 --- /dev/null +++ b/projects/microb2010/ballboard/main.c @@ -0,0 +1,252 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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 index 0000000..36915bd --- /dev/null +++ b/projects/microb2010/ballboard/main.h @@ -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 index 0000000..fa95f08 --- /dev/null +++ b/projects/microb2010/ballboard/pid_config.h @@ -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 index 0000000..e69de29 diff --git a/projects/microb2010/ballboard/scheduler_config.h b/projects/microb2010/ballboard/scheduler_config.h new file mode 100755 index 0000000..2547a24 --- /dev/null +++ b/projects/microb2010/ballboard/scheduler_config.h @@ -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 index 0000000..6cfb491 --- /dev/null +++ b/projects/microb2010/ballboard/sensor.c @@ -0,0 +1,258 @@ +/* + * Copyright Droids Corporation (2009) + * Olivier MATZ + * + * 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 + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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_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 index 0000000..c2b18a7 --- /dev/null +++ b/projects/microb2010/ballboard/sensor.h @@ -0,0 +1,56 @@ +/* + * Copyright Droids Corporation (2009) + * Olivier MATZ + * + * 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 index 0000000..76697c3 --- /dev/null +++ b/projects/microb2010/ballboard/spi_config.h @@ -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 index 0000000..e0f7f2a --- /dev/null +++ b/projects/microb2010/ballboard/time_config.h @@ -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 index 0000000..810525c --- /dev/null +++ b/projects/microb2010/ballboard/timer_config.h @@ -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 index 0000000..2a27e07 --- /dev/null +++ b/projects/microb2010/ballboard/uart_config.h @@ -0,0 +1,102 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: uart_config.h,v 1.3 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART1 definitions + */ + +/* compile uart1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable uart1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable uart1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 1 + +#define UART1_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART1_USE_DOUBLE_SPEED 1 + +#define UART1_RX_FIFO_SIZE 64 +#define UART1_TX_FIFO_SIZE 64 +#define UART1_NBITS 8 + +#define UART1_PARITY UART_PARTITY_NONE + +#define UART1_STOP_BIT UART_STOP_BITS_1 + + +/* + * UART3 definitions + */ + +/* compile uart3 fonctions, undefine it to pass compilation */ +#define UART3_COMPILE + +/* enable uart3 if == 1, disable if == 0 */ +#define UART3_ENABLED 1 + +/* enable uart3 interrupts if == 1, disable if == 0 */ +#define UART3_INTERRUPT_ENABLED 1 + +#define UART3_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART3_USE_DOUBLE_SPEED 1 +//#define UART3_USE_DOUBLE_SPEED 1 + +#define UART3_RX_FIFO_SIZE 64 +#define UART3_TX_FIFO_SIZE 64 +//#define UART3_NBITS 5 +//#define UART3_NBITS 6 +//#define UART3_NBITS 7 +#define UART3_NBITS 8 +//#define UART3_NBITS 9 + +#define UART3_PARITY UART_PARTITY_NONE +//#define UART3_PARITY UART_PARTITY_ODD +//#define UART3_PARITY UART_PARTITY_EVEN + +#define UART3_STOP_BIT UART_STOP_BITS_1 +//#define UART3_STOP_BIT UART_STOP_BITS_2 + + + + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/projects/microb2010/cobboard/.config b/projects/microb2010/cobboard/.config new file mode 100644 index 0000000..706895c --- /dev/null +++ b/projects/microb2010/cobboard/.config @@ -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 index 0000000..0c93238 --- /dev/null +++ b/projects/microb2010/cobboard/Makefile @@ -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 index 0000000..67598ea --- /dev/null +++ b/projects/microb2010/cobboard/actuator.c @@ -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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#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 index 0000000..cd2e28b --- /dev/null +++ b/projects/microb2010/cobboard/actuator.h @@ -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 index 0000000..e69de29 diff --git a/projects/microb2010/cobboard/ax12_config.h b/projects/microb2010/cobboard/ax12_config.h new file mode 100755 index 0000000..072e8fb --- /dev/null +++ b/projects/microb2010/cobboard/ax12_config.h @@ -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 index 0000000..b022ef7 --- /dev/null +++ b/projects/microb2010/cobboard/ax12_user.c @@ -0,0 +1,305 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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< 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< + * + * 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 index 0000000..6de7569 --- /dev/null +++ b/projects/microb2010/cobboard/cmdline.c @@ -0,0 +1,149 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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; ierr_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 index 0000000..e5bb569 --- /dev/null +++ b/projects/microb2010/cobboard/cmdline.h @@ -0,0 +1,40 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 index 0000000..1723f2a --- /dev/null +++ b/projects/microb2010/cobboard/commands.c @@ -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 + */ + +#include +#include +#include + +/* 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 index 0000000..ebe3834 --- /dev/null +++ b/projects/microb2010/cobboard/commands_ax12.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..b822e4a --- /dev/null +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..27be783 --- /dev/null +++ b/projects/microb2010/cobboard/commands_cs.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..393f874 --- /dev/null +++ b/projects/microb2010/cobboard/commands_gen.c @@ -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 + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#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; iarg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("SENSOR values: ")); + for (i=0; iarg1, 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; iarg3, PSTR("off"))) { + for (i=0; i + * + * 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 +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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 index 0000000..5b3be83 --- /dev/null +++ b/projects/microb2010/cobboard/cs.h @@ -0,0 +1,25 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 index 0000000..7302bfa --- /dev/null +++ b/projects/microb2010/cobboard/diagnostic_config.h @@ -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 index 0000000..e71e662 --- /dev/null +++ b/projects/microb2010/cobboard/encoders_spi_config.h @@ -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 index 0000000..2df45d0 --- /dev/null +++ b/projects/microb2010/cobboard/error_config.h @@ -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 index 0000000..93ce0ec --- /dev/null +++ b/projects/microb2010/cobboard/i2c_config.h @@ -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 index 0000000..b790128 --- /dev/null +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -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 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 index 0000000..7f022ef --- /dev/null +++ b/projects/microb2010/cobboard/i2c_protocol.h @@ -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 + +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 index 0000000..418f5e1 --- /dev/null +++ b/projects/microb2010/cobboard/main.c @@ -0,0 +1,266 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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 index 0000000..33fb833 --- /dev/null +++ b/projects/microb2010/cobboard/main.h @@ -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 index 0000000..fa95f08 --- /dev/null +++ b/projects/microb2010/cobboard/pid_config.h @@ -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/cobboard/rdline_config.h b/projects/microb2010/cobboard/rdline_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/cobboard/scheduler_config.h b/projects/microb2010/cobboard/scheduler_config.h new file mode 100755 index 0000000..98690be --- /dev/null +++ b/projects/microb2010/cobboard/scheduler_config.h @@ -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-04-07 20:03:48 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 9 + + +#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/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c new file mode 100644 index 0000000..a80201f --- /dev/null +++ b/projects/microb2010/cobboard/sensor.c @@ -0,0 +1,254 @@ +/* + * Copyright Droids Corporation (2009) + * Olivier MATZ + * + * 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.6 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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(__attribute__((unused)) 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 1,5,6,7,8 + */ +static struct sensor_filter sensor_filter[SENSOR_MAX] = { + [S_CAP1] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ + [S_FRONT] = { 5, 0, 4, 1, 0, 0 }, /* 1 */ + [S_CAP3] = { 10, 0, 3, 7, 0, 0 }, /* 2 */ + [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ + [S_COL_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 4 */ + [S_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 5 */ + [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ + [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */ + [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ + [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(__attribute__((unused)) void *dummy) +{ + uint8_t i; + uint8_t flags; + uint16_t sensor = sensor_read(); + uint16_t tmp = 0; + + for (i=0; i= 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 && !sensor_filter[i].invert) { + tmp |= (1UL << i); + } + else if (!sensor_filter[i].prev && sensor_filter[i].invert) { + tmp |= (1UL << i); + } + } + IRQ_LOCK(flags); + sensor_filtered = tmp; + IRQ_UNLOCK(flags); +} + + + +/************ global sensor init */ + +/* called every 10 ms, see init below */ +static void do_sensors(__attribute__((unused)) void *dummy) +{ + do_adc(NULL); + do_boolean_sensors(NULL); +} + +void sensor_init(void) +{ + adc_init(); + 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/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h new file mode 100644 index 0000000..27460db --- /dev/null +++ b/projects/microb2010/cobboard/sensor.h @@ -0,0 +1,56 @@ +/* + * Copyright Droids Corporation (2009) + * Olivier MATZ + * + * 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.4 2009-04-24 19:30:42 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_FRONT 1 +#define S_CAP3 2 +#define S_CAP4 3 +#define S_COL_LEFT 4 +#define S_LEFT 5 +#define S_RIGHT 6 +#define S_COL_RIGHT 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/cobboard/spi_config.h b/projects/microb2010/cobboard/spi_config.h new file mode 100644 index 0000000..76697c3 --- /dev/null +++ b/projects/microb2010/cobboard/spi_config.h @@ -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/cobboard/state.c b/projects/microb2010/cobboard/state.c new file mode 100644 index 0000000..f9aefa6 --- /dev/null +++ b/projects/microb2010/cobboard/state.c @@ -0,0 +1,163 @@ +/* + * 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: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "cmdline.h" +#include "sensor.h" +#include "actuator.h" +#include "state.h" + +#define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args) +#define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args) +#define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args) + +/* shorter aliases for this file */ +#define INIT I2C_COBBOARD_MODE_INIT +#define MANUAL I2C_COBBOARD_MODE_MANUAL +#define HARVEST I2C_COBBOARD_MODE_HARVEST +#define EXIT I2C_COBBOARD_MODE_EXIT + +static struct i2c_cmd_cobboard_set_mode mainboard_command; +static struct vt100 local_vt100; +static volatile uint8_t prev_state; +static volatile uint8_t changed = 0; + +uint8_t state_debug = 0; + +void state_dump_sensors(void) +{ + STMCH_DEBUG("TODO\n"); +} + +void state_debug_wait_key_pressed(void) +{ + if (!state_debug) + return; + printf_P(PSTR("press a key\r\n")); + while(!cmdline_keypressed()); +} + +/* set a new state, return 0 on success */ +int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +{ + changed = 1; + prev_state = mainboard_command.mode; + memcpy(&mainboard_command, cmd, sizeof(mainboard_command)); + STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode); + return 0; +} + +/* check that state is the one in parameter and that state did not + * changed */ +uint8_t state_check(uint8_t mode) +{ + int16_t c; + if (mode != mainboard_command.mode) + return 0; + + if (changed) + return 0; + + /* force quit when CTRL-C is typed */ + c = cmdline_getchar(); + if (c == -1) + return 1; + if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) { + mainboard_command.mode = EXIT; + return 0; + } + return 1; +} + +uint8_t state_get_mode(void) +{ + return mainboard_command.mode; +} + +/* manual mode, arm position is sent from mainboard */ +static void state_do_manual(void) +{ + if (!state_check(MANUAL)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(MANUAL)); +} + +/* init mode */ +static void state_do_init(void) +{ + if (!state_check(INIT)) + return; + state_init(); + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(INIT)); +} + +/* harvest columns elts from area */ +static void state_do_harvest(void) +{ + if (!state_check(HARVEST)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(HARVEST)); +} +/* main state machine */ +void state_machine(void) +{ + while (state_get_mode() != EXIT) { + changed = 0; + state_do_init(); + state_do_manual(); + state_do_harvest(); + } +} + +void state_init(void) +{ + vt100_init(&local_vt100); + mainboard_command.mode = HARVEST; + cobboard.cob_count = 0; +} diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h new file mode 100644 index 0000000..889970b --- /dev/null +++ b/projects/microb2010/cobboard/state.h @@ -0,0 +1,36 @@ +/* + * 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: state.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#ifndef _STATE_H_ +#define _STATE_H_ + +/* set a new state, return 0 on success */ +int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd); + +/* get current state */ +uint8_t state_get_mode(void); + +/* launch state machine */ +void state_machine(void); + +void state_init(void); + +#endif diff --git a/projects/microb2010/cobboard/time_config.h b/projects/microb2010/cobboard/time_config.h new file mode 100755 index 0000000..6a1e4c5 --- /dev/null +++ b/projects/microb2010/cobboard/time_config.h @@ -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.3 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +/** precision of the time processor, in us */ +#define TIME_PRECISION 1000l diff --git a/projects/microb2010/cobboard/timer_config.h b/projects/microb2010/cobboard/timer_config.h new file mode 100755 index 0000000..bbfad76 --- /dev/null +++ b/projects/microb2010/cobboard/timer_config.h @@ -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-05 22:52:35 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/cobboard/uart_config.h b/projects/microb2010/cobboard/uart_config.h new file mode 100644 index 0000000..a54811e --- /dev/null +++ b/projects/microb2010/cobboard/uart_config.h @@ -0,0 +1,67 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: uart_config.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* compile uart0 fonctions, undefine it to pass compilation */ +#define UART0_COMPILE +#define UART0_ENABLED 1 +#define UART0_INTERRUPT_ENABLED 1 +#define UART0_BAUDRATE 57600 +//#define UART0_BAUDRATE 1000000 +#define UART0_USE_DOUBLE_SPEED 1 +#define UART0_RX_FIFO_SIZE 32 +#define UART0_TX_FIFO_SIZE 32 +#define UART0_NBITS 8 +#define UART0_PARITY UART_PARTITY_NONE +#define UART0_STOP_BIT UART_STOP_BITS_2 + +#define UART1_COMPILE +#define UART1_ENABLED 1 +#define UART1_INTERRUPT_ENABLED 1 +#define UART1_BAUDRATE 57600 +#define UART1_USE_DOUBLE_SPEED 1 +#define UART1_RX_FIFO_SIZE 128 +#define UART1_TX_FIFO_SIZE 128 +#define UART1_NBITS 8 +#define UART1_PARITY UART_PARTITY_NONE +#define UART1_STOP_BIT UART_STOP_BITS_1 + +#define UART3_COMPILE +#define UART3_ENABLED 1 +#define UART3_INTERRUPT_ENABLED 1 +#define UART3_BAUDRATE 57600 +#define UART3_USE_DOUBLE_SPEED 1 +#define UART3_RX_FIFO_SIZE 128 +#define UART3_TX_FIFO_SIZE 128 +#define UART3_NBITS 8 +#define UART3_PARITY UART_PARTITY_NONE +#define UART3_STOP_BIT UART_STOP_BITS_1 + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/projects/microb2010/common/eeprom_mapping.h b/projects/microb2010/common/eeprom_mapping.h index 01bd6e3..5ea6125 100644 --- a/projects/microb2010/common/eeprom_mapping.h +++ b/projects/microb2010/common/eeprom_mapping.h @@ -23,8 +23,8 @@ #define _EEPROM_MAPPING_H_ #define EEPROM_MAGIC_MAINBOARD 1 -#define EEPROM_MAGIC_MECHBOARD 2 -#define EEPROM_MAGIC_SENSORBOARD 3 +#define EEPROM_MAGIC_COBBOARD 2 +#define EEPROM_MAGIC_BALLBOARD 3 #define EEPROM_MAGIC_ADDRESS ((uint8_t *)0x100) diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index fdb5d49..35605b8 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -23,19 +23,17 @@ #define _I2C_COMMANDS_H_ #define I2C_MAINBOARD_ADDR 1 -#define I2C_MECHBOARD_ADDR 2 -#define I2C_SENSORBOARD_ADDR 3 +#define I2C_COBBOARD_ADDR 2 +#define I2C_BALLBOARD_ADDR 3 #define I2C_LEFT_SIDE 0 #define I2C_RIGHT_SIDE 1 -#define I2C_AUTO_SIDE 2 /* for prepare_pickup */ -#define I2C_CENTER_SIDE 3 /* for prepare_pickup */ +#define I2C_AUTO_SIDE 2 +#define I2C_CENTER_SIDE 3 #define I2C_COLOR_RED 0 #define I2C_COLOR_GREEN 1 -#define I2C_AUTOBUILD_DEFAULT_DIST 210 - struct i2c_cmd_hdr { uint8_t cmd; }; @@ -63,146 +61,21 @@ struct i2c_cmd_generic_color { /****/ -#define I2C_CMD_MECHBOARD_SET_MODE 0x02 +#define I2C_CMD_COBBOARD_SET_MODE 0x02 -struct i2c_cmd_mechboard_set_mode { +struct i2c_cmd_cobboard_set_mode { struct i2c_cmd_hdr hdr; -#define I2C_MECHBOARD_MODE_MANUAL 0x00 -#define I2C_MECHBOARD_MODE_HARVEST 0x01 -#define I2C_MECHBOARD_MODE_PREPARE_PICKUP 0x02 -#define I2C_MECHBOARD_MODE_PICKUP 0x03 -#define I2C_MECHBOARD_MODE_PREPARE_BUILD 0x04 -#define I2C_MECHBOARD_MODE_AUTOBUILD 0x05 -#define I2C_MECHBOARD_MODE_WAIT 0x06 -#define I2C_MECHBOARD_MODE_INIT 0x07 -#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08 -#define I2C_MECHBOARD_MODE_GET_LINTEL 0x09 -#define I2C_MECHBOARD_MODE_PUT_LINTEL 0x0A -#define I2C_MECHBOARD_MODE_PREPARE_EJECT 0x0B -#define I2C_MECHBOARD_MODE_EJECT 0x0C -#define I2C_MECHBOARD_MODE_CLEAR 0x0D -#define I2C_MECHBOARD_MODE_LAZY_HARVEST 0x0E -#define I2C_MECHBOARD_MODE_LOADED 0x0F -#define I2C_MECHBOARD_MODE_PREPARE_INSIDE 0x10 -#define I2C_MECHBOARD_MODE_STORE 0x11 -#define I2C_MECHBOARD_MODE_LAZY_PICKUP 0x12 -#define I2C_MECHBOARD_MODE_MANIVELLE 0x13 -#define I2C_MECHBOARD_MODE_PUSH_TEMPLE 0x14 -#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC 0x15 -#define I2C_MECHBOARD_MODE_EXIT 0xFF +#define I2C_COBBOARD_MODE_INIT 0x00 +#define I2C_COBBOARD_MODE_MANUAL 0x01 +#define I2C_COBBOARD_MODE_HARVEST 0x02 +#define I2C_COBBOARD_MODE_EXIT 0xFF uint8_t mode; union { struct { - } manual; struct { - uint8_t side; - uint8_t next_mode; - } prep_pickup; - - struct { - uint8_t level_l; - uint8_t level_r; - } prep_build; - - struct { - uint8_t side; - } push_temple_disc; - - struct { - uint8_t level; - } push_temple; - - struct { - uint8_t level_left; - uint8_t level_right; - uint8_t count_left; - uint8_t count_right; - uint8_t distance_left; - uint8_t distance_right; - uint8_t do_lintel; - } autobuild; - - struct { - uint8_t level_l; - uint8_t level_r; - } prep_inside; - }; -}; - -/****/ - -/* only valid in manual mode */ -#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03 - -struct i2c_cmd_mechboard_arm_goto { - struct i2c_cmd_hdr hdr; -#define I2C_MECHBOARD_ARM_LEFT 0 -#define I2C_MECHBOARD_ARM_RIGHT 1 -#define I2C_MECHBOARD_ARM_BOTH 2 - uint8_t which; - - uint8_t height; /* in cm */ - uint8_t distance; /* in cm */ -}; - -/****/ - -#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04 - -struct i2c_cmd_sensorboard_start_beacon { - struct i2c_cmd_hdr hdr; - uint8_t enable; -}; - - -/****/ - -#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05 - -struct i2c_cmd_sensorboard_scanner { - struct i2c_cmd_hdr hdr; - -#define I2C_SENSORBOARD_SCANNER_STOP 0x00 -#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01 -#define I2C_SENSORBOARD_SCANNER_START 0x02 - uint8_t mode; -}; - -/*****/ - -#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06 -struct i2c_cmd_sensorboard_calib_scanner { - struct i2c_cmd_hdr hdr; -}; - -/*****/ - -#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07 -struct i2c_cmd_sensorboard_scanner_algo { - struct i2c_cmd_hdr hdr; - -#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1 -#define I2C_SCANNER_ALGO_CHECK_TEMPLE 2 -#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3 - uint8_t algo; - - union { - struct { -#define I2C_SCANNER_ZONE_0 0 -#define I2C_SCANNER_ZONE_1 1 -#define I2C_SCANNER_ZONE_DISC 2 - uint8_t working_zone; - int16_t center_x; - int16_t center_y; - } drop_zone; - - struct { - uint8_t level; - int16_t temple_x; - int16_t temple_y; - } check_temple; + } harvest; }; }; @@ -211,85 +84,46 @@ struct i2c_cmd_sensorboard_scanner_algo { /****/ -#define I2C_REQ_MECHBOARD_STATUS 0x80 +#define I2C_REQ_COBBOARD_STATUS 0x80 -struct i2c_req_mechboard_status { +struct i2c_req_cobboard_status { struct i2c_cmd_hdr hdr; - - int16_t pump_left1_current; - int16_t pump_left2_current; + int16_t sickle_left1_current; }; -#define I2C_ANS_MECHBOARD_STATUS 0x81 +#define I2C_ANS_COBBOARD_STATUS 0x81 -struct i2c_ans_mechboard_status { +struct i2c_ans_cobboard_status { struct i2c_cmd_hdr hdr; - /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */ + /* mode type are defined above: I2C_COBBOARD_MODE_xxx */ uint8_t mode; -#define I2C_MECHBOARD_STATUS_F_READY 0x00 -#define I2C_MECHBOARD_STATUS_F_BUSY 0x01 -#define I2C_MECHBOARD_STATUS_F_EXCPT 0x02 +#define I2C_COBBOARD_STATUS_F_READY 0x00 +#define I2C_COBBOARD_STATUS_F_BUSY 0x01 +#define I2C_COBBOARD_STATUS_F_EXCPT 0x02 uint8_t status; - uint8_t lintel_count; - - /* flag is there if column was taken by this pump. Note that - * we should also check ADC (current) to see if the column is - * still there. */ -#define I2C_MECHBOARD_COLUMN_L1 0x01 -#define I2C_MECHBOARD_COLUMN_L2 0x02 -#define I2C_MECHBOARD_COLUMN_R1 0x04 -#define I2C_MECHBOARD_COLUMN_R2 0x08 - uint8_t column_flags; - - int16_t pump_left1; - int16_t pump_right1; - int16_t pump_left2; - int16_t pump_right2; - -#define I2C_MECHBOARD_CURRENT_COLUMN 85 - int16_t pump_right1_current; - int16_t pump_right2_current; - - uint16_t servo_lintel_left; - uint16_t servo_lintel_right; + uint8_t cob_count; }; -#define I2C_REQ_SENSORBOARD_STATUS 0x82 +#define I2C_REQ_BALLBOARD_STATUS 0x82 -struct i2c_req_sensorboard_status { +struct i2c_req_ballboard_status { struct i2c_cmd_hdr hdr; - - /* position sent by mainboard */ - int16_t x; - int16_t y; - int16_t a; - - /* PWM for pickup */ - uint8_t enable_pickup_wheels; }; -#define I2C_ANS_SENSORBOARD_STATUS 0x83 +#define I2C_ANS_BALLBOARD_STATUS 0x83 -struct i2c_ans_sensorboard_status { +struct i2c_ans_ballboard_status { struct i2c_cmd_hdr hdr; - uint8_t status; -#define I2C_OPPONENT_NOT_THERE -1000 - int16_t opponent_x; - int16_t opponent_y; - int16_t opponent_a; - int16_t opponent_d; -#define I2C_SCAN_DONE 1 -#define I2C_SCAN_MAX_COLUMN 2 - uint8_t scan_status; +#define I2C_BALLBOARD_STATUS_F_READY 0x00 +#define I2C_BALLBOARD_STATUS_F_BUSY 0x01 +#define I2C_BALLBOARD_STATUS_F_EXCPT 0x02 + uint8_t status; -#define I2C_COLUMN_NO_DROPZONE -1 - int8_t dropzone_h; - int16_t dropzone_x; - int16_t dropzone_y; + uint8_t ball_count; }; #endif /* _I2C_PROTOCOL_H_ */ diff --git a/projects/microb2010/mechboard/.config b/projects/microb2010/mechboard/.config deleted file mode 100644 index fd14974..0000000 --- a/projects/microb2010/mechboard/.config +++ /dev/null @@ -1,279 +0,0 @@ -# -# 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_GEOMETRY_CREATE_CONFIG is not set -CONFIG_MODULE_SCHEDULER=y -# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set -# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set -# CONFIG_MODULE_SCHEDULER_TIMER0 is not set -CONFIG_MODULE_SCHEDULER_MANUAL=y -CONFIG_MODULE_TIME=y -# CONFIG_MODULE_TIME_CREATE_CONFIG is not set -# CONFIG_MODULE_TIME_EXT is not set -# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set - -# -# Communication modules -# - -# -# 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 -# -# CONFIG_MODULE_ROBOT_SYSTEM is not set -# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set -# CONFIG_MODULE_POSITION_MANAGER is not set -# CONFIG_MODULE_TRAJECTORY_MANAGER is not set -CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=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/mechboard/Makefile b/projects/microb2010/mechboard/Makefile deleted file mode 100644 index 34efb9f..0000000 --- a/projects/microb2010/mechboard/Makefile +++ /dev/null @@ -1,35 +0,0 @@ -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_mechboard.c commands.c -SRC += i2c_protocol.c sensor.c actuator.c cs.c -SRC += arm_xy.c state.c ax12_user.c arm_highlevel.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/mechboard/actuator.c b/projects/microb2010/mechboard/actuator.c deleted file mode 100644 index a36a70c..0000000 --- a/projects/microb2010/mechboard/actuator.c +++ /dev/null @@ -1,163 +0,0 @@ -/* - * 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 -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "../common/i2c_commands.h" -#include "actuator.h" -#include "ax12_user.h" -#include "main.h" - -#define FINGER_DEBUG(args...) DEBUG(E_USER_FINGER, args) -#define FINGER_NOTICE(args...) NOTICE(E_USER_FINGER, args) -#define FINGER_ERROR(args...) ERROR(E_USER_FINGER, args) - -struct finger { - int8_t event; - uint16_t destination; -}; - -struct finger finger; - -static void finger_goto_cb(void *); - -/* schedule a single event for the finger */ -static void finger_schedule_event(struct finger *finger) -{ - uint8_t flags; - int8_t ret; - - IRQ_LOCK(flags); - ret = scheduler_add_event(SCHEDULER_SINGLE, - (void *)finger_goto_cb, - finger, 1, ARM_PRIO); - if (ret == -1) { - IRQ_UNLOCK(flags); - FINGER_ERROR("Cannot load finger event"); - return; - } - finger->event = ret; - IRQ_UNLOCK(flags); -} - -static void finger_goto_cb(void *data) -{ - uint8_t flags; - struct finger *finger = data; - uint16_t position; - - IRQ_LOCK(flags); - finger->event = -1; - position = finger->destination; - IRQ_UNLOCK(flags); - FINGER_DEBUG("goto_cb %d", position); - ax12_user_write_int(&gen.ax12, FINGER_AX12, - AA_GOAL_POSITION_L, position); -} - -/* load an event that will move the ax12 for us */ -void finger_goto(uint16_t position) -{ - uint8_t flags; - FINGER_NOTICE("goto %d", position); - - IRQ_LOCK(flags); - finger.destination = position; - if (finger.event != -1) { - IRQ_UNLOCK(flags); - return; /* nothing to do, event already scheduled */ - } - IRQ_UNLOCK(flags); - finger_schedule_event(&finger); -} - -static void finger_init(void) -{ - finger.event = -1; - finger.destination = 0; - /* XXX set pos ? */ -} - -uint16_t finger_get_goal_pos(void) -{ - return finger.destination; -} - -uint8_t finger_get_side(void) -{ - if (finger.destination <= FINGER_CENTER) - return I2C_LEFT_SIDE; - return I2C_RIGHT_SIDE; -} - -/**********/ - -#define SERVO_LEFT_OUT 400 -#define SERVO_LEFT_1LIN 520 -#define SERVO_LEFT_2LIN 485 -#define SERVO_RIGHT_OUT 290 -#define SERVO_RIGHT_1LIN 155 -#define SERVO_RIGHT_2LIN 180 - -void servo_lintel_out(void) -{ - mechboard.servo_lintel_left = SERVO_LEFT_OUT; - mechboard.servo_lintel_right = SERVO_RIGHT_OUT; -} - -void servo_lintel_1lin(void) -{ - mechboard.servo_lintel_left = SERVO_LEFT_1LIN; - mechboard.servo_lintel_right = SERVO_RIGHT_1LIN; -} - -void servo_lintel_2lin(void) -{ - mechboard.servo_lintel_left = SERVO_LEFT_2LIN; - mechboard.servo_lintel_right = SERVO_RIGHT_2LIN; -} - -/**********/ - -void actuator_init(void) -{ - finger_init(); - servo_lintel_out(); -} diff --git a/projects/microb2010/mechboard/actuator.h b/projects/microb2010/mechboard/actuator.h deleted file mode 100644 index c55f564..0000000 --- a/projects/microb2010/mechboard/actuator.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * 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 $ - * - */ - -#define FINGER_RIGHT 666 -#define FINGER_RIGHT_RELAX 621 -#define FINGER_CENTER_RIGHT 491 -#define FINGER_CENTER 490 -#define FINGER_CENTER_LEFT 489 -#define FINGER_LEFT 340 -#define FINGER_LEFT_RELAX 385 - -void finger_goto(uint16_t position); -uint16_t finger_get_goal_pos(void); -uint8_t finger_get_side(void); - -void servo_lintel_out(void); -void servo_lintel_1lin(void); -void servo_lintel_2lin(void); - -void actuator_init(void); - diff --git a/projects/microb2010/mechboard/adc_config.h b/projects/microb2010/mechboard/adc_config.h deleted file mode 100644 index e69de29..0000000 diff --git a/projects/microb2010/mechboard/arm_highlevel.c b/projects/microb2010/mechboard/arm_highlevel.c deleted file mode 100644 index abda4d4..0000000 --- a/projects/microb2010/mechboard/arm_highlevel.c +++ /dev/null @@ -1,644 +0,0 @@ -/* - * 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: arm_highlevel.c,v 1.4 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "arm_xy.h" -#include "arm_highlevel.h" - -#define WRIST_ANGLE_PUMP1 -2 /* in degree */ -#define WRIST_ANGLE_PUMP2 103 /* in degree */ - -struct arm *arm_num2ptr(uint8_t arm_num) -{ - switch (arm_num) { - case ARM_LEFT_NUM: - return &left_arm; - case ARM_RIGHT_NUM: - return &right_arm; - default: - return NULL; - } -} - -#define ARM_MAX_H 110 -#define ARM_STRAIGHT_D 254 -#define ARM_STRAIGHT_H 0 - -void arm_goto_straight(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_STRAIGHT_D, ARM_STRAIGHT_H, angle); -} - -/* position to get a column */ -#define ARM_GET_D 60 -#define ARM_GET_H -140 - -void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_GET_D, ARM_GET_H, angle); -} - -/* position to get a column */ -#define ARM_PREPARE_D 62 -#define ARM_PREPARE_H -133 - -void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_PREPARE_D, ARM_PREPARE_H, angle); -} - -#define ARM_INTERMEDIATE_D (65) -#define ARM_INTERMEDIATE_H (-115) - -void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_INTERMEDIATE_D, ARM_INTERMEDIATE_H, angle); -} - -/* used in prepare pickup */ -#define ARM_INTERMEDIATE_FRONT_D (90) -#define ARM_INTERMEDIATE_FRONT_H (-105) - -void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_INTERMEDIATE_FRONT_D, - ARM_INTERMEDIATE_FRONT_H, angle); -} - -/* ****** */ - -#define ARM_PREPARE_EJECT_D (70) -#define ARM_PREPARE_EJECT_H (-50) - -void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_PREPARE_EJECT_D, ARM_PREPARE_EJECT_H, angle); -} - -#define ARM_EJECT_D (200) -#define ARM_EJECT_H (30) - -void arm_goto_eject(uint8_t arm_num, uint8_t pump_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - arm_do_xy(arm, ARM_EJECT_D, ARM_EJECT_H, angle); -} - -/* ****** */ - -#define ARM_PREPARE_GET_LINTEL_INSIDE1_D 90 -#define ARM_PREPARE_GET_LINTEL_INSIDE1_H -75 -#define ARM_PREPARE_GET_LINTEL_INSIDE1_A -30 -void arm_goto_prepare_get_lintel_inside1(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D, - ARM_PREPARE_GET_LINTEL_INSIDE1_H, - ARM_PREPARE_GET_LINTEL_INSIDE1_A); - arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D, - ARM_PREPARE_GET_LINTEL_INSIDE1_H, - ARM_PREPARE_GET_LINTEL_INSIDE1_A); -} - -#define ARM_PREPARE_GET_LINTEL_INSIDE2_D 30 -#define ARM_PREPARE_GET_LINTEL_INSIDE2_H -75 -#define ARM_PREPARE_GET_LINTEL_INSIDE2_A -30 -void arm_goto_prepare_get_lintel_inside2(uint8_t lintel_count) -{ - uint16_t d; - d = ARM_PREPARE_GET_LINTEL_INSIDE2_D; - if (lintel_count == 2) - d += 34; - arm_do_xy(&left_arm, d, - ARM_PREPARE_GET_LINTEL_INSIDE2_H, - ARM_PREPARE_GET_LINTEL_INSIDE2_A); - arm_do_xy(&right_arm, d, - ARM_PREPARE_GET_LINTEL_INSIDE2_H, - ARM_PREPARE_GET_LINTEL_INSIDE2_A); -} - -#define ARM_GET_LINTEL_INSIDE_D 10 -#define ARM_GET_LINTEL_INSIDE_H -75 -#define ARM_GET_LINTEL_INSIDE_A -30 -void arm_goto_get_lintel_inside(uint8_t lintel_count) -{ - uint16_t d; - d = ARM_GET_LINTEL_INSIDE_D; - if (lintel_count == 2) - d += 34; - arm_do_xy(&left_arm, d, - ARM_GET_LINTEL_INSIDE_H, - ARM_GET_LINTEL_INSIDE_A); - arm_do_xy(&right_arm, d, - ARM_GET_LINTEL_INSIDE_H, - ARM_GET_LINTEL_INSIDE_A); -} - -#define ARM_PREPARE_BUILD_LINTEL1_D 30 -#define ARM_PREPARE_BUILD_LINTEL1_H -50 -#define ARM_PREPARE_BUILD_LINTEL1_A -30 -void arm_goto_prepare_build_lintel1(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL1_D, - ARM_PREPARE_BUILD_LINTEL1_H, - ARM_PREPARE_BUILD_LINTEL1_A); - arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL1_D, - ARM_PREPARE_BUILD_LINTEL1_H, - ARM_PREPARE_BUILD_LINTEL1_A); -} - -#define ARM_PREPARE_BUILD_LINTEL2_D 80 -#define ARM_PREPARE_BUILD_LINTEL2_H -110 -#define ARM_PREPARE_BUILD_LINTEL2_A 60 -void arm_goto_prepare_build_lintel2(uint8_t level) -{ - int16_t h; - if (level < 3) - level = 3; - h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL2_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL2_D, - h, ARM_PREPARE_BUILD_LINTEL2_A); - arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL2_D, - h, ARM_PREPARE_BUILD_LINTEL2_A); -} - -#define ARM_PREPARE_BUILD_LINTEL3_D 205 -#define ARM_PREPARE_BUILD_LINTEL3_H -100 -#define ARM_PREPARE_BUILD_LINTEL3_A 50 -void arm_goto_prepare_build_lintel3(uint8_t level) -{ - int16_t h; - if (level < 2) - level = 2; - h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL3_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL3_D, - h, ARM_PREPARE_BUILD_LINTEL3_A); - arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL3_D, - h, ARM_PREPARE_BUILD_LINTEL3_A); -} - -#define ARM_BUILD_LINTEL_D 205 -#define ARM_BUILD_LINTEL_H -128 -#define ARM_BUILD_LINTEL_A 50 -void arm_goto_build_lintel(uint8_t level) -{ - int16_t h; - h = (int16_t)level * 30 + ARM_BUILD_LINTEL_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(&left_arm, ARM_BUILD_LINTEL_D, - h, ARM_BUILD_LINTEL_A); - arm_do_xy(&right_arm, ARM_BUILD_LINTEL_D, - h, ARM_BUILD_LINTEL_A); -} - -/* ****** */ - -#define ARM_PREPARE_GET_LINTEL_DISP_D 190 -#define ARM_PREPARE_GET_LINTEL_DISP_H -40 -#define ARM_PREPARE_GET_LINTEL_DISP_A 50 -void arm_goto_prepare_get_lintel_disp(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_DISP_D, - ARM_PREPARE_GET_LINTEL_DISP_H, - ARM_PREPARE_GET_LINTEL_DISP_A); - arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_DISP_D, - ARM_PREPARE_GET_LINTEL_DISP_H, - ARM_PREPARE_GET_LINTEL_DISP_A); -} - -#define ARM_GET_LINTEL_DISP_D 190 -#define ARM_GET_LINTEL_DISP_H -70 -#define ARM_GET_LINTEL_DISP_A 40 -void arm_goto_get_lintel_disp(void) -{ - arm_do_xy(&left_arm, ARM_GET_LINTEL_DISP_D, - ARM_GET_LINTEL_DISP_H, - ARM_GET_LINTEL_DISP_A); - arm_do_xy(&right_arm, ARM_GET_LINTEL_DISP_D, - ARM_GET_LINTEL_DISP_H, - ARM_GET_LINTEL_DISP_A); -} - -#define ARM_PREPARE_PUT_LINTEL_DISP_D 130 -#define ARM_PREPARE_PUT_LINTEL_DISP_H 0 -#define ARM_PREPARE_PUT_LINTEL_DISP_A 0 -void arm_goto_prepare_put_lintel(void) -{ - arm_do_xy(&left_arm, ARM_PREPARE_PUT_LINTEL_DISP_D, - ARM_PREPARE_PUT_LINTEL_DISP_H, - ARM_PREPARE_PUT_LINTEL_DISP_A); - arm_do_xy(&right_arm, ARM_PREPARE_PUT_LINTEL_DISP_D, - ARM_PREPARE_PUT_LINTEL_DISP_H, - ARM_PREPARE_PUT_LINTEL_DISP_A); -} - -#define ARM_PUT_LINTEL_DISP_D 30 -#define ARM_PUT_LINTEL_DISP_H -60 -#define ARM_PUT_LINTEL_DISP_A -30 -void arm_goto_put_lintel(uint8_t lintel_count) -{ - arm_do_xy(&left_arm, - ARM_PUT_LINTEL_DISP_D + lintel_count * 30, - ARM_PUT_LINTEL_DISP_H, - ARM_PUT_LINTEL_DISP_A); - arm_do_xy(&right_arm, - ARM_PUT_LINTEL_DISP_D + lintel_count * 30, - ARM_PUT_LINTEL_DISP_H, - ARM_PUT_LINTEL_DISP_A); -} - -/* ****** */ - - -#define ARM_LOADED_D 100 -#define ARM_LOADED_H 0 - -void arm_goto_loaded(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - arm_do_xy(arm, ARM_LOADED_D, ARM_LOADED_H, WRIST_ANGLE_PUMP2); -} - - -/* for columns */ -#define ARM_PREPARE_BUILD_INSIDE_D 90 -#define ARM_PREPARE_BUILD_INSIDE_H -45 - -void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (level < 2) - level = 2; - h = (int16_t)level * 30 + ARM_PREPARE_BUILD_INSIDE_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, ARM_PREPARE_BUILD_INSIDE_D, h, angle); -} - -#define ARM_PREPARE_AUTOBUILD_INSIDE_D 90 -#define ARM_PREPARE_AUTOBUILD_INSIDE_H -70 - -void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (level < 2) - level = 2; - h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_INSIDE_H; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, ARM_PREPARE_AUTOBUILD_INSIDE_D, h, angle); -} - -#define ARM_PREPARE_AUTOBUILD_OUTSIDE_D 210 /* not used, see dist below */ -#define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1 (-110) -#define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2 (-90) - -void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) - h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1; - else - h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, dist, h, angle); -} - -#define ARM_AUTOBUILD_D_P1 208 /* not used, see dist below */ -#define ARM_AUTOBUILD_D_P2 210 /* not used, see dist below */ -#define ARM_AUTOBUILD_D_P1_OFFSET (-2) -#define ARM_AUTOBUILD_D_P2_OFFSET (0) -#define ARM_AUTOBUILD_H_P1 (-133) -#define ARM_AUTOBUILD_H_P2 (-130) - -void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t angle = pump_num2angle(pump_num); - int16_t h; - if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) { - h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P1; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P1_OFFSET, h, angle); - } - else { - h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P2; - if (h > ARM_MAX_H) - h = ARM_MAX_H; - arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P2_OFFSET, h, angle); - } -} - -#define ARM_PUSH_TEMPLE_D 170 -#define ARM_PUSH_TEMPLE_H -165 - -void arm_goto_push_temple(uint8_t arm_num, uint8_t level) -{ - struct arm *arm = arm_num2ptr(arm_num); - int16_t h = ARM_PUSH_TEMPLE_H; - - /* level can be 0 or 1 */ - if (level) - h += 30; - arm_do_xy(arm, ARM_PUSH_TEMPLE_D, h, WRIST_ANGLE_PUMP1); -} - -#define ARM_PREPARE_PUSH_TEMPLE_D 120 -#define ARM_PREPARE_PUSH_TEMPLE_H -60 - -void arm_goto_prepare_push_temple(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_D, - ARM_PREPARE_PUSH_TEMPLE_H, WRIST_ANGLE_PUMP1); -} - -#define ARM_PUSH_TEMPLE_DISC_D1 215 -#define ARM_PUSH_TEMPLE_DISC_H1 -100 -#define ARM_PUSH_TEMPLE_DISC_D2 190 -#define ARM_PUSH_TEMPLE_DISC_H2 -65 - -void arm_goto_push_temple_disc(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - int8_t pump_num; - - pump_num = arm_get_busy_pump(arm_num); - if (pump_num == -1) - arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D1, - ARM_PUSH_TEMPLE_DISC_H1, WRIST_ANGLE_PUMP1); - else - arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D2, - ARM_PUSH_TEMPLE_DISC_H2, WRIST_ANGLE_PUMP1); -} - -#define ARM_PREPARE_PUSH_TEMPLE_DISC_D 100 -#define ARM_PREPARE_PUSH_TEMPLE_DISC_H -80 - -void arm_goto_prepare_push_temple_disc(uint8_t arm_num) -{ - struct arm *arm = arm_num2ptr(arm_num); - arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_DISC_D, - ARM_PREPARE_PUSH_TEMPLE_DISC_H, WRIST_ANGLE_PUMP1); -} - -void arm_prepare_free_pumps(void) -{ - int8_t pump_num; - - pump_num = arm_get_free_pump(ARM_LEFT_NUM); - if (pump_num != -1) - arm_goto_prepare_get(ARM_LEFT_NUM, pump_num); - pump_num = arm_get_free_pump(ARM_RIGHT_NUM); - if (pump_num != -1) - arm_goto_prepare_get(ARM_RIGHT_NUM, pump_num); -} - - -/* return the id of a free pump on this arm */ -int8_t arm_get_free_pump(uint8_t arm_num) -{ - switch (arm_num) { - case ARM_LEFT_NUM: - if (pump_is_free(PUMP_LEFT1_NUM) && - pump_is_free(PUMP_LEFT2_NUM)) - return PUMP_LEFT1_NUM; - else if (pump_is_free(PUMP_LEFT2_NUM)) - return PUMP_LEFT2_NUM; - return -1; - case ARM_RIGHT_NUM: - if (pump_is_free(PUMP_RIGHT1_NUM) && - pump_is_free(PUMP_RIGHT2_NUM)) - return PUMP_RIGHT1_NUM; - else if (pump_is_free(PUMP_RIGHT2_NUM)) - return PUMP_RIGHT2_NUM; - return -1; - default: - return -1; - } -} - -/* return the id of a busy pump on this arm */ -int8_t arm_get_busy_pump(uint8_t arm_num) -{ - switch (arm_num) { - case ARM_LEFT_NUM: - if (pump_is_busy(PUMP_LEFT2_NUM)) - return PUMP_LEFT2_NUM; - else if (pump_is_busy(PUMP_LEFT1_NUM)) - return PUMP_LEFT1_NUM; - return -1; - case ARM_RIGHT_NUM: - if (pump_is_busy(PUMP_RIGHT2_NUM)) - return PUMP_RIGHT2_NUM; - else if (pump_is_busy(PUMP_RIGHT1_NUM)) - return PUMP_RIGHT1_NUM; - return -1; - default: - return -1; - } -} - -uint8_t arm_wait_both(uint8_t mask) -{ - uint8_t ret; - ret = arm_wait_traj_end(&left_arm, mask); - if (ret != ARM_TRAJ_END && ret != ARM_TRAJ_NEAR) - return ret; - return arm_wait_traj_end(&right_arm, mask); -} - -uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask) -{ - if (left && right) - return arm_wait_both(mask); - if (left) - return arm_wait_traj_end(&left_arm, mask); - if (right) - return arm_wait_traj_end(&right_arm, mask); - return ARM_TRAJ_END; -} - -/*********************/ - -int16_t *pump_num2ptr(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - return &mechboard.pump_left1; - case PUMP_RIGHT1_NUM: - return &mechboard.pump_right1; - case PUMP_LEFT2_NUM: - return &mechboard.pump_left2; - case PUMP_RIGHT2_NUM: - return &mechboard.pump_right2; - default: - return NULL; - } -} - -void pump_set(uint8_t pump_num, int16_t val) -{ - int16_t *pump_ptr = pump_num2ptr(pump_num); - - *pump_ptr = val; - - switch (pump_num) { - case PUMP_RIGHT1_NUM: - pwm_ng_set(RIGHT_PUMP1_PWM, val); - break; - case PUMP_RIGHT2_NUM: - pwm_ng_set(RIGHT_PUMP2_PWM, val); - break; - - /* no pwm, it's remote */ - case PUMP_LEFT1_NUM: - case PUMP_LEFT2_NUM: - default: - break; - } -} - -int16_t pump_num2angle(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - case PUMP_RIGHT1_NUM: - return WRIST_ANGLE_PUMP1; - case PUMP_LEFT2_NUM: - case PUMP_RIGHT2_NUM: - return WRIST_ANGLE_PUMP2; - default: - return 0; - } -} - -void pump_mark_busy(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L1; - break; - case PUMP_RIGHT1_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R1; - break; - case PUMP_LEFT2_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L2; - break; - case PUMP_RIGHT2_NUM: - mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R2; - break; - default: - break; - } - -} - -void pump_mark_free(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L1); - break; - case PUMP_RIGHT1_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R1); - break; - case PUMP_LEFT2_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L2); - break; - case PUMP_RIGHT2_NUM: - mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R2); - break; - default: - break; - } - -} - -uint8_t pump_is_free(uint8_t pump_num) -{ - switch (pump_num) { - case PUMP_LEFT1_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1); - case PUMP_RIGHT1_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1); - case PUMP_LEFT2_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2); - case PUMP_RIGHT2_NUM: - return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2); - default: - return 0; - } -} - -uint8_t pump_is_busy(uint8_t pump_num) -{ - return !pump_is_free(pump_num); -} diff --git a/projects/microb2010/mechboard/arm_highlevel.h b/projects/microb2010/mechboard/arm_highlevel.h deleted file mode 100644 index eaf3ae2..0000000 --- a/projects/microb2010/mechboard/arm_highlevel.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * 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: arm_highlevel.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#define ARM_LEFT_NUM 0 -#define ARM_RIGHT_NUM 1 - -#define PUMP_LEFT1_NUM 0 -#define PUMP_RIGHT1_NUM 1 -#define PUMP_LEFT2_NUM 2 -#define PUMP_RIGHT2_NUM 3 - -struct arm *arm_num2ptr(uint8_t arm_num); - -void arm_goto_straight(uint8_t arm_num, uint8_t pump_num); -void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num); -void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num); -void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num); -void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num); -void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num); -void arm_goto_eject(uint8_t arm_num, uint8_t pump_num); -void arm_goto_loaded(uint8_t arm_num); -void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, - uint8_t level); -void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, - uint8_t level); -void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist); -void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num, - uint8_t level, uint8_t dist); - -void arm_goto_prepare_get_lintel_inside1(void); -void arm_goto_prepare_get_lintel_inside2(uint8_t lintel_count); -void arm_goto_get_lintel_inside(uint8_t lintel_count); -void arm_goto_prepare_build_lintel1(void); -void arm_goto_prepare_build_lintel2(uint8_t level); -void arm_goto_prepare_build_lintel3(uint8_t level); -void arm_goto_build_lintel(uint8_t level); - -void arm_goto_prepare_get_lintel_disp(void); -void arm_goto_get_lintel_disp(void); - -void arm_goto_prepare_put_lintel(void); -void arm_goto_put_lintel(uint8_t lintel_count); -void arm_goto_prepare_push_temple(uint8_t arm_num); -void arm_goto_push_temple(uint8_t arm_num, uint8_t level); -void arm_goto_prepare_push_temple_disc(uint8_t arm_num); -void arm_goto_push_temple_disc(uint8_t arm_num); - -void arm_prepare_free_pumps(void); -uint8_t arm_wait_both(uint8_t mask); -uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask); - -/* return the id of the free pump for the arm, or return -1 */ -int8_t arm_get_free_pump(uint8_t arm_num); -int8_t arm_get_busy_pump(uint8_t arm_num); - -#define PUMP_ON 3400 -#define PUMP_OFF 0 -#define PUMP_REVERSE -3400 - -void pump_set(uint8_t pump_num, int16_t val); -int16_t pump_num2angle(uint8_t pump_num); -void pump_mark_busy(uint8_t pump_num); -void pump_mark_free(uint8_t pump_num); -uint8_t pump_is_free(uint8_t pump_num); -uint8_t pump_is_busy(uint8_t pump_num); diff --git a/projects/microb2010/mechboard/arm_xy.c b/projects/microb2010/mechboard/arm_xy.c deleted file mode 100644 index 2690844..0000000 --- a/projects/microb2010/mechboard/arm_xy.c +++ /dev/null @@ -1,809 +0,0 @@ -/* - * 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: arm_xy.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ - * - * Fabrice DESCLAUX - * Olivier MATZ - */ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "main.h" -#include "cmdline.h" -#include "arm_xy.h" -#include "ax12_user.h" - -#define ARM_DEBUG(args...) DEBUG(E_USER_ARM, args) -#define ARM_NOTICE(args...) NOTICE(E_USER_ARM, args) -#define ARM_ERROR(args...) ERROR(E_USER_ARM, args) - -#define DEG(x) (((double)(x)) * (180.0 / M_PI)) -#define RAD(x) (((double)(x)) * (M_PI / 180.0)) -#define M_2PI (2*M_PI) - -/* physical location/dimensions of arm */ -#define ARM_S_LEN 124. -#define ARM_E_LEN 130. - -/* timeout after 1 second if position is not reached */ -#define ARM_GLOBAL_TIMEOUT 1000000L - -/* timeout 100ms after position is reached if not in window */ -#define ARM_WINDOW_TIMEOUT 200000L - -/* default (template) period, but real one is variable */ -#define ARM_PERIOD 50000L -#define ARM_MAX_DIST 40L - -/* we pos reached, check arm in window every period */ -#define ARM_SURVEY_PERIOD 25000UL /* in us */ - -/* number of steps/s */ -#define ARM_AX12_MAX_SPEED (800L) - -/* Maximum number of steps in one ARM_PERIOD */ -#define ARM_MAX_E (((ARM_AX12_MAX_SPEED*ARM_PERIOD)/1000000L)) -/* 4000 steps/CS => 800step/ms */ -#define ARM_MAX_S ((800L*ARM_PERIOD)/1000L) - - -/* window limits in ax12/cs unit */ -#define ARM_SHOULDER_WINDOW_POS 250 -#define ARM_ELBOW_WINDOW_POS 8 -#define ARM_ELBOW_WINDOW_SPEED 100 -#define ARM_WRIST_WINDOW_POS 8 -#define ARM_WRIST_WINDOW_SPEED 100 - -/* default and max speeds */ -#define SHOULDER_DEFAULT_SPEED 800 -#define ELBOW_DEFAULT_SPEED 0x3ff -#define SHOULDER_MAX_SPEED 10000 -#define ELBOW_MAX_SPEED 0x3ff - -/* window status flags */ -#define SHOULDER_NOT_IN_WIN 1 -#define ELBOW_NOT_IN_WIN 2 -#define WRIST_NOT_IN_WIN 4 - -static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out); -static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot); -static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad); -static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out); -static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot); -static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad); - -static void arm_schedule_event(struct arm *arm, uint32_t time); - -struct arm left_arm = { - .config = { - .wrist_angle_deg2robot = wrist_angle_deg2robot_l, - .angle_rad2robot = angle_rad2robot_l, - .angle_robot2rad = angle_robot2rad_l, - .elbow_ax12 = L_ELBOW_AX12, - .wrist_ax12 = L_WRIST_AX12, - }, -}; - -struct arm right_arm = { - .config = { - .wrist_angle_deg2robot = wrist_angle_deg2robot_r, - .angle_rad2robot = angle_rad2robot_r, - .angle_robot2rad = angle_robot2rad_r, - .elbow_ax12 = R_ELBOW_AX12, - .wrist_ax12 = R_WRIST_AX12, - }, -}; - -/* process shoulder + elbow angles from height and distance */ -int8_t cart2angle(int32_t h_int, int32_t d_int, double *alpha, double *beta) -{ - double h, d, l; - double elbow, shoulder; - - d = d_int; - h = h_int; - l = sqrt(d*d + h*h); - if (l > (ARM_S_LEN + ARM_E_LEN)) - return -1; - - elbow = -acos((d*d + h*h - ARM_E_LEN*ARM_E_LEN - - ARM_S_LEN*ARM_S_LEN) / (2*ARM_S_LEN*ARM_E_LEN)); - shoulder = atan2(h,d) - atan2(ARM_E_LEN*sin(elbow), - ARM_S_LEN+ARM_E_LEN*cos(elbow)); - - *alpha = shoulder; - *beta = elbow; - - return 0; -} - - -/* process height and distance from shoulder + elbow angles */ -void angle2cart(double alpha, double beta, int32_t *h, int32_t *d) -{ - double tmp_a; - int32_t tmp_h, tmp_d; - - tmp_h = ARM_S_LEN * sin(alpha); - tmp_d = ARM_S_LEN * cos(alpha); - - tmp_a = alpha+beta; - *h = tmp_h + ARM_E_LEN * sin(tmp_a); - *d = tmp_d + ARM_E_LEN * cos(tmp_a); -} - -/*** left arm */ - -#define ARM_LEFT_S_OFFSET -1150. -#define ARM_LEFT_E_OFFSET 476. -#define ARM_LEFT_W_OFFSET 90. - -static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out) -{ - *wrist_out = -wrist_deg * 3.41 + ARM_LEFT_W_OFFSET; -} - -/* convert an angle in radian into a robot-specific unit - * for shoulder and elbow for LEFT ARM*/ -static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot) -{ - *shoulder_robot = shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_LEFT_S_OFFSET; - *elbow_robot = -elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_LEFT_E_OFFSET; -} - -/* convert a robot-specific unit into an angle in radian - * for shoulder and elbow for LEFT ARM */ -static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad) -{ - *shoulder_rad = ((shoulder_robot - ARM_LEFT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); - *elbow_rad = -((elbow_robot - ARM_LEFT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); -} - -/*** right arm */ - -#define ARM_RIGHT_S_OFFSET 1150. -#define ARM_RIGHT_E_OFFSET 673. -#define ARM_RIGHT_W_OFFSET 935. - -static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out) -{ - *wrist_out = wrist_deg * 3.41 + ARM_RIGHT_W_OFFSET; -} - -/* convert an angle in radian into a robot-specific unit - * for shoulder and elbow */ -static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, - double *shoulder_robot, double *elbow_robot) -{ - *shoulder_robot = -shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_RIGHT_S_OFFSET; - *elbow_robot = elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_RIGHT_E_OFFSET; -} - -/* convert a robot-specific unit into an angle in radian - * for shoulder and elbow */ -static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad) -{ - *shoulder_rad = -((shoulder_robot - ARM_RIGHT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); - *elbow_rad = ((elbow_robot - ARM_RIGHT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); -} - - -/* - * Fill the arm_status structure according to request. - * - * return: - * 0 => success - * < 0 => error - */ -static int8_t arm_do_step(struct arm *arm) -{ - const struct arm_config *conf = &arm->config; - const struct arm_request *req = &arm->req; - struct arm_status *status = &arm->status; - - int8_t ret; - int32_t diff_h, diff_d; /* position delta in steps */ - int32_t next_h, next_d; /* next position in steps */ - int32_t l; /* distance between cur pos and next pos */ - - double as_cur_rad, ae_cur_rad; /* current angle in rad */ - double as_next_rad, ae_next_rad; /* next angle in rad */ - double as_cur, ae_cur; /* current angle in angle_steps */ - double as_next, ae_next; /* next angle in angle_steps */ - - int32_t as_diff, ae_diff; /* angle delta in angle_steps */ - int32_t s_speed, e_speed; /* elbow/shoulder speed in angle_steps */ - - double as_coef, ae_coef; - - /* process diff between final request and current pos */ - diff_h = req->h_mm - status->h_mm; - diff_d = req->d_mm - status->d_mm; - ARM_NOTICE("goal:d=%ld,h=%ld cur:d=%ld,h=%ld diff:d=%ld,h=%ld", - req->d_mm, req->h_mm, status->d_mm, status->h_mm, - diff_d, diff_h); - - /* if distance to next point is too large, saturate it */ - l = sqrt(diff_h*diff_h + diff_d*diff_d); - if (l > ARM_MAX_DIST) { - diff_h = diff_h * ARM_MAX_DIST / l; - diff_d = diff_d * ARM_MAX_DIST / l; - } - ARM_NOTICE("l=%ld ; after max dist: diff:d=%ld,h=%ld", l, diff_d, diff_h); - - /* process next position */ - next_h = status->h_mm + diff_h; - next_d = status->d_mm + diff_d; - ARM_DEBUG("next:d=%ld,h=%ld", next_d, next_h); - - /* calculate the current angle of arm in radian */ - ret = cart2angle(status->h_mm, status->d_mm, &as_cur_rad, &ae_cur_rad); - if (ret) - return ret; - ARM_DEBUG("as_cur_rad=%f ae_cur_rad=%f", as_cur_rad, ae_cur_rad); - - /* calculate the next angle of arm in radian */ - ret = cart2angle(next_h, next_d, &as_next_rad, &ae_next_rad); - if (ret) - return ret; - ARM_DEBUG("as_next_rad=%f ae_next_rad=%f", as_next_rad, ae_next_rad); - - /* convert radian in angle_steps */ - conf->angle_rad2robot(as_cur_rad, ae_cur_rad, - &as_cur, &ae_cur); - ARM_DEBUG("as_cur=%f ae_cur=%f", as_cur, ae_cur); - conf->angle_rad2robot(as_next_rad, ae_next_rad, - &as_next, &ae_next); - ARM_DEBUG("as_next=%f ae_next=%f", as_next, ae_next); - - /* process angle delta in angle_steps */ - as_diff = as_next - as_cur; - ae_diff = ae_next - ae_cur; - ARM_DEBUG("as_diff=%ld ae_diff=%ld", as_diff, ae_diff); - - /* update position status */ - status->h_mm = next_h; - status->d_mm = next_d; - status->shoulder_angle_steps = as_next; - status->elbow_angle_steps = ae_next; - status->shoulder_angle_rad = as_next_rad; - status->elbow_angle_rad = ae_next_rad; - - /* we reached destination, nothing to do */ - if (as_diff == 0 && ae_diff == 0) { - status->shoulder_speed = SHOULDER_DEFAULT_SPEED; - status->elbow_speed = ELBOW_DEFAULT_SPEED; - status->next_update_time = 0; - ARM_NOTICE("reaching end"); - return 0; - } - - /* test if one actuator is already in position */ - if (as_diff == 0) { - ARM_DEBUG("shoulder reached destination"); - ae_coef = (double)ARM_MAX_E / (double)ae_diff; - status->next_update_time = ARM_PERIOD * ABS(ae_coef); - e_speed = ABS(ae_coef) * ABS(ae_diff); - s_speed = ARM_MAX_S; - } - else if (ae_diff == 0) { - ARM_DEBUG("elbow reached destination"); - as_coef = (double)ARM_MAX_S / (double)as_diff; - status->next_update_time = ARM_PERIOD / ABS(as_coef); - e_speed = ARM_MAX_E; - s_speed = ABS(as_coef) * ABS(as_diff); - } - - else { - as_coef = (double)ARM_MAX_S / (double)as_diff; - ae_coef = (double)ARM_MAX_E / (double)ae_diff; - - ARM_DEBUG("as_coef=%f ae_coef=%f", as_coef, ae_coef); - - /* if elbow is limitating */ - if (ABS(as_coef) >= ABS(ae_coef)) { - ARM_DEBUG("elbow limit"); - status->next_update_time = ARM_PERIOD / ABS(ae_coef); - s_speed = ABS(ae_coef) * ABS(as_diff); - e_speed = ABS(ae_coef) * ABS(ae_diff); - } - /* else, shoulder is limitating */ - else { - ARM_DEBUG("shoulder limit"); - status->next_update_time = ARM_PERIOD / ABS(as_coef); - s_speed = ABS(as_coef) * ABS(as_diff); - e_speed = ABS(as_coef) * ABS(ae_diff); - } - } - - ARM_NOTICE("next update: %ld", status->next_update_time); - - /* convert speed in specific unit */ - status->shoulder_speed = (s_speed * CS_PERIOD) / ARM_PERIOD; - status->elbow_speed = (e_speed * 0x3ff) / ARM_MAX_E; - - ARM_DEBUG("speeds: s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); - - /* avoid limits */ - if (status->shoulder_speed == 0) - status->shoulder_speed = 1; - if (status->elbow_speed == 0) - status->elbow_speed = 1; - if (status->shoulder_speed >= SHOULDER_MAX_SPEED) - status->shoulder_speed = SHOULDER_MAX_SPEED; - if (status->elbow_speed >= ELBOW_MAX_SPEED) - status->elbow_speed = ELBOW_MAX_SPEED; - - ARM_DEBUG("speeds (sat): s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); - - return 0; -} - -static void arm_delete_event(struct arm *arm) -{ - if (arm->status.event == -1) - return; - ARM_DEBUG("Delete arm event"); - scheduler_del_event(arm->status.event); - arm->status.event = -1; -} - -/* write values to ax12 + cs */ -static void arm_apply(struct arm *arm) -{ - struct cs_block *csb = arm->config.csb; - const struct arm_status *st = &arm->status; - - ARM_DEBUG("arm_apply"); - - if (arm->config.simulate) - return; - - /* set speed and pos of shoulder */ - quadramp_set_1st_order_vars(&csb->qr, - st->shoulder_speed, - st->shoulder_speed); - cs_set_consign(&csb->cs, st->shoulder_angle_steps); - - /* set speed and position of elbow */ - ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, - AA_MOVING_SPEED_L, st->elbow_speed); - ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, - AA_GOAL_POSITION_L, st->elbow_angle_steps); -} - -/* return true if one of the mask condition is true */ -uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask) -{ - if ((mask & ARM_TRAJ_END) && (arm->status.state & ARM_FLAG_IN_WINDOW)) - return ARM_TRAJ_END; - - if ((mask & ARM_TRAJ_NEAR) && (arm->status.state & ARM_FLAG_LAST_STEP)) - return ARM_TRAJ_NEAR; - - if ((mask & ARM_TRAJ_TIMEOUT) && (arm->status.state & ARM_FLAG_TIMEOUT)) - return ARM_TRAJ_TIMEOUT; - - if ((mask & ARM_TRAJ_ERROR) && (arm->status.state & ARM_FLAG_ERROR)) - return ARM_TRAJ_ERROR; - - return 0; -} - -uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask) -{ - uint8_t ret; - while(1) { - ret = arm_test_traj_end(arm, mask); - if (ret) - return ret; - } -} - -/* return true if one of the mask condition is true */ -uint8_t arm_in_window(struct arm *arm, uint8_t *status) -{ - int8_t err; -/* uint16_t spd; */ - int16_t pos; - int32_t cs_err; - - *status = 0; - - if (arm->config.simulate) - return 1; - - /* shoulder, just check position */ - cs_err = cs_get_error(&arm->config.csb->cs); - if (ABS(cs_err) > ARM_SHOULDER_WINDOW_POS) - *status |= SHOULDER_NOT_IN_WIN; - -#if 0 - /* check elbow speed */ - err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, - AA_PRESENT_SPEED_L, &spd); - if (err) - goto fail; - if (spd > ARM_ELBOW_WINDOW_SPEED) - return 0; - - /* check wrist speed */ - err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, - AA_PRESENT_SPEED_L, &spd); - if (err) - goto fail; - if (spd > ARM_WRIST_WINDOW_SPEED) - return 0; -#endif - /* check elbow pos */ - err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, - AA_PRESENT_POSITION_L, (uint16_t *)&pos); - if (err) - goto fail; - if (ABS(arm->status.elbow_angle_steps - pos) > ARM_ELBOW_WINDOW_POS) - *status |= ELBOW_NOT_IN_WIN; - - /* check wrist pos */ - err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, - AA_PRESENT_POSITION_L, (uint16_t *)&pos); - if (err) - goto fail; - if (ABS(arm->status.wrist_angle_steps - pos) > ARM_WRIST_WINDOW_POS) - *status |= WRIST_NOT_IN_WIN; - - if (*status) - return 0; - - ARM_NOTICE("arm is in window (%ld us after reach pos)", - time_get_us2() - arm->status.pos_reached_time); - return 1; /* ok, we are in window */ - - fail: - return 0; -} - -/* process wrist pos and apply it. it's done only once. */ -static int8_t arm_set_wrist(struct arm *arm) -{ - int8_t err; - int32_t as_deg, ae_deg, aw_deg; - uint16_t wrist_out_u16; - double wrist_out, as_rad, ae_rad; - int16_t pos; - uint32_t diff_time; - - /* calculate the destination angle of arm in radian */ - err = cart2angle(arm->req.h_mm, arm->req.d_mm, - &as_rad, &ae_rad); - if (err) - return -1; - - /* calc angle destination */ - as_deg = DEG(as_rad); - ae_deg = DEG(ae_rad); - ARM_DEBUG("as_dest_deg=%d ae_dest_deg=%d", as_deg, ae_deg); - aw_deg = as_deg + ae_deg - arm->req.w_deg; - arm->config.wrist_angle_deg2robot(aw_deg, &wrist_out); - wrist_out_u16 = wrist_out; - - ARM_DEBUG("set wrist to %ld degrees (%d steps)", aw_deg, - wrist_out_u16); - - /* process the theorical reach time for the wrist */ - if (arm->config.simulate) { - pos = arm->status.wrist_angle_steps; - } - else { - err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, - AA_PRESENT_POSITION_L, (uint16_t *)&pos); - if (err) - pos = arm->status.wrist_angle_steps; - } - /* 600 is the number of steps/s */ - diff_time = (ABS((int16_t)wrist_out_u16 - pos) * 1000000L) / 600; - arm->status.wrist_reach_time = arm->status.start_time + diff_time; - ARM_DEBUG("wrist reach time is %ld (diff=%ld)", - arm->status.wrist_reach_time, diff_time); - - /* update current position to destination */ - arm->status.wrist_angle_steps = wrist_out_u16; - - if (arm->config.simulate) - return 0; - - /* send it to ax12 */ - ax12_user_write_int(&gen.ax12, arm->config.wrist_ax12, - AA_GOAL_POSITION_L, wrist_out_u16); - return 0; -} - -/* event callback */ -static void arm_do_xy_cb(struct arm *arm) -{ - uint8_t win_status; - - arm->status.event = -1; - - /* if consign haven't reach destination */ - if ((arm->status.state & ARM_FLAG_LAST_STEP) == 0) { - if (arm_do_step(arm)) - arm->status.state |= ARM_FLAG_ERROR; - - /* it's the first call for the traj */ - if (arm->status.state == ARM_STATE_INIT) { - arm->status.state |= ARM_FLAG_MOVING; - if (arm_set_wrist(arm)) - arm->status.state |= ARM_FLAG_ERROR; - } - - /* we have more steps to do */ - if (arm->status.next_update_time == 0) { - arm->status.state &= ~ARM_FLAG_MOVING; - arm->status.state |= ARM_FLAG_LAST_STEP; - arm->status.pos_reached_time = time_get_us2(); - } - arm_apply(arm); - } - /* last step is reached, we can check that arm is in window */ - else if ((arm->status.state & ARM_FLAG_IN_WINDOW) == 0) { - if (arm_in_window(arm, &win_status)) - arm->status.state |= ARM_FLAG_IN_WINDOW; - - /* check for window arm timeout */ - else { - microseconds t; - int32_t diff1, diff2; - t = time_get_us2(); - diff1 = t - arm->status.pos_reached_time; - diff2 = t - arm->status.wrist_reach_time; - if (diff1 > ARM_WINDOW_TIMEOUT && - diff2 > ARM_WINDOW_TIMEOUT) { - ARM_NOTICE("win timeout at %ld win_status=%x", - t, win_status); - arm->status.state |= ARM_FLAG_TIMEOUT; - } - } - } - - /* check for global arm timeout */ - if ((time_get_us2() - arm->status.start_time) > ARM_GLOBAL_TIMEOUT) { - ARM_NOTICE("global timeout at %ld", time_get_us2()); - arm->status.state |= ARM_FLAG_TIMEOUT; - } - - /* reload event if needed */ - if ((arm->status.state & ARM_FLAG_FINISHED) == ARM_FLAG_FINISHED) { - ARM_NOTICE("arm traj finished"); - return; /* no more event, position reached */ - } - if (arm->status.state & (ARM_FLAG_ERROR|ARM_FLAG_TIMEOUT)) { - ARM_NOTICE("error or timeout"); - return; /* no more event */ - } - else if (arm->status.state & ARM_FLAG_LAST_STEP) { - /* theorical position is reached, but reload an event - * for position survey (window), every 25ms */ - arm_schedule_event(arm, ARM_SURVEY_PERIOD); - } - else { - /* reload event for next position step */ - arm_schedule_event(arm, arm->status.next_update_time); - } -} - -/* schedule a single event for this arm */ -static void arm_schedule_event(struct arm *arm, uint32_t time) -{ - uint8_t flags; - int8_t ret; - - arm_delete_event(arm); - if (time < SCHEDULER_UNIT) - time = SCHEDULER_UNIT; - IRQ_LOCK(flags); - ret = scheduler_add_event(SCHEDULER_SINGLE, - (void *)arm_do_xy_cb, - arm, time/SCHEDULER_UNIT, ARM_PRIO); - if (ret == -1) { - IRQ_UNLOCK(flags); - ARM_ERROR("Cannot load arm event"); - return; - } - arm->status.event = ret; - IRQ_UNLOCK(flags); -} - -int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg) -{ - ARM_NOTICE("arm_do_xy: d_mm=%d h_mm=%d w_deg=%d", d_mm, h_mm, w_deg); - - /* remove previous event if any */ - arm_delete_event(arm); - - /* init mandatory params */ - arm->req.d_mm = d_mm; - arm->req.h_mm = h_mm; - arm->req.w_deg = w_deg; - arm->status.start_time = time_get_us2(); - arm->status.state = ARM_STATE_INIT; - - /* all the job will be done asynchronously now */ - arm_schedule_event(arm, 0); - return 0; -} - -void arm_dump(struct arm *arm) -{ - printf_P(PSTR("config: simulate=%d\r\n"), - arm->config.simulate); - printf_P(PSTR("req: d_mm=%ld h_mm=%ld w_deg=%ld\r\n"), - arm->req.d_mm, arm->req.h_mm, arm->req.w_deg); - printf_P(PSTR("status: ")); - if (arm->status.state == ARM_STATE_INIT) - printf_P(PSTR("ARM_STATE_INIT ")); - if (arm->status.state & ARM_FLAG_MOVING) - printf_P(PSTR("ARM_FLAG_MOVING ")); - if (arm->status.state & ARM_FLAG_LAST_STEP) - printf_P(PSTR("ARM_FLAG_LAST_STEP ")); - if (arm->status.state & ARM_FLAG_IN_WINDOW) - printf_P(PSTR("ARM_FLAG_IN_WINDOW ")); - if (arm->status.state & ARM_FLAG_ERROR) - printf_P(PSTR("ARM_FLAG_ERROR ")); - if (arm->status.state & ARM_FLAG_TIMEOUT) - printf_P(PSTR("ARM_FLAG_TIMEOUT ")); - printf_P(PSTR("\r\n")); - - printf_P(PSTR(" d_mm=%ld h_mm=%ld goal_w_steps=%d\r\n"), - arm->status.d_mm, arm->status.h_mm, arm->status.wrist_angle_steps); - printf_P(PSTR(" cur_shl_steps=%ld cur_elb_steps=%ld\r\n"), - arm->status.shoulder_angle_steps, arm->status.elbow_angle_steps); - printf_P(PSTR(" cur_shl_rad=%f cur_elb_rad=%f\r\n"), - arm->status.shoulder_angle_rad, arm->status.elbow_angle_rad); - printf_P(PSTR(" cur_shl_deg=%f cur_elb_deg=%f\r\n"), - DEG(arm->status.shoulder_angle_rad), DEG(arm->status.elbow_angle_rad)); - printf_P(PSTR(" event=%d next_update_time=%ld\r\n"), - arm->status.event, arm->status.next_update_time); - printf_P(PSTR(" start_time=%ld pos_reached_time=%ld wrist_reach_time=%ld\r\n"), - arm->status.start_time, arm->status.pos_reached_time, - arm->status.wrist_reach_time); -} - -#define CALIB_ANGLE (RAD(-93.)) - -void arm_calibrate(void) -{ - double shoulder, elbow; - - pwm_ng_set(LEFT_ARM_PWM, 500); - pwm_ng_set(RIGHT_ARM_PWM, -500); - wait_ms(200); - - pwm_ng_set(LEFT_ARM_PWM, 300); - pwm_ng_set(RIGHT_ARM_PWM, -300); - wait_ms(700); - - printf_P(PSTR("Init arm, please wait...")); - ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_TORQUE_ENABLE, 0x1); - ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_ALARM_SHUTDOWN, 0x04); - - angle_rad2robot_r(0, CALIB_ANGLE, &shoulder, &elbow); - ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); - ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_GOAL_POSITION_L, 628); - - angle_rad2robot_l(0, CALIB_ANGLE, &shoulder, &elbow); - ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); - ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_GOAL_POSITION_L, 394); - pwm_ng_set(LEFT_ARM_PWM, -100); - pwm_ng_set(RIGHT_ARM_PWM, 100); - - wait_ms(2000); - - cs_set_consign(&mechboard.left_arm.cs, 0); - cs_set_consign(&mechboard.right_arm.cs, 0); - encoders_spi_set_value(LEFT_ARM_ENCODER, 0); - encoders_spi_set_value(RIGHT_ARM_ENCODER, 0); - - printf_P(PSTR("ok\r\n")); -} - -/* init arm config */ -void arm_init(void) -{ - uint32_t shoulder_robot; - uint16_t elbow_robot, wrist_robot; - double shoulder_rad, elbow_rad; - int32_t h, d; - uint8_t err = 0; - - memset(&left_arm.status, 0, sizeof(left_arm.status)); - memset(&right_arm.status, 0, sizeof(right_arm.status)); - left_arm.status.event = -1; - right_arm.status.event = -1; - - arm_calibrate(); - - /* set des slopes XXX */ - - /* set maximum moving speeds */ - err |= ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); - err |= ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); - err |= ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); - err |= ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); - - /* left arm init */ - shoulder_robot = encoders_spi_get_value(LEFT_ARM_ENCODER); - err |= ax12_user_read_int(&gen.ax12, L_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); - err |= ax12_user_read_int(&gen.ax12, L_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); - - angle_robot2rad_l(shoulder_robot, elbow_robot, - &shoulder_rad, &elbow_rad); - angle2cart(shoulder_rad, elbow_rad, &h, &d); - printf_P(PSTR("left arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); - left_arm.status.h_mm = h; - left_arm.status.d_mm = d; - left_arm.status.wrist_angle_steps = wrist_robot; - left_arm.status.state = ARM_FLAG_FINISHED; - left_arm.config.csb = &mechboard.left_arm; - - /* left arm init */ - shoulder_robot = encoders_spi_get_value(RIGHT_ARM_ENCODER); - err |= ax12_user_read_int(&gen.ax12, R_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); - err |= ax12_user_read_int(&gen.ax12, R_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); - - angle_robot2rad_r(shoulder_robot, elbow_robot, - &shoulder_rad, &elbow_rad); - angle2cart(shoulder_rad, elbow_rad, &h, &d); - printf_P(PSTR("right arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); - right_arm.status.h_mm = h; - right_arm.status.d_mm = d; - right_arm.status.wrist_angle_steps = wrist_robot; - right_arm.status.state = ARM_FLAG_FINISHED; - right_arm.config.csb = &mechboard.right_arm; - - if (err) - ARM_ERROR("ARM INIT ERROR"); -} diff --git a/projects/microb2010/mechboard/arm_xy.h b/projects/microb2010/mechboard/arm_xy.h deleted file mode 100644 index 3b1407f..0000000 --- a/projects/microb2010/mechboard/arm_xy.h +++ /dev/null @@ -1,123 +0,0 @@ -/* - * 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: arm_xy.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $ - * - * Fabrice DESCLAUX - * Olivier MATZ - */ - -/* all static configuration for an arm */ -struct arm_config { - void (*wrist_angle_deg2robot)(double wrist_edg, - double *wrist_out); - - void (*angle_rad2robot)(double shoulder_deg, double elbow_deg, - double *shoulder_out, double *elbow_out); - - void (*angle_robot2rad)(double shoulder_robot, double elbow_robot, - double *shoulder_rad, double *elbow_rad); - - /* ax12 identifiers */ - int8_t elbow_ax12; - int8_t wrist_ax12; - - /* related control system */ - struct cs_block *csb; - - /* if true, don't apply to ax12 / cs */ - uint8_t simulate; -}; - -/* request for a final position, in mm */ -struct arm_request { - int32_t h_mm; - int32_t d_mm; - int32_t w_deg; -}; - -/* returned by arm_test_traj_end() */ -#define ARM_TRAJ_END 0x01 -#define ARM_TRAJ_NEAR 0x02 -#define ARM_TRAJ_TIMEOUT 0x04 -#define ARM_TRAJ_ERROR 0x08 - -#define ARM_TRAJ_ALL (ARM_TRAJ_END|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR) -#define ARM_TRAJ_ALL_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR) -#define ARM_TRAJ_END_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR) - -#define ARM_STATE_INIT 0 -#define ARM_FLAG_MOVING 0x01 /* arm is currently moving */ -#define ARM_FLAG_LAST_STEP 0x02 /* no more step is needed */ -#define ARM_FLAG_IN_WINDOW 0x04 /* arm speed and pos are ok */ -#define ARM_FLAG_TIMEOUT 0x08 /* too much time too reach pos */ -#define ARM_FLAG_ERROR 0x10 /* error */ -#define ARM_FLAG_FINISHED (ARM_FLAG_LAST_STEP | ARM_FLAG_IN_WINDOW) - -/* Describes the current position of the arm. Mainly filled by - * arm_do_step(), arm_set_wrist(), arm_do_xy_cb(), ... */ -struct arm_status { - /* current position */ - int32_t h_mm; - int32_t d_mm; - - /* wrist goal position (set once at init) */ - int16_t wrist_angle_steps; - - /* current angles in steps */ - int32_t elbow_angle_steps; - int32_t shoulder_angle_steps; - - /* current angles in radian */ - double elbow_angle_rad; - double shoulder_angle_rad; - - /* time before next update */ - uint32_t next_update_time; - - /* what speed to be applied, in specific speed unit */ - uint32_t shoulder_speed; - uint32_t elbow_speed; - - volatile int8_t state; /* see list of flags above */ - int8_t event; /* scheduler event, -1 if not running */ - - microseconds start_time; /* when we started that command */ - microseconds wrist_reach_time; /* when the wrist should reach dest */ - microseconds pos_reached_time; /* when last step is sent */ -}; - -struct arm { - struct arm_config config; - struct arm_status status; - struct arm_request req; -}; - -extern struct arm left_arm; -extern struct arm right_arm; - -uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask); -uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask); - -/* do a specific move to distance, height. This function _must_ be - * called from a context with a prio < ARM_PRIO to avoid any race - * condition. */ -int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg); - -void arm_dump(struct arm *arm); -void arm_calibrate(void); -void arm_init(void); diff --git a/projects/microb2010/mechboard/ax12_config.h b/projects/microb2010/mechboard/ax12_config.h deleted file mode 100755 index 072e8fb..0000000 --- a/projects/microb2010/mechboard/ax12_config.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef _AX12_CONFIG_H_ -#define _AX12_CONFIG_H_ - -#define AX12_MAX_PARAMS 32 - - -#endif/*_AX12_CONFIG_H_*/ diff --git a/projects/microb2010/mechboard/ax12_user.c b/projects/microb2010/mechboard/ax12_user.c deleted file mode 100644 index 9000bbd..0000000 --- a/projects/microb2010/mechboard/ax12_user.c +++ /dev/null @@ -1,305 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#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< 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< - * - * 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/mechboard/cmdline.c b/projects/microb2010/mechboard/cmdline.c deleted file mode 100644 index f784b35..0000000 --- a/projects/microb2010/mechboard/cmdline.c +++ /dev/null @@ -1,149 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#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; ierr_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), "mechboard > "); - 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/mechboard/cmdline.h b/projects/microb2010/mechboard/cmdline.h deleted file mode 100644 index e5bb569..0000000 --- a/projects/microb2010/mechboard/cmdline.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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/mechboard/commands.c b/projects/microb2010/mechboard/commands.c deleted file mode 100644 index 1c2186d..0000000 --- a/projects/microb2010/mechboard/commands.c +++ /dev/null @@ -1,150 +0,0 @@ -/* - * 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 - */ - -#include -#include -#include - -/* 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_mechboard.c */ -extern parse_pgm_inst_t cmd_event; -extern parse_pgm_inst_t cmd_color; -extern parse_pgm_inst_t cmd_arm_show; -extern parse_pgm_inst_t cmd_arm_goto; -extern parse_pgm_inst_t cmd_arm_goto_fixed; -extern parse_pgm_inst_t cmd_arm_simulate; -extern parse_pgm_inst_t cmd_finger; -extern parse_pgm_inst_t cmd_pump; -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_state4; -extern parse_pgm_inst_t cmd_state5; -extern parse_pgm_inst_t cmd_state_debug; -extern parse_pgm_inst_t cmd_state_machine; -extern parse_pgm_inst_t cmd_servo_lintel; -extern parse_pgm_inst_t cmd_pump_current; -extern parse_pgm_inst_t cmd_manivelle; -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_mechboard.c */ - (parse_pgm_inst_t *)&cmd_event, - (parse_pgm_inst_t *)&cmd_color, - (parse_pgm_inst_t *)&cmd_arm_show, - (parse_pgm_inst_t *)&cmd_arm_goto, - (parse_pgm_inst_t *)&cmd_arm_goto_fixed, - (parse_pgm_inst_t *)&cmd_arm_simulate, - (parse_pgm_inst_t *)&cmd_finger, - (parse_pgm_inst_t *)&cmd_pump, - (parse_pgm_inst_t *)&cmd_state1, - (parse_pgm_inst_t *)&cmd_state2, - (parse_pgm_inst_t *)&cmd_state3, - (parse_pgm_inst_t *)&cmd_state4, - (parse_pgm_inst_t *)&cmd_state5, - (parse_pgm_inst_t *)&cmd_state_debug, - (parse_pgm_inst_t *)&cmd_state_machine, - (parse_pgm_inst_t *)&cmd_servo_lintel, - (parse_pgm_inst_t *)&cmd_pump_current, - (parse_pgm_inst_t *)&cmd_manivelle, - (parse_pgm_inst_t *)&cmd_test, - - NULL, -}; diff --git a/projects/microb2010/mechboard/commands_ax12.c b/projects/microb2010/mechboard/commands_ax12.c deleted file mode 100644 index b052944..0000000 --- a/projects/microb2010/mechboard/commands_ax12.c +++ /dev/null @@ -1,427 +0,0 @@ -/* - * 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 - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#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/mechboard/commands_cs.c b/projects/microb2010/mechboard/commands_cs.c deleted file mode 100644 index c2937f5..0000000 --- a/projects/microb2010/mechboard/commands_cs.c +++ /dev/null @@ -1,670 +0,0 @@ -/* - * 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 - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "main.h" -#include "cs.h" -#include "cmdline.h" - -struct csb_list { - const prog_char *name; - struct cs_block *csb; -}; - -prog_char csb_left_arm_str[] = "left_arm"; -prog_char csb_right_arm_str[] = "right_arm"; -struct csb_list csb_list[] = { - { .name = csb_left_arm_str, .csb = &mechboard.left_arm }, - { .name = csb_right_arm_str, .csb = &mechboard.right_arm }, -}; - -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_arm#right_arm"; -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/mechboard/commands_gen.c b/projects/microb2010/mechboard/commands_gen.c deleted file mode 100644 index 6517ea0..0000000 --- a/projects/microb2010/mechboard/commands_gen.c +++ /dev/null @@ -1,579 +0,0 @@ -/* - * 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 - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#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; iarg1, PSTR("loop_show"))) - loop = 1; - - do { - printf_P(PSTR("SENSOR values: ")); - for (i=0; iarg1, 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; iarg3, PSTR("off"))) { - for (i=0; i - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#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 "arm_xy.h" -#include "arm_highlevel.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"))) - mechboard.flags |= bit; - else if (!strcmp_P(res->arg2, PSTR("off"))) - mechboard.flags &= bit; - else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), - (DO_ENCODERS & mechboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), - (DO_CS & mechboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), - (DO_BD & mechboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), - (DO_POWER & mechboard.flags) ? "on":"off"); - } - return; - } - - if (!strcmp_P(res->arg1, PSTR("encoders"))) - bit = DO_ENCODERS; - else if (!strcmp_P(res->arg1, PSTR("cs"))) { - if (!strcmp_P(res->arg2, PSTR("on"))) - arm_calibrate(); - 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"))) - mechboard.flags |= bit; - else if (!strcmp_P(res->arg2, PSTR("off"))) { - if (!strcmp_P(res->arg1, PSTR("cs"))) { - pwm_ng_set(LEFT_ARM_PWM, 0); - pwm_ng_set(RIGHT_ARM_PWM, 0); - } - mechboard.flags &= (~bit); - } - printf_P(PSTR("%s is %s\r\n"), res->arg1, - (bit & mechboard.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"))) { - mechboard.our_color = I2C_COLOR_RED; - } - else if (!strcmp_P(res->color, PSTR("green"))) { - mechboard.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, - }, -}; - -/**********************************************************/ -/* arm_show */ - -/* this structure is filled when cmd_arm_show is parsed successfully */ -struct cmd_arm_show_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - -/* function called when cmd_arm_show is parsed successfully */ -static void cmd_arm_show_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_show_result *res = parsed_result; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) - arm_dump(&left_arm); - else if (strcmp_P(res->arg1, PSTR("right")) == 0) - arm_dump(&right_arm); - else { - arm_dump(&left_arm); - arm_dump(&right_arm); - } -} - -prog_char str_arm_show_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg0, str_arm_show_arg0); -prog_char str_arm_show_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg1, str_arm_show_arg1); -prog_char str_arm_show_arg2[] = "show"; -parse_pgm_token_string_t cmd_arm_show_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg2, str_arm_show_arg2); - -prog_char help_arm_show[] = "Show arm status"; -parse_pgm_inst_t cmd_arm_show = { - .f = cmd_arm_show_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_show, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_show_arg0, - (prog_void *)&cmd_arm_show_arg1, - (prog_void *)&cmd_arm_show_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* arm_goto */ - -/* this structure is filled when cmd_arm_goto is parsed successfully */ -struct cmd_arm_goto_result { - fixed_string_t arg0; - fixed_string_t arg1; - int16_t arg2; - int16_t arg3; - int16_t arg4; -}; - -/* function called when cmd_arm_goto is parsed successfully */ -static void cmd_arm_goto_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_goto_result *res = parsed_result; - uint8_t err; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) { - arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4); - err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("err %x\r\n"), err); - } - else if (strcmp_P(res->arg1, PSTR("right")) == 0) { - arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4); - err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("err %x\r\n"), err); - } - else { - arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4); - arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4); - err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("left err %x\r\n"), err); - err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("right err %x\r\n"), err); - } -} - -prog_char str_arm_goto_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg0, str_arm_goto_arg0); -prog_char str_arm_goto_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_goto_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg1, str_arm_goto_arg1); -parse_pgm_token_num_t cmd_arm_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg2, INT16); -parse_pgm_token_num_t cmd_arm_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg3, INT16); -parse_pgm_token_num_t cmd_arm_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg4, INT16); - -prog_char help_arm_goto[] = "Arm goto d_mm,h_mm,w_deg"; -parse_pgm_inst_t cmd_arm_goto = { - .f = cmd_arm_goto_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_goto, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_goto_arg0, - (prog_void *)&cmd_arm_goto_arg1, - (prog_void *)&cmd_arm_goto_arg2, - (prog_void *)&cmd_arm_goto_arg3, - (prog_void *)&cmd_arm_goto_arg4, - NULL, - }, -}; - -/**********************************************************/ -/* arm_goto_fixed */ - -/* this structure is filled when cmd_arm_goto_fixed is parsed successfully */ -struct cmd_arm_goto_fixed_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; - fixed_string_t arg3; -}; - -/* function called when cmd_arm_goto_fixed is parsed successfully */ -static void cmd_arm_goto_fixed_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_goto_fixed_result *res = parsed_result; - void (*f)(uint8_t, uint8_t) = NULL; - uint8_t err, pump_num = 0; - - if (strcmp_P(res->arg2, PSTR("prepare")) == 0) - f = arm_goto_prepare_get; - else if (strcmp_P(res->arg2, PSTR("get")) == 0) - f = arm_goto_get_column; - else if (strcmp_P(res->arg2, PSTR("inter")) == 0) - f = arm_goto_intermediate_get; - else if (strcmp_P(res->arg2, PSTR("inter")) == 0) - f = arm_goto_straight; - - if (f == NULL) - return; - - /* no matter if it's left or right here */ - if (strcmp_P(res->arg3, PSTR("p1")) == 0) - pump_num = PUMP_LEFT1_NUM; - if (strcmp_P(res->arg3, PSTR("p2")) == 0) - pump_num = PUMP_LEFT2_NUM; - - /* /!\ strcmp() inverted logic do handle "both" case */ - if (strcmp_P(res->arg1, PSTR("right"))) - f(ARM_LEFT_NUM, pump_num); - if (strcmp_P(res->arg1, PSTR("left"))) - f(ARM_RIGHT_NUM, pump_num); - - if (strcmp_P(res->arg1, PSTR("right"))) { - err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("left err %x\r\n"), err); - } - if (strcmp_P(res->arg1, PSTR("left"))) { - err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); - if (err != ARM_TRAJ_END) - printf_P(PSTR("right err %x\r\n"), err); - } -} - -prog_char str_arm_goto_fixed_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg0, str_arm_goto_fixed_arg0); -prog_char str_arm_goto_fixed_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg1, str_arm_goto_fixed_arg1); -prog_char str_arm_goto_fixed_arg2[] = "prepare#get#inter#straight"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg2, str_arm_goto_fixed_arg2); -prog_char str_arm_goto_fixed_arg3[] = "p1#p2"; -parse_pgm_token_string_t cmd_arm_goto_fixed_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg3, str_arm_goto_fixed_arg3); - -prog_char help_arm_goto_fixed[] = "Goto fixed positions"; -parse_pgm_inst_t cmd_arm_goto_fixed = { - .f = cmd_arm_goto_fixed_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_goto_fixed, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_goto_fixed_arg0, - (prog_void *)&cmd_arm_goto_fixed_arg1, - (prog_void *)&cmd_arm_goto_fixed_arg2, - (prog_void *)&cmd_arm_goto_fixed_arg3, - NULL, - }, -}; - -/**********************************************************/ -/* arm_simulate */ - -/* this structure is filled when cmd_arm_simulate is parsed successfully */ -struct cmd_arm_simulate_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - -/* function called when cmd_arm_simulate is parsed successfully */ -static void cmd_arm_simulate_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_arm_simulate_result *res = parsed_result; - uint8_t val; - - if (strcmp_P(res->arg2, PSTR("simulate")) == 0) - val = 1; - else - val = 0; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) - left_arm.config.simulate = 1; - else if (strcmp_P(res->arg1, PSTR("right")) == 0) - right_arm.config.simulate = 1; - else { - left_arm.config.simulate = 1; - right_arm.config.simulate = 1; - } -} - -prog_char str_arm_simulate_arg0[] = "arm"; -parse_pgm_token_string_t cmd_arm_simulate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg0, str_arm_simulate_arg0); -prog_char str_arm_simulate_arg1[] = "left#right#both"; -parse_pgm_token_string_t cmd_arm_simulate_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg1, str_arm_simulate_arg1); -prog_char str_arm_simulate_arg2[] = "simulate#real"; -parse_pgm_token_string_t cmd_arm_simulate_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg2, str_arm_simulate_arg2); - -prog_char help_arm_simulate[] = "Simulation or real for arm"; -parse_pgm_inst_t cmd_arm_simulate = { - .f = cmd_arm_simulate_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_arm_simulate, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_arm_simulate_arg0, - (prog_void *)&cmd_arm_simulate_arg1, - (prog_void *)&cmd_arm_simulate_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* finger */ - -/* this structure is filled when cmd_finger is parsed successfully */ -struct cmd_finger_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_finger is parsed successfully */ -static void cmd_finger_parsed(void *parsed_result, __attribute__((unused)) void *data) -{ - struct cmd_finger_result *res = parsed_result; - uint16_t dest = 0; - - if (strcmp_P(res->arg1, PSTR("left")) == 0) - dest = FINGER_LEFT; - else if (strcmp_P(res->arg1, PSTR("right")) == 0) - dest = FINGER_RIGHT; - else if (strcmp_P(res->arg1, PSTR("center")) == 0) - dest = FINGER_CENTER; - finger_goto(dest); -} - -prog_char str_finger_arg0[] = "finger"; -parse_pgm_token_string_t cmd_finger_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg0, str_finger_arg0); -prog_char str_finger_arg1[] = "left#right#center"; -parse_pgm_token_string_t cmd_finger_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg1, str_finger_arg1); - -prog_char help_finger[] = "Move finger"; -parse_pgm_inst_t cmd_finger = { - .f = cmd_finger_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_finger, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_finger_arg0, - (prog_void *)&cmd_finger_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* pump */ - -/* this structure is filled when cmd_pump is parsed successfully */ -struct cmd_pump_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; -}; - -/* function called when cmd_pump is parsed successfully */ -static void cmd_pump_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_pump_result *res = parsed_result; - int8_t pump_num = 0; - int16_t val = 0; - - if (strcmp_P(res->arg1, PSTR("left1")) == 0) - pump_num = PUMP_LEFT1_NUM; - else if (strcmp_P(res->arg1, PSTR("right1")) == 0) - pump_num = PUMP_RIGHT1_NUM; - else if (strcmp_P(res->arg1, PSTR("left2")) == 0) - pump_num = PUMP_LEFT2_NUM; - else if (strcmp_P(res->arg1, PSTR("right2")) == 0) - pump_num = PUMP_RIGHT2_NUM; - - if (strcmp_P(res->arg2, PSTR("on")) == 0) - val = PUMP_ON; - else if (strcmp_P(res->arg2, PSTR("off")) == 0) - val = PUMP_OFF; - else if (strcmp_P(res->arg2, PSTR("reverse")) == 0) - val = PUMP_REVERSE; - - pump_set(pump_num, val); -} - -prog_char str_pump_arg0[] = "pump"; -parse_pgm_token_string_t cmd_pump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg0, str_pump_arg0); -prog_char str_pump_arg1[] = "left1#left2#right1#right2"; -parse_pgm_token_string_t cmd_pump_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg1, str_pump_arg1); -prog_char str_pump_arg2[] = "on#off#reverse"; -parse_pgm_token_string_t cmd_pump_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg2, str_pump_arg2); - -prog_char help_pump[] = "activate pump"; -parse_pgm_inst_t cmd_pump = { - .f = cmd_pump_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pump, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pump_arg0, - (prog_void *)&cmd_pump_arg1, - (prog_void *)&cmd_pump_arg2, - 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_mechboard_set_mode command; - - if (!strcmp_P(res->arg1, PSTR("init"))) { - state_init(); - return; - } - - if (!strcmp_P(res->arg1, PSTR("manual"))) - command.mode = I2C_MECHBOARD_MODE_MANUAL; - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - command.mode = I2C_MECHBOARD_MODE_HARVEST; - else if (!strcmp_P(res->arg1, PSTR("lazy_harvest"))) - command.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; - else if (!strcmp_P(res->arg1, PSTR("pickup"))) - command.mode = I2C_MECHBOARD_MODE_PICKUP; - else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel"))) - command.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("get_lintel"))) - command.mode = I2C_MECHBOARD_MODE_GET_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("put_lintel"))) - command.mode = I2C_MECHBOARD_MODE_PUT_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("clear"))) - command.mode = I2C_MECHBOARD_MODE_CLEAR; - else if (!strcmp_P(res->arg1, PSTR("loaded"))) - command.mode = I2C_MECHBOARD_MODE_LOADED; - else if (!strcmp_P(res->arg1, PSTR("store"))) - command.mode = I2C_MECHBOARD_MODE_STORE; - else if (!strcmp_P(res->arg1, PSTR("lazy_pickup"))) - command.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - state_set_mode(&command); -} - -prog_char str_state1_arg0[] = "mechboard"; -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#pickup#prepare_get_lintel#get_lintel#put_lintel#clear#lazy_harvest#harvest#loaded#store#lazy_pickup"; -parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); - -prog_char help_state1[] = "set mechboard 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_mechboard_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; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else - side = I2C_AUTO_SIDE; - - if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) { - command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - command.prep_pickup.side = side; - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - } - else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) { - command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC; - command.push_temple_disc.side = side; - } - - - state_set_mode(&command); -} - -prog_char str_state2_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0); -prog_char str_state2_arg1[] = "prepare_pickup#push_temple_disc"; -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#auto#center"; -parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2); - -prog_char help_state2[] = "set mechboard 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 level; -}; - -/* 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_mechboard_set_mode command; - - if (!strcmp_P(res->arg1, PSTR("prepare_build"))) { - command.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD; - command.prep_build.level_l = res->level; - command.prep_build.level_r = res->level; - } - else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) { - command.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE; - command.prep_inside.level_l = res->level; - command.prep_inside.level_r = res->level; - } - else if (!strcmp_P(res->arg1, PSTR("autobuild"))) { - command.mode = I2C_MECHBOARD_MODE_AUTOBUILD; - command.autobuild.level_left = res->level; - command.autobuild.level_right = res->level; - command.autobuild.count_left = 2; - command.autobuild.count_right = 2; - command.autobuild.distance_left = I2C_AUTOBUILD_DEFAULT_DIST; - command.autobuild.distance_right = I2C_AUTOBUILD_DEFAULT_DIST; - command.autobuild.do_lintel = 1; - } - else if (!strcmp_P(res->arg1, PSTR("push_temple"))) { - command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE; - command.push_temple.level = res->level; - } - state_set_mode(&command); -} - -prog_char str_state3_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0); -prog_char str_state3_arg1[] = "prepare_build#autobuild#prepare_inside"; -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, level, UINT8); - -prog_char help_state3[] = "set mechboard 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, - }, -}; - -/**********************************************************/ -/* State4 */ - -/* this structure is filled when cmd_state4 is parsed successfully */ -struct cmd_state4_result { - fixed_string_t arg0; - fixed_string_t arg1; - uint8_t level_l; - uint8_t count_l; - uint8_t dist_l; - uint8_t level_r; - uint8_t count_r; - uint8_t dist_r; - uint8_t do_lintel; -}; - -/* function called when cmd_state4 is parsed successfully */ -static void cmd_state4_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state4_result *res = parsed_result; - struct i2c_cmd_mechboard_set_mode command; - - if (!strcmp_P(res->arg1, PSTR("autobuild"))) { - command.mode = I2C_MECHBOARD_MODE_AUTOBUILD; - command.autobuild.distance_left = res->dist_l; - command.autobuild.distance_right = res->dist_r; - command.autobuild.level_left = res->level_l; - command.autobuild.level_right = res->level_r; - command.autobuild.count_left = res->count_l; - command.autobuild.count_right = res->count_r; - command.autobuild.do_lintel = res->do_lintel; - } - state_set_mode(&command); -} - -prog_char str_state4_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg0, str_state4_arg0); -prog_char str_state4_arg1[] = "autobuild"; -parse_pgm_token_string_t cmd_state4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg1, str_state4_arg1); -parse_pgm_token_num_t cmd_state4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_l, UINT8); -parse_pgm_token_num_t cmd_state4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_l, UINT8); -parse_pgm_token_num_t cmd_state4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_l, UINT8); -parse_pgm_token_num_t cmd_state4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_r, UINT8); -parse_pgm_token_num_t cmd_state4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_r, UINT8); -parse_pgm_token_num_t cmd_state4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_r, UINT8); -parse_pgm_token_num_t cmd_state4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, do_lintel, UINT8); - -prog_char help_state4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)"; -parse_pgm_inst_t cmd_state4 = { - .f = cmd_state4_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state4, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state4_arg0, - (prog_void *)&cmd_state4_arg1, - (prog_void *)&cmd_state4_arg2, - (prog_void *)&cmd_state4_arg3, - (prog_void *)&cmd_state4_arg4, - (prog_void *)&cmd_state4_arg5, - (prog_void *)&cmd_state4_arg6, - (prog_void *)&cmd_state4_arg7, - (prog_void *)&cmd_state4_arg8, - NULL, - }, -}; - -/**********************************************************/ -/* State5 */ - -/* this structure is filled when cmd_state5 is parsed successfully */ -struct cmd_state5_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; - fixed_string_t arg3; -}; - -/* function called when cmd_state5 is parsed successfully */ -static void cmd_state5_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_state5_result *res = parsed_result; - struct i2c_cmd_mechboard_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; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else - side = I2C_AUTO_SIDE; - - command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - command.prep_pickup.side = side; - - if (!strcmp_P(res->arg3, PSTR("harvest"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("lazy_harvest"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("pickup"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PICKUP; - else if (!strcmp_P(res->arg3, PSTR("clear"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_CLEAR; - else if (!strcmp_P(res->arg3, PSTR("store"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_STORE; - else if (!strcmp_P(res->arg3, PSTR("lazy_pickup"))) - command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - - state_set_mode(&command); -} - -prog_char str_state5_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_state5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg0, str_state5_arg0); -prog_char str_state5_arg1[] = "prepare_pickup"; -parse_pgm_token_string_t cmd_state5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg1, str_state5_arg1); -prog_char str_state5_arg2[] = "left#right#auto#center"; -parse_pgm_token_string_t cmd_state5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg2, str_state5_arg2); -prog_char str_state5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear"; -parse_pgm_token_string_t cmd_state5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg3, str_state5_arg3); - -prog_char help_state5[] = "set mechboard mode 2"; -parse_pgm_inst_t cmd_state5 = { - .f = cmd_state5_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_state5, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state5_arg0, - (prog_void *)&cmd_state5_arg1, - (prog_void *)&cmd_state5_arg2, - (prog_void *)&cmd_state5_arg3, - 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_Lintel */ - -/* this structure is filled when cmd_servo_lintel is parsed successfully */ -struct cmd_servo_lintel_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_servo_lintel is parsed successfully */ -static void cmd_servo_lintel_parsed(void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_servo_lintel_result *res = parsed_result; - if (!strcmp_P(res->arg1, PSTR("out"))) - servo_lintel_out(); - else if (!strcmp_P(res->arg1, PSTR("1lin"))) - servo_lintel_1lin(); - else if (!strcmp_P(res->arg1, PSTR("2lin"))) - servo_lintel_2lin(); - -} - -prog_char str_servo_lintel_arg0[] = "servo_lintel"; -parse_pgm_token_string_t cmd_servo_lintel_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg0, str_servo_lintel_arg0); -prog_char str_servo_lintel_arg1[] = "out#1lin#2lin"; -parse_pgm_token_string_t cmd_servo_lintel_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg1, str_servo_lintel_arg1); - -prog_char help_servo_lintel[] = "Servo_Lintel function"; -parse_pgm_inst_t cmd_servo_lintel = { - .f = cmd_servo_lintel_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_servo_lintel, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_lintel_arg0, - (prog_void *)&cmd_servo_lintel_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Pump_Current */ - -/* this structure is filled when cmd_pump_current is parsed successfully */ -struct cmd_pump_current_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_pump_current is parsed successfully */ -static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ - printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"), - mechboard.pump_left1_current, mechboard.pump_left2_current, - sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4)); -} - -prog_char str_pump_current_arg0[] = "pump_current"; -parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0); -prog_char str_pump_current_arg1[] = "show"; -parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1); - -prog_char help_pump_current[] = "dump pump current"; -parse_pgm_inst_t cmd_pump_current = { - .f = cmd_pump_current_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pump_current, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pump_current_arg0, - (prog_void *)&cmd_pump_current_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Manivelle */ - -/* this structure is filled when cmd_manivelle is parsed successfully */ -struct cmd_manivelle_result { - fixed_string_t arg0; - uint8_t step; -}; - -/* function called when cmd_manivelle is parsed successfully */ -static void cmd_manivelle_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ - struct cmd_manivelle_result *res = parsed_result; - state_manivelle(res->step); -} - -prog_char str_manivelle_arg0[] = "manivelle"; -parse_pgm_token_string_t cmd_manivelle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_manivelle_result, arg0, str_manivelle_arg0); -parse_pgm_token_num_t cmd_manivelle_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_manivelle_result, step, UINT8); - -prog_char help_manivelle[] = "Manivelle function"; -parse_pgm_inst_t cmd_manivelle = { - .f = cmd_manivelle_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_manivelle, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_manivelle_arg0, - (prog_void *)&cmd_manivelle_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/mechboard/cs.c b/projects/microb2010/mechboard/cs.c deleted file mode 100644 index 29a2cab..0000000 --- a/projects/microb2010/mechboard/cs.c +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "main.h" -#include "actuator.h" - -/* called every 5 ms */ -static void do_cs(__attribute__((unused)) void *dummy) -{ - /* read encoders */ - if (mechboard.flags & DO_ENCODERS) { - encoders_spi_manage(NULL); - } - /* control system */ - if (mechboard.flags & DO_CS) { - if (mechboard.left_arm.on) - cs_manage(&mechboard.left_arm.cs); - if (mechboard.right_arm.on) - cs_manage(&mechboard.right_arm.cs); - } - if (mechboard.flags & DO_BD) { - bd_manage_from_cs(&mechboard.left_arm.bd, &mechboard.left_arm.cs); - bd_manage_from_cs(&mechboard.right_arm.bd, &mechboard.right_arm.cs); - } - if (mechboard.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_arm */ - /* PID */ - pid_init(&mechboard.left_arm.pid); - pid_set_gains(&mechboard.left_arm.pid, 500, 40, 5000); - pid_set_maximums(&mechboard.left_arm.pid, 0, 5000, 2400); /* max is 12 V */ - pid_set_out_shift(&mechboard.left_arm.pid, 10); - pid_set_derivate_filter(&mechboard.left_arm.pid, 4); - - /* QUADRAMP */ - quadramp_init(&mechboard.left_arm.qr); - quadramp_set_1st_order_vars(&mechboard.left_arm.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mechboard.left_arm.qr, 20, 20); /* set accel */ - - /* CS */ - cs_init(&mechboard.left_arm.cs); - cs_set_consign_filter(&mechboard.left_arm.cs, quadramp_do_filter, &mechboard.left_arm.qr); - cs_set_correct_filter(&mechboard.left_arm.cs, pid_do_filter, &mechboard.left_arm.pid); - cs_set_process_in(&mechboard.left_arm.cs, pwm_ng_set, LEFT_ARM_PWM); - cs_set_process_out(&mechboard.left_arm.cs, encoders_spi_get_value, LEFT_ARM_ENCODER); - cs_set_consign(&mechboard.left_arm.cs, 0); - - /* Blocking detection */ - bd_init(&mechboard.left_arm.bd); - bd_set_speed_threshold(&mechboard.left_arm.bd, 150); - bd_set_current_thresholds(&mechboard.left_arm.bd, 500, 8000, 1000000, 40); - - /* ---- CS right_arm */ - /* PID */ - pid_init(&mechboard.right_arm.pid); - pid_set_gains(&mechboard.right_arm.pid, 500, 40, 5000); - pid_set_maximums(&mechboard.right_arm.pid, 0, 5000, 2400); /* max is 12 V */ - pid_set_out_shift(&mechboard.right_arm.pid, 10); - pid_set_derivate_filter(&mechboard.right_arm.pid, 6); - - /* QUADRAMP */ - quadramp_init(&mechboard.right_arm.qr); - quadramp_set_1st_order_vars(&mechboard.right_arm.qr, 1000, 1000); /* set speed */ - quadramp_set_2nd_order_vars(&mechboard.right_arm.qr, 20, 20); /* set accel */ - - /* CS */ - cs_init(&mechboard.right_arm.cs); - cs_set_consign_filter(&mechboard.right_arm.cs, quadramp_do_filter, &mechboard.right_arm.qr); - cs_set_correct_filter(&mechboard.right_arm.cs, pid_do_filter, &mechboard.right_arm.pid); - cs_set_process_in(&mechboard.right_arm.cs, pwm_ng_set, RIGHT_ARM_PWM); - cs_set_process_out(&mechboard.right_arm.cs, encoders_spi_get_value, RIGHT_ARM_ENCODER); - cs_set_consign(&mechboard.right_arm.cs, 0); - - /* Blocking detection */ - bd_init(&mechboard.right_arm.bd); - bd_set_speed_threshold(&mechboard.right_arm.bd, 150); - bd_set_current_thresholds(&mechboard.right_arm.bd, 500, 8000, 1000000, 40); - - /* set them on !! */ - mechboard.left_arm.on = 1; - mechboard.right_arm.on = 1; - - - scheduler_add_periodical_event_priority(do_cs, NULL, - CS_PERIOD / SCHEDULER_UNIT, - CS_PRIO); -} diff --git a/projects/microb2010/mechboard/cs.h b/projects/microb2010/mechboard/cs.h deleted file mode 100644 index 5b3be83..0000000 --- a/projects/microb2010/mechboard/cs.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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/mechboard/diagnostic_config.h b/projects/microb2010/mechboard/diagnostic_config.h deleted file mode 100644 index 7302bfa..0000000 --- a/projects/microb2010/mechboard/diagnostic_config.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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/mechboard/encoders_spi_config.h b/projects/microb2010/mechboard/encoders_spi_config.h deleted file mode 100644 index e71e662..0000000 --- a/projects/microb2010/mechboard/encoders_spi_config.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * 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/mechboard/error_config.h b/projects/microb2010/mechboard/error_config.h deleted file mode 100644 index 2df45d0..0000000 --- a/projects/microb2010/mechboard/error_config.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * 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/mechboard/i2c_config.h b/projects/microb2010/mechboard/i2c_config.h deleted file mode 100644 index 93ce0ec..0000000 --- a/projects/microb2010/mechboard/i2c_config.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * 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/mechboard/i2c_protocol.c b/projects/microb2010/mechboard/i2c_protocol.c deleted file mode 100644 index 902207b..0000000 --- a/projects/microb2010/mechboard/i2c_protocol.c +++ /dev/null @@ -1,211 +0,0 @@ -/* - * 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 - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#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_mechboard_status ans; - i2c_flush(); - ans.hdr.cmd = I2C_ANS_MECHBOARD_STATUS; - - /* status */ - ans.mode = state_get_mode(); - ans.status = mechboard.status; - - ans.lintel_count = mechboard.lintel_count; - ans.column_flags = mechboard.column_flags; - /* pumps pwm */ - ans.pump_left1 = mechboard.pump_left1; - ans.pump_right1 = mechboard.pump_right1; - ans.pump_left2 = mechboard.pump_left2; - ans.pump_right2 = mechboard.pump_right2; - /* pumps current */ - ans.pump_right1_current = sensor_get_adc(ADC_CSENSE3); - ans.pump_right2_current = sensor_get_adc(ADC_CSENSE4); - /* servos */ - ans.servo_lintel_left = mechboard.servo_lintel_left; - ans.servo_lintel_right = mechboard.servo_lintel_right; - - i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, - sizeof(ans), I2C_CTRL_GENERIC); -} - -static int8_t i2c_set_mode(struct i2c_cmd_mechboard_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_MECHBOARD_SET_MODE: - { - struct i2c_cmd_mechboard_set_mode *cmd = void_cmd; - if (size != sizeof(struct i2c_cmd_mechboard_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; - mechboard.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_MECHBOARD_STATUS: - { - struct i2c_req_mechboard_status *cmd = void_cmd; - if (size != sizeof (struct i2c_req_mechboard_status)) - goto error; - - mechboard.pump_left1_current = cmd->pump_left1_current; - mechboard.pump_left2_current = cmd->pump_left2_current; - 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/mechboard/i2c_protocol.h b/projects/microb2010/mechboard/i2c_protocol.h deleted file mode 100644 index 7f022ef..0000000 --- a/projects/microb2010/mechboard/i2c_protocol.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * 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 - -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/mechboard/main.c b/projects/microb2010/mechboard/main.c deleted file mode 100755 index 27c0138..0000000 --- a/projects/microb2010/mechboard/main.c +++ /dev/null @@ -1,269 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#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 mechboard mechboard; - -/***********************/ - -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(&mechboard, 0, sizeof(mechboard)); - /* cs is enabled after arm_calibrate() */ - mechboard.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_MECHBOARD); - /* check eeprom to avoid to run the bad program */ - if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != - EEPROM_MAGIC_MECHBOARD) { - 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_MECHBOARD_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; - arm_init(); - mechboard.flags |= DO_CS; - arm_do_xy(&left_arm, 60, -80, 0); - arm_do_xy(&right_arm, 60, -80, 0); - - state_machine(); - cmdline_interact(); - - return 0; -} diff --git a/projects/microb2010/mechboard/main.h b/projects/microb2010/mechboard/main.h deleted file mode 100755 index 5422f6c..0000000 --- a/projects/microb2010/mechboard/main.h +++ /dev/null @@ -1,174 +0,0 @@ -/* - * 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_ARM_ENCODER ((void *)0) -#define RIGHT_ARM_ENCODER ((void *)1) - -#define LEFT_ARM_PWM ((void *)&gen.pwm1_4A) -#define RIGHT_ARM_PWM ((void *)&gen.pwm2_4B) - -#define RIGHT_PUMP1_PWM ((void *)&gen.pwm3_1A) -#define RIGHT_PUMP2_PWM ((void *)&gen.pwm4_1B) - -#define R_ELBOW_AX12 1 -#define R_WRIST_AX12 2 -#define L_ELBOW_AX12 4 -#define L_WRIST_AX12 3 -#define FINGER_AX12 5 - -/** ERROR NUMS */ -#define E_USER_I2C_PROTO 195 -#define E_USER_SENSOR 196 -#define E_USER_ARM 197 -#define E_USER_FINGER 198 -#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 ARM_PRIO 50 -#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; -}; - -/* mechboard specific */ -struct mechboard { -#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_arm; - struct cs_block right_arm; - - /* robot status */ - uint8_t our_color; - volatile uint8_t lintel_count; - volatile uint8_t column_flags; - volatile uint8_t status; - - /* local pumps */ - int16_t pump_right1; - int16_t pump_right2; - /* remote pump (on mainboard) */ - int16_t pump_left1; - int16_t pump_left2; - - /* remote pump current */ - int16_t pump_left1_current; - int16_t pump_left2_current; - - uint16_t servo_lintel_left; - uint16_t servo_lintel_right; - -}; - -extern struct genboard gen; -extern struct mechboard mechboard; - -/* 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/mechboard/pid_config.h b/projects/microb2010/mechboard/pid_config.h deleted file mode 100755 index fa95f08..0000000 --- a/projects/microb2010/mechboard/pid_config.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * 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/mechboard/rdline_config.h b/projects/microb2010/mechboard/rdline_config.h deleted file mode 100644 index e69de29..0000000 diff --git a/projects/microb2010/mechboard/scheduler_config.h b/projects/microb2010/mechboard/scheduler_config.h deleted file mode 100755 index 98690be..0000000 --- a/projects/microb2010/mechboard/scheduler_config.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * 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-04-07 20:03:48 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 9 - - -#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/mechboard/sensor.c b/projects/microb2010/mechboard/sensor.c deleted file mode 100644 index a80201f..0000000 --- a/projects/microb2010/mechboard/sensor.c +++ /dev/null @@ -1,254 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * Olivier MATZ - * - * 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.6 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#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(__attribute__((unused)) 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 1,5,6,7,8 - */ -static struct sensor_filter sensor_filter[SENSOR_MAX] = { - [S_CAP1] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ - [S_FRONT] = { 5, 0, 4, 1, 0, 0 }, /* 1 */ - [S_CAP3] = { 10, 0, 3, 7, 0, 0 }, /* 2 */ - [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ - [S_COL_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 4 */ - [S_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 5 */ - [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ - [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */ - [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ - [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(__attribute__((unused)) void *dummy) -{ - uint8_t i; - uint8_t flags; - uint16_t sensor = sensor_read(); - uint16_t tmp = 0; - - for (i=0; i= 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 && !sensor_filter[i].invert) { - tmp |= (1UL << i); - } - else if (!sensor_filter[i].prev && sensor_filter[i].invert) { - tmp |= (1UL << i); - } - } - IRQ_LOCK(flags); - sensor_filtered = tmp; - IRQ_UNLOCK(flags); -} - - - -/************ global sensor init */ - -/* called every 10 ms, see init below */ -static void do_sensors(__attribute__((unused)) void *dummy) -{ - do_adc(NULL); - do_boolean_sensors(NULL); -} - -void sensor_init(void) -{ - adc_init(); - 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/mechboard/sensor.h b/projects/microb2010/mechboard/sensor.h deleted file mode 100644 index 27460db..0000000 --- a/projects/microb2010/mechboard/sensor.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * Olivier MATZ - * - * 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.4 2009-04-24 19:30:42 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_FRONT 1 -#define S_CAP3 2 -#define S_CAP4 3 -#define S_COL_LEFT 4 -#define S_LEFT 5 -#define S_RIGHT 6 -#define S_COL_RIGHT 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/mechboard/spi_config.h b/projects/microb2010/mechboard/spi_config.h deleted file mode 100644 index 76697c3..0000000 --- a/projects/microb2010/mechboard/spi_config.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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/mechboard/state.c b/projects/microb2010/mechboard/state.c deleted file mode 100644 index 10af0f2..0000000 --- a/projects/microb2010/mechboard/state.c +++ /dev/null @@ -1,1407 +0,0 @@ -/* - * 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: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "cmdline.h" -#include "sensor.h" -#include "actuator.h" -#include "arm_xy.h" -#include "arm_highlevel.h" -#include "state.h" - -#define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args) -#define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args) -#define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args) - -/* shorter aliases for this file */ -#define MANUAL I2C_MECHBOARD_MODE_MANUAL -#define HARVEST I2C_MECHBOARD_MODE_HARVEST -#define PREPARE_PICKUP I2C_MECHBOARD_MODE_PREPARE_PICKUP -#define PICKUP I2C_MECHBOARD_MODE_PICKUP -#define PREPARE_BUILD I2C_MECHBOARD_MODE_PREPARE_BUILD -#define AUTOBUILD I2C_MECHBOARD_MODE_AUTOBUILD -#define WAIT I2C_MECHBOARD_MODE_WAIT -#define INIT I2C_MECHBOARD_MODE_INIT -#define PREPARE_GET_LINTEL I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL -#define GET_LINTEL I2C_MECHBOARD_MODE_GET_LINTEL -#define PUT_LINTEL I2C_MECHBOARD_MODE_PUT_LINTEL -#define PREPARE_EJECT I2C_MECHBOARD_MODE_PREPARE_EJECT -#define EJECT I2C_MECHBOARD_MODE_EJECT -#define CLEAR I2C_MECHBOARD_MODE_CLEAR -#define LAZY_HARVEST I2C_MECHBOARD_MODE_LAZY_HARVEST -#define LOADED I2C_MECHBOARD_MODE_LOADED -#define PREPARE_INSIDE I2C_MECHBOARD_MODE_PREPARE_INSIDE -#define STORE I2C_MECHBOARD_MODE_STORE -#define LAZY_PICKUP I2C_MECHBOARD_MODE_LAZY_PICKUP -#define MANIVELLE I2C_MECHBOARD_MODE_MANIVELLE -#define PUSH_TEMPLE I2C_MECHBOARD_MODE_PUSH_TEMPLE -#define PUSH_TEMPLE_DISC I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC -#define EXIT I2C_MECHBOARD_MODE_EXIT - -static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode); - -static struct i2c_cmd_mechboard_set_mode mainboard_command; -static struct vt100 local_vt100; -static volatile uint8_t prev_state; -static uint8_t pickup_side; -static volatile uint8_t changed = 0; - -uint8_t state_debug = 0; - -void state_dump_sensors(void) -{ - uint16_t tmp = sensor_get_all(); - prog_char *front = PSTR("no_front"); - prog_char *left = PSTR("no_left"); - prog_char *right = PSTR("no_right"); - - if (tmp & _BV(S_FRONT)) - front = PSTR("FRONT"); - if (tmp & _BV(S_LEFT)) { - if (tmp & _BV(S_COL_LEFT)) - left = PSTR("LEFT(red)"); - else - left = PSTR("LEFT(green)"); - } - if (tmp & _BV(S_RIGHT)) { - if (tmp & _BV(S_COL_RIGHT)) - right = PSTR("RIGHT(red)"); - else - right = PSTR("RIGHT(green)"); - } - - STMCH_DEBUG("sensors = %S %S %S", front, left, right); -} - -/* return 1 if column is there */ -uint8_t arm_get_sensor(uint8_t arm_num) -{ - if (arm_num == ARM_LEFT_NUM) { - return sensor_get(S_LEFT); - } - else if (arm_num == ARM_RIGHT_NUM) { - return sensor_get(S_RIGHT); - } - return 0; -} - -/* return 0 if color is correct, else return -1 */ -int8_t arm_get_color_sensor(uint8_t arm_num) -{ - uint8_t col = 0; - if (arm_num == ARM_LEFT_NUM) { - col = sensor_get(S_COL_LEFT); - } - else if (arm_num == ARM_RIGHT_NUM) { - col = sensor_get(S_COL_RIGHT); - } - - /* if col != 0, column is red */ - if (col) { - if (mechboard.our_color == I2C_COLOR_RED) - return 0; - return -1; - } - else { - if (mechboard.our_color == I2C_COLOR_GREEN) - return 0; - return -1; - } -} - -void state_debug_wait_key_pressed(void) -{ - if (!state_debug) - return; - printf_P(PSTR("press a key\r\n")); - while(!cmdline_keypressed()); -} - -/* set a new state, return 0 on success */ -int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd) -{ - changed = 1; - prev_state = mainboard_command.mode; - memcpy(&mainboard_command, cmd, sizeof(mainboard_command)); - STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode); - return 0; -} - -/* check that state is the one in parameter and that state did not - * changed */ -uint8_t state_check(uint8_t mode) -{ - int16_t c; - if (mode != mainboard_command.mode) - return 0; - - if (changed) - return 0; - - /* force quit when CTRL-C is typed */ - c = cmdline_getchar(); - if (c == -1) - return 1; - if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) { - mainboard_command.mode = EXIT; - return 0; - } - return 1; -} - -uint8_t state_get_mode(void) -{ - return mainboard_command.mode; -} - -void pump_reset_all(void) -{ - uint8_t i; - for (i=0; i<4; i++) { - pump_set(i, PUMP_OFF); - pump_mark_free(i); - } -} - -void pump_check_all(void) -{ - if (pump_is_busy(PUMP_LEFT1_NUM) && - mechboard.pump_left1_current < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark l1 as free"); - pump_mark_free(PUMP_LEFT1_NUM); - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - } - - if (pump_is_busy(PUMP_LEFT2_NUM) && - mechboard.pump_left2_current < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark l2 as free"); - pump_mark_free(PUMP_LEFT2_NUM); - pump_set(PUMP_LEFT2_NUM, PUMP_OFF); - } - - if (pump_is_busy(PUMP_RIGHT1_NUM) && - sensor_get_adc(ADC_CSENSE3) < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark r1 as free"); - pump_mark_free(PUMP_RIGHT1_NUM); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - } - - if (pump_is_busy(PUMP_RIGHT2_NUM) && - sensor_get_adc(ADC_CSENSE4) < I2C_MECHBOARD_CURRENT_COLUMN) { - STMCH_DEBUG("Mark r2 as free"); - pump_mark_free(PUMP_RIGHT2_NUM); - pump_set(PUMP_RIGHT2_NUM, PUMP_OFF); - } -} - -uint8_t get_free_pump_count(void) -{ - uint8_t i, free_pump_count = 0; - for (i=0; i<4; i++) { - if (pump_is_free(i)) - free_pump_count++; - } - return free_pump_count; -} - -/* move finger if we are not in lazy harvest */ -void state_finger_goto(uint8_t mode, uint16_t position) -{ - if (mode == LAZY_HARVEST) - return; - finger_goto(position); -} - -void state_manivelle(int16_t step_deg) -{ - double add_h = 0.; - double add_d = 160.; - double l = 70.; - double step = RAD(step_deg); - microseconds us; - double al = RAD(0); - double ar = RAD(180); - - time_wait_ms(500); - - us = time_get_us2(); - while (1) { - al += step; - ar += step; - arm_do_xy(&left_arm, add_d+l*sin(al), add_h+l*cos(al), 10); - arm_do_xy(&right_arm, add_d+l*sin(ar), add_h+l*cos(ar), 10); - time_wait_ms(25); - if (time_get_us2() - us > (4000L * 1000L)) - break; - } -} - -static void state_do_manivelle(void) -{ - if (!state_check(MANIVELLE)) - return; - state_manivelle(30); - while (state_check(MANIVELLE)); -} - -/* common function for pickup/harvest */ -static void state_pickup_or_harvest(uint8_t mode) -{ - int8_t arm_num, pump_num; - int8_t other_arm_num, other_pump_num; - struct arm *arm; - microseconds us; - uint8_t flags, bad_color = 0, have_2cols = 0; - - pump_check_all(); - - /* get arm num */ - if (pickup_side == I2C_LEFT_SIDE) { - arm_num = ARM_LEFT_NUM; - other_arm_num = ARM_RIGHT_NUM; - } - else { - arm_num = ARM_RIGHT_NUM; - other_arm_num = ARM_LEFT_NUM; - } - - pump_num = arm_get_free_pump(arm_num); - other_pump_num = arm_get_free_pump(other_arm_num); - - /* pump is not free... skip to other arm */ - if (mode == HARVEST && pump_num == -1) { - STMCH_DEBUG("%s no free pump", __FUNCTION__); - if (arm_num == ARM_RIGHT_NUM) { - state_finger_goto(mode, FINGER_CENTER_RIGHT); - pickup_side = I2C_LEFT_SIDE; - } - else { - state_finger_goto(mode, FINGER_CENTER_LEFT); - pickup_side = I2C_RIGHT_SIDE; - } - return; - } - else if (mode == PICKUP && pump_num == -1) { - /* or exit when we are in pickup mode */ - IRQ_LOCK(flags); - if (mainboard_command.mode == mode) - mainboard_command.mode = WAIT; - IRQ_UNLOCK(flags); - } - - us = time_get_us2(); - /* wait front sensor */ - if (mode == HARVEST || mode == LAZY_HARVEST) { - STMCH_DEBUG("%s wait front", __FUNCTION__); - - while (1) { - if (sensor_get(S_FRONT)) - break; - if (state_check(mode) == 0) - return; - /* wait 500ms before reading other - sensors */ - if (time_get_us2() - us < (500 * 1000L)) - continue; - if (arm_get_sensor(arm_num)) - break; - if (arm_get_sensor(other_arm_num)) { - uint8_t tmp; - tmp = arm_num; - arm_num = other_arm_num; - other_arm_num = tmp; - pump_num = arm_get_free_pump(arm_num); - other_pump_num = arm_get_free_pump(other_arm_num); - if (other_pump_num == -1) - return; // XXX - break; - } - } - } - - - STMCH_DEBUG("%s arm_num=%d pump_num=%d", - __FUNCTION__, arm_num, pump_num); - - /* when ready, move finger */ - if (arm_num == ARM_RIGHT_NUM) - state_finger_goto(mode, FINGER_RIGHT); - else - state_finger_goto(mode, FINGER_LEFT); - - state_debug_wait_key_pressed(); - - - arm = arm_num2ptr(arm_num); - - /* prepare arm, should be already done */ - arm_goto_prepare_get(arm_num, pump_num); - while (arm_test_traj_end(arm, ARM_TRAJ_ALL) && - state_check(mode)); - - STMCH_DEBUG("%s arm pos ok", __FUNCTION__); - - state_debug_wait_key_pressed(); - - /* wait to see the column on the sensor */ - us = time_get_us2(); - while (1) { - if (arm_get_sensor(arm_num)) - break; - if (state_check(mode) == 0) - return; - if (mode == PICKUP) /* no timeout in pickup */ - continue; - /* 500ms timeout in harvest, go back */ - if (time_get_us2() - us > 500*1000L) { - STMCH_DEBUG("%s timeout", __FUNCTION__); - - if (arm_num == ARM_RIGHT_NUM) - state_finger_goto(mode, FINGER_LEFT); - else - state_finger_goto(mode, FINGER_RIGHT); - - if (sensor_get(S_FRONT)) - time_wait_ms(500); - - pump_set(pump_num, PUMP_OFF); - return; - } - } - - state_dump_sensors(); - - pump_set(pump_num, PUMP_ON); - /* bad color */ - if (arm_get_color_sensor(arm_num) == -1) { - bad_color = 1; - STMCH_DEBUG("%s prepare eject", __FUNCTION__); - mainboard_command.mode = PREPARE_EJECT; - state_do_eject(arm_num, pump_num, mode); - return; - } - - STMCH_DEBUG("%s sensor ok", __FUNCTION__); - - /* by the way, prepare the other arm */ - if (other_pump_num != -1) - arm_goto_prepare_get(other_arm_num, other_pump_num); - - /* get the column */ - arm_goto_get_column(arm_num, pump_num); - - us = time_get_us2(); - while (1) { - /* wait 50 ms */ - if (time_get_us2() - us > 50*1000L) - break; - if (mode != HARVEST) - continue; - /* if we still see the front sensor, it's because - * there are 2 columns instead of one or because there - * is another column, so send the arm on other - * side. */ - if (sensor_get(S_FRONT) && have_2cols == 0) { - STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__); - have_2cols = 1; - if (finger_get_side() == I2C_LEFT_SIDE) - state_finger_goto(mode, FINGER_RIGHT); - else - state_finger_goto(mode, FINGER_LEFT); - } - } - - if (mode == HARVEST && have_2cols == 0) { - /* just release a bit of effort */ - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(mode, FINGER_RIGHT_RELAX); - } - } - else if (mode == PICKUP) { - /* no free pump on other arm */ - if (other_pump_num == -1) { - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(mode, FINGER_RIGHT_RELAX); - } - } - /* else send finger on the other side */ - else { - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(mode, FINGER_RIGHT); - } - else { - state_finger_goto(mode, FINGER_LEFT); - } - } - } - - us = time_get_us2(); - while (1) { - /* wait 100 ms */ - if (time_get_us2() - us > 100*1000L) - break; - if (mode != HARVEST) - continue; - /* if we still see the front sensor, it's because - * there are 2 columns instead of one or because there - * is another column, so send the arm on other - * side. */ - if (sensor_get(S_FRONT) && have_2cols == 0) { - STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__); - have_2cols = 1; - if (finger_get_side() == I2C_LEFT_SIDE) - state_finger_goto(mode, FINGER_RIGHT); - else - state_finger_goto(mode, FINGER_LEFT); - } - } - - /* consider the column as taken */ - pump_mark_busy(pump_num); - - state_debug_wait_key_pressed(); - - arm_goto_intermediate_get(arm_num, pump_num); - arm_wait_traj_end(arm, ARM_TRAJ_ALL_NEAR); - - /* prepare next */ - pump_num = arm_get_free_pump(arm_num); - if (pump_num == -1) - arm_goto_loaded(arm_num); - else - arm_goto_intermediate_get(arm_num, pump_num); - - state_debug_wait_key_pressed(); - - /* switch to wait state */ - if (get_free_pump_count() == 0) { - IRQ_LOCK(flags); - if (mainboard_command.mode == mode) - mainboard_command.mode = WAIT; - IRQ_UNLOCK(flags); - } - - /* next pickup/harvest will be on the other side */ - if (pickup_side == I2C_LEFT_SIDE) - pickup_side = I2C_RIGHT_SIDE; - else - pickup_side = I2C_LEFT_SIDE; -} - - -/* manual mode, arm position is sent from mainboard */ -static void state_do_manual(void) -{ - if (!state_check(MANUAL)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(MANUAL)); -} - -/* wait mode */ -static void state_do_wait(void) -{ - if (!state_check(WAIT)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(WAIT)); -} - -/* init mode */ -static void state_do_init(void) -{ - if (!state_check(INIT)) - return; - state_init(); - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(INIT)); -} - -/* harvest columns elts from area */ -static void state_do_harvest(void) -{ - if (!state_check(HARVEST)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - state_pickup_or_harvest(HARVEST); -} - -/* harvest columns elts from area without moving finger */ -static void state_do_lazy_harvest(void) -{ - if (!state_check(LAZY_HARVEST)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - state_pickup_or_harvest(LAZY_HARVEST); -} - -/* eject a column. always called from pickup mode. */ -static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode) -{ - struct arm *arm; - arm = arm_num2ptr(arm_num); - - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(old_mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(old_mode, FINGER_RIGHT_RELAX); - } - - /* wait mainboard to eject */ - while (state_check(PREPARE_EJECT)); - - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(old_mode, FINGER_CENTER_LEFT); - } - else { - state_finger_goto(old_mode, FINGER_CENTER_RIGHT); - } - - arm_goto_get_column(arm_num, pump_num); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - time_wait_ms(150); - - state_debug_wait_key_pressed(); - - arm_goto_prepare_eject(arm_num, pump_num); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - - state_debug_wait_key_pressed(); - - if (finger_get_side() == I2C_LEFT_SIDE) { - state_finger_goto(old_mode, FINGER_LEFT_RELAX); - } - else { - state_finger_goto(old_mode, FINGER_RIGHT_RELAX); - } - - state_debug_wait_key_pressed(); - - time_wait_ms(300); - arm_goto_eject(arm_num, pump_num); - time_wait_ms(200); - pump_set(pump_num, PUMP_REVERSE); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - - arm_goto_intermediate_get(arm_num, pump_num); - pump_set(pump_num, PUMP_OFF); -} - - -/* prepare pickup in a dispenser, or harvest */ -static void state_do_prepare_pickup(void) -{ - uint8_t left_count = 0, right_count = 0; - int8_t pump_l, pump_r; - - if (!state_check(PREPARE_PICKUP)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_check_all(); - - pump_l = arm_get_free_pump(ARM_LEFT_NUM); - if (pump_l == -1) { - arm_goto_loaded(ARM_LEFT_NUM); - } - else { - arm_goto_intermediate_front_get(ARM_LEFT_NUM, pump_l); - } - - pump_r = arm_get_free_pump(ARM_RIGHT_NUM); - if (pump_r == -1) { - arm_goto_loaded(ARM_RIGHT_NUM); - } - else { - arm_goto_intermediate_front_get(ARM_RIGHT_NUM, pump_r); - } - - arm_wait_both(ARM_TRAJ_ALL); - - if (pump_l != -1) - arm_goto_prepare_get(ARM_LEFT_NUM, pump_l); - if (pump_r != -1) - arm_goto_prepare_get(ARM_RIGHT_NUM, pump_r); - - if (mainboard_command.prep_pickup.side == I2C_AUTO_SIDE) { - left_count += pump_is_busy(PUMP_LEFT1_NUM); - left_count += pump_is_busy(PUMP_LEFT2_NUM); - right_count += pump_is_busy(PUMP_RIGHT1_NUM); - right_count += pump_is_busy(PUMP_RIGHT2_NUM); - if (left_count < right_count) - finger_goto(FINGER_RIGHT); - else - finger_goto(FINGER_LEFT); - } - else if (mainboard_command.prep_pickup.side == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else if (mainboard_command.prep_pickup.side == I2C_RIGHT_SIDE) - finger_goto(FINGER_RIGHT); - else if (mainboard_command.prep_pickup.side == I2C_CENTER_SIDE) - finger_goto(FINGER_CENTER_LEFT); - - /* try to know on which side we have to pickup */ - if (finger_get_side() == I2C_RIGHT_SIDE) { - pickup_side = I2C_LEFT_SIDE; - } - else { - pickup_side = I2C_RIGHT_SIDE; - } - - arm_prepare_free_pumps(); - - mainboard_command.mode = mainboard_command.prep_pickup.next_mode; - - while (state_check(PREPARE_PICKUP)); -} - -/* clear pickup zone, will switch to harvest if needed */ -static void state_do_clear(void) -{ - uint8_t flags, err; - - if (!state_check(CLEAR)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - finger_goto(FINGER_LEFT); - err = WAIT_COND_OR_TIMEOUT(sensor_get(S_LEFT), 500); - if (err) { - IRQ_LOCK(flags); - if (mainboard_command.mode == CLEAR) - mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; - IRQ_UNLOCK(flags); - pickup_side = I2C_LEFT_SIDE; - return; - } - - finger_goto(FINGER_RIGHT); - err = WAIT_COND_OR_TIMEOUT(sensor_get(S_RIGHT), 500); - if (err) { - IRQ_LOCK(flags); - if (mainboard_command.mode == CLEAR) - mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; - IRQ_UNLOCK(flags); - pickup_side = I2C_RIGHT_SIDE; - return; - } - - IRQ_LOCK(flags); - if (mainboard_command.mode == CLEAR) - mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; - IRQ_UNLOCK(flags); -} - -/* do a lazy pickup */ -static void state_do_lazy_pickup(void) -{ - int8_t flags, arm_num, pump_num; - uint32_t us; - - if (!state_check(LAZY_PICKUP)) - return; - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - if (arm_get_sensor(ARM_LEFT_NUM) && - arm_get_sensor(ARM_RIGHT_NUM)) { - IRQ_LOCK(flags); - if (mainboard_command.mode == LAZY_PICKUP) { - mainboard_command.mode = WAIT; - } - IRQ_UNLOCK(flags); - return; - } - - if (finger_get_side() == I2C_RIGHT_SIDE) { - finger_goto(FINGER_LEFT); - arm_num = ARM_LEFT_NUM; - } - else { - finger_goto(FINGER_RIGHT); - arm_num = ARM_RIGHT_NUM; - } - - us = time_get_us2(); - while(1) { - if (state_check(LAZY_PICKUP) == 0) - return; - if (arm_get_sensor(arm_num)) - break; - if (time_get_us2() - us > 500*1000L) { - if (finger_get_side() == I2C_RIGHT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - return; - } - } - - if (arm_get_color_sensor(arm_num) == -1) { - pump_num = arm_get_free_pump(arm_num); - if (pump_num == -1) - return; /* XXX */ - pump_set(pump_num, PUMP_ON); - STMCH_DEBUG("%s prepare eject", __FUNCTION__); - mainboard_command.mode = PREPARE_EJECT; - state_do_eject(arm_num, pump_num, LAZY_PICKUP); - } -} - -/* pickup from a dispenser automatically */ -static void state_do_pickup(void) -{ - if (!state_check(PICKUP)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - /* XXX check that finger is at correct place */ - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - state_pickup_or_harvest(PICKUP); -} - -/* store columns without using arms */ -static void state_do_store(void) -{ - int8_t arm_num; - int8_t other_arm_num; - microseconds us; - - if (!state_check(STORE)) - return; - - if (get_free_pump_count() == 0) { - mainboard_command.mode = WAIT; - return; - } - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - /* get arm num */ - if (pickup_side == I2C_LEFT_SIDE) { - arm_num = ARM_LEFT_NUM; - other_arm_num = ARM_RIGHT_NUM; - } - else { - arm_num = ARM_RIGHT_NUM; - other_arm_num = ARM_LEFT_NUM; - } - - while (1) { - if (sensor_get(S_FRONT)) - break; - if (state_check(STORE) == 0) - return; - } - - /* when ready, move finger */ - if (arm_num == ARM_RIGHT_NUM) - finger_goto(FINGER_RIGHT); - else - finger_goto(FINGER_LEFT); - - /* wait to see the column on the sensor */ - us = time_get_us2(); - while (1) { - if (arm_get_sensor(arm_num)) - break; - if (state_check(STORE) == 0) - return; - /* 500ms timeout in harvest, go back */ - if (time_get_us2() - us > 500*1000L) { - STMCH_DEBUG("%s timeout", __FUNCTION__); - - if (arm_num == ARM_RIGHT_NUM) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - return; - } - } - - if (arm_get_sensor(arm_num) && arm_get_sensor(other_arm_num)) { - STMCH_DEBUG("%s full", __FUNCTION__); - while (state_check(STORE)); - return; - } - - /* next store will be on the other side */ - if (pickup_side == I2C_LEFT_SIDE) - pickup_side = I2C_RIGHT_SIDE; - else - pickup_side = I2C_LEFT_SIDE; -} - -/* prepare the building of a temple */ -static void state_do_prepare_build(void) -{ - int8_t pump_num, level; - if (!state_check(PREPARE_BUILD)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_check_all(); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - pump_num = arm_get_busy_pump(ARM_LEFT_NUM); - level = mainboard_command.prep_build.level_l; - if (pump_num != -1 && level != -1) - arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_num, - level, I2C_AUTOBUILD_DEFAULT_DIST); - - pump_num = arm_get_busy_pump(ARM_RIGHT_NUM); - level = mainboard_command.prep_build.level_r; - if (pump_num != -1 && level != -1) - arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_num, - level, I2C_AUTOBUILD_DEFAULT_DIST); - - while (state_check(PREPARE_BUILD)); -} - -/* prepare the building of a temple */ -static void state_do_push_temple(void) -{ - uint8_t level; - - level = mainboard_command.push_temple.level; - - if (!state_check(PUSH_TEMPLE)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - arm_goto_prepare_push_temple(ARM_LEFT_NUM); - arm_goto_prepare_push_temple(ARM_RIGHT_NUM); - arm_wait_both(ARM_TRAJ_ALL); - - arm_goto_push_temple(ARM_LEFT_NUM, level); - arm_goto_push_temple(ARM_RIGHT_NUM, level); - - while (state_check(PUSH_TEMPLE)); -} - -/* prepare the building of a temple */ -static void state_do_push_temple_disc(void) -{ - uint8_t side; - struct arm *arm; - - if (!state_check(PUSH_TEMPLE_DISC)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - side = mainboard_command.push_temple_disc.side; - - if (side == I2C_LEFT_SIDE) { - arm = arm_num2ptr(ARM_LEFT_NUM); - arm_goto_prepare_push_temple_disc(ARM_LEFT_NUM); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - arm_goto_push_temple_disc(ARM_LEFT_NUM); - } - else { - arm = arm_num2ptr(ARM_RIGHT_NUM); - arm_goto_prepare_push_temple_disc(ARM_RIGHT_NUM); - arm_wait_traj_end(arm, ARM_TRAJ_ALL); - arm_goto_push_temple_disc(ARM_RIGHT_NUM); - } - - while (state_check(PUSH_TEMPLE_DISC)); -} - -/* prepare the building of a temple (mainly for columns) */ -static void state_do_prepare_inside(void) -{ - int8_t pump_num, level_l, level_r; - if (!state_check(PREPARE_INSIDE)) - return; - - level_l = mainboard_command.prep_inside.level_l; - level_r = mainboard_command.prep_inside.level_r; - STMCH_DEBUG("%s mode=%d level_l=%d, level_r=%d", __FUNCTION__, - state_get_mode(), level_l, level_r); - - pump_check_all(); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - pump_num = arm_get_busy_pump(ARM_LEFT_NUM); - if (pump_num == -1) - pump_num = PUMP_LEFT1_NUM; - if (level_l != -1) - arm_goto_prepare_build_inside(ARM_LEFT_NUM, pump_num, - level_l); - - pump_num = arm_get_busy_pump(ARM_RIGHT_NUM); - if (pump_num == -1) - pump_num = PUMP_RIGHT1_NUM; - if (level_r != -1) - arm_goto_prepare_build_inside(ARM_RIGHT_NUM, pump_num, - level_r); - - while (state_check(PREPARE_INSIDE)); -} - -/* moving position */ -static void state_do_loaded(void) -{ - if (!state_check(LOADED)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_check_all(); - - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - arm_goto_loaded(ARM_LEFT_NUM); - arm_goto_loaded(ARM_RIGHT_NUM); - - while (state_check(LOADED)); -} - -static void state_do_build_lintel(uint8_t level) -{ - STMCH_DEBUG("%s() level=%d have_lintel=%d", - __FUNCTION__, level, mechboard.lintel_count); - - servo_lintel_out(); - - arm_goto_prepare_get_lintel_inside1(); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE); - pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE); - arm_goto_prepare_get_lintel_inside2(mechboard.lintel_count); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - arm_goto_get_lintel_inside(mechboard.lintel_count); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - time_wait_ms(150); - arm_goto_prepare_build_lintel1(); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - arm_goto_prepare_build_lintel2(level); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - arm_goto_prepare_build_lintel3(level); - arm_wait_both(ARM_TRAJ_ALL); - state_debug_wait_key_pressed(); - - if (mechboard.lintel_count == 1) - servo_lintel_1lin(); - else - servo_lintel_2lin(); - - arm_goto_build_lintel(level); - arm_wait_both(ARM_TRAJ_ALL); - time_wait_ms(170); - pump_set(PUMP_LEFT1_NUM, PUMP_ON); - time_wait_ms(50); /* right arm a bit after */ - pump_set(PUMP_RIGHT1_NUM, PUMP_ON); - time_wait_ms(130); - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - - mechboard.lintel_count --; -} - -/* Build one level of column. If pump_r or pump_l is -1, don't build - * with this arm. */ -static void state_do_build_column(uint8_t level_l, int8_t pump_l, - uint8_t dist_l, - uint8_t level_r, int8_t pump_r, - uint8_t dist_r) -{ - STMCH_DEBUG("%s() level_l=%d pump_l=%d level_r=%d pump_r=%d", - __FUNCTION__, level_l, pump_l, level_r, pump_r); - - /* nothing to do */ - if (pump_l == -1 && pump_r == -1) - return; - - /* go above the selected level */ - if (pump_l != -1) - arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_l, level_l, dist_l); - if (pump_r != -1) - arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_r, level_r, dist_r); - STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL), - arm_test_traj_end(&right_arm, ARM_TRAJ_ALL)); - arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL); - STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL), - arm_test_traj_end(&right_arm, ARM_TRAJ_ALL)); - - state_debug_wait_key_pressed(); - - /* drop columns of P2 */ - if (pump_l != -1) - arm_goto_autobuild(ARM_LEFT_NUM, pump_l, level_l, dist_l); - if (pump_r != -1) - arm_goto_autobuild(ARM_RIGHT_NUM, pump_r, level_r, dist_r); - arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL); - - state_debug_wait_key_pressed(); - - time_wait_ms(150); - if (pump_l != -1) - pump_set(pump_l, PUMP_REVERSE); - if (pump_r != -1) - pump_set(pump_r, PUMP_REVERSE); - time_wait_ms(150); - if (pump_l != -1) { - pump_set(pump_l, PUMP_OFF); - pump_mark_free(pump_l); - } - if (pump_r != -1) { - pump_set(pump_r, PUMP_OFF); - pump_mark_free(pump_r); - } - - state_debug_wait_key_pressed(); -} - -/* autobuild columns elts from area */ -/* check level to avoid bad things ? */ -/* check if enough cols ? */ -static void state_do_autobuild(void) -{ - int8_t pump_l, pump_r; - /* copy command into local data */ - int8_t level_l = mainboard_command.autobuild.level_left; - int8_t level_r = mainboard_command.autobuild.level_right; - uint8_t count_l = mainboard_command.autobuild.count_left; - uint8_t count_r = mainboard_command.autobuild.count_right; - uint8_t dist_l = mainboard_command.autobuild.distance_left; - uint8_t dist_r = mainboard_command.autobuild.distance_right; - uint8_t do_lintel = mainboard_command.autobuild.do_lintel; - int8_t max_level = level_l; - - - if (!state_check(AUTOBUILD)) - return; - - STMCH_DEBUG("%s mode=%d do_lintel=%d", __FUNCTION__, - state_get_mode(), do_lintel); - STMCH_DEBUG(" left: level=%d count=%d", level_l, count_l); - STMCH_DEBUG(" right: level=%d count=%d", level_r, count_r); - - /* - * build the first level of column if needed - */ - - /* don't build with this arm if no pump or if we don't ask to */ - pump_l = arm_get_busy_pump(ARM_LEFT_NUM); - if (count_l == 0) - pump_l = -1; - pump_r = arm_get_busy_pump(ARM_RIGHT_NUM); - if (count_r == 0) - pump_r = -1; - - if (pump_l == -1 && pump_r == -1) - goto lintel_only; - - state_do_build_column(level_l, pump_l, dist_l, - level_r, pump_r, dist_r); - - /* one level up */ - if (pump_l != -1) { - count_l --; - level_l ++; - max_level = level_l; - } - if (pump_r != -1) { - count_r --; - level_r ++; - if (level_r > max_level) - max_level = level_r; - } - - /* - * build the second level of column if needed - */ - - /* don't build with this arm if no pump or if we don't ask to */ - pump_l = arm_get_busy_pump(ARM_LEFT_NUM); - if (count_l == 0) - pump_l = -1; - pump_r = arm_get_busy_pump(ARM_RIGHT_NUM); - if (count_r == 0) - pump_r = -1; - - state_do_build_column(level_l, pump_l, dist_l, - level_r, pump_r, dist_r); - - /* one level up */ - if (pump_l != -1) { - count_l --; - level_l ++; - max_level = level_l; - } - if (pump_r != -1) { - count_r --; - level_r ++; - if (level_r > max_level) - max_level = level_r; - } - - state_debug_wait_key_pressed(); - - if (mechboard.lintel_count != 0 && do_lintel != 0) { - arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, - PUMP_LEFT1_NUM, - max_level, - I2C_AUTOBUILD_DEFAULT_DIST); - arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, - PUMP_RIGHT1_NUM, - max_level, - I2C_AUTOBUILD_DEFAULT_DIST); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - state_debug_wait_key_pressed(); - - arm_goto_prepare_autobuild_inside(ARM_LEFT_NUM, - PUMP_LEFT1_NUM, - max_level); - arm_goto_prepare_autobuild_inside(ARM_RIGHT_NUM, - PUMP_RIGHT1_NUM, - max_level); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - state_debug_wait_key_pressed(); - } - - lintel_only: - if (mechboard.lintel_count == 0 || do_lintel == 0) { - mainboard_command.mode = WAIT; - return; - } - - state_do_build_lintel(max_level); - mainboard_command.mode = WAIT; -} - -/* prepare to get the lintel */ -static void state_do_prepare_get_lintel(void) -{ - if (!state_check(PREPARE_GET_LINTEL)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - arm_goto_prepare_get_lintel_disp(); - arm_wait_both(ARM_TRAJ_ALL); - - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - - /* go fully left or right */ - if (finger_get_side() == I2C_LEFT_SIDE) - finger_goto(FINGER_LEFT); - else - finger_goto(FINGER_RIGHT); - - while (state_check(PREPARE_GET_LINTEL)); -} - -/* get the lintel from the dispenser */ -static void state_do_get_lintel(void) -{ - if (!state_check(GET_LINTEL)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - - pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE); - pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE); - - arm_goto_get_lintel_disp(); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - - time_wait_ms(200); - - STMCH_DEBUG("%s left1=%d left2=%d", __FUNCTION__, - mechboard.pump_left1_current, - sensor_get_adc(ADC_CSENSE3)); - - while (state_check(GET_LINTEL)); - - /* mainboard asked to release lintel, so release pump first */ - if (state_get_mode() == PREPARE_GET_LINTEL) { - pump_set(PUMP_LEFT1_NUM, PUMP_ON); - pump_set(PUMP_RIGHT1_NUM, PUMP_ON); - time_wait_ms(200); - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - } -} - -/* put the lintel inside the robot */ -static void state_do_put_lintel(void) -{ - uint8_t prev_lin_count; - - if (!state_check(PUT_LINTEL)) - return; - - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - prev_lin_count = mechboard.lintel_count; - mechboard.lintel_count ++; - - arm_goto_prepare_get_lintel_disp(); - arm_wait_both(ARM_TRAJ_ALL); - - servo_lintel_out(); - - arm_goto_prepare_put_lintel(); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - - arm_goto_put_lintel(prev_lin_count); - arm_wait_both(ARM_TRAJ_ALL); - - pump_set(PUMP_LEFT1_NUM, PUMP_ON); - pump_set(PUMP_RIGHT1_NUM, PUMP_ON); - - if (mechboard.lintel_count == 1) - servo_lintel_1lin(); - else - servo_lintel_2lin(); - - time_wait_ms(300); - - pump_set(PUMP_LEFT1_NUM, PUMP_OFF); - pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); - - arm_goto_prepare_put_lintel(); - arm_wait_both(ARM_TRAJ_ALL_NEAR); - - while (state_check(PUT_LINTEL)); -} - -/* main state machine */ -void state_machine(void) -{ - while (state_get_mode() != EXIT) { - changed = 0; - state_do_init(); - state_do_manual(); - state_do_harvest(); - state_do_lazy_harvest(); - state_do_prepare_pickup(); - state_do_pickup(); - state_do_prepare_inside(); - state_do_prepare_build(); - state_do_autobuild(); - state_do_prepare_get_lintel(); - state_do_get_lintel(); - state_do_put_lintel(); - state_do_loaded(); - state_do_clear(); - state_do_lazy_pickup(); - state_do_wait(); - state_do_store(); - state_do_manivelle(); - state_do_push_temple(); - state_do_push_temple_disc(); - } -} - -void state_init(void) -{ - vt100_init(&local_vt100); - mainboard_command.mode = WAIT; - pump_reset_all(); - mechboard.lintel_count = 1; - mechboard.column_flags = 0; - servo_lintel_1lin(); - finger_goto(FINGER_LEFT); -} diff --git a/projects/microb2010/mechboard/state.h b/projects/microb2010/mechboard/state.h deleted file mode 100644 index 0b49890..0000000 --- a/projects/microb2010/mechboard/state.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * 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: state.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -#ifndef _STATE_H_ -#define _STATE_H_ - -extern volatile uint8_t lintel_count; - -void state_manivelle(int16_t step_deg); - -/* set a new state, return 0 on success */ -int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd); - -/* get current state */ -uint8_t state_get_mode(void); - -/* launch state machine */ -void state_machine(void); - -void state_init(void); - -#endif diff --git a/projects/microb2010/mechboard/time_config.h b/projects/microb2010/mechboard/time_config.h deleted file mode 100755 index 6a1e4c5..0000000 --- a/projects/microb2010/mechboard/time_config.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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.3 2009-04-07 20:03:48 zer0 Exp $ - * - */ - -/** precision of the time processor, in us */ -#define TIME_PRECISION 1000l diff --git a/projects/microb2010/mechboard/timer_config.h b/projects/microb2010/mechboard/timer_config.h deleted file mode 100755 index bbfad76..0000000 --- a/projects/microb2010/mechboard/timer_config.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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-05 22:52:35 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/mechboard/uart_config.h b/projects/microb2010/mechboard/uart_config.h deleted file mode 100644 index a54811e..0000000 --- a/projects/microb2010/mechboard/uart_config.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: uart_config.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $ - * - */ - -/* Droids-corp 2004 - Zer0 - * config for uart module - */ - -#ifndef UART_CONFIG_H -#define UART_CONFIG_H - -/* compile uart0 fonctions, undefine it to pass compilation */ -#define UART0_COMPILE -#define UART0_ENABLED 1 -#define UART0_INTERRUPT_ENABLED 1 -#define UART0_BAUDRATE 57600 -//#define UART0_BAUDRATE 1000000 -#define UART0_USE_DOUBLE_SPEED 1 -#define UART0_RX_FIFO_SIZE 32 -#define UART0_TX_FIFO_SIZE 32 -#define UART0_NBITS 8 -#define UART0_PARITY UART_PARTITY_NONE -#define UART0_STOP_BIT UART_STOP_BITS_2 - -#define UART1_COMPILE -#define UART1_ENABLED 1 -#define UART1_INTERRUPT_ENABLED 1 -#define UART1_BAUDRATE 57600 -#define UART1_USE_DOUBLE_SPEED 1 -#define UART1_RX_FIFO_SIZE 128 -#define UART1_TX_FIFO_SIZE 128 -#define UART1_NBITS 8 -#define UART1_PARITY UART_PARTITY_NONE -#define UART1_STOP_BIT UART_STOP_BITS_1 - -#define UART3_COMPILE -#define UART3_ENABLED 1 -#define UART3_INTERRUPT_ENABLED 1 -#define UART3_BAUDRATE 57600 -#define UART3_USE_DOUBLE_SPEED 1 -#define UART3_RX_FIFO_SIZE 128 -#define UART3_TX_FIFO_SIZE 128 -#define UART3_NBITS 8 -#define UART3_PARITY UART_PARTITY_NONE -#define UART3_STOP_BIT UART_STOP_BITS_1 - -/* .... same for uart 1, 2, 3 ... */ - -#endif - diff --git a/projects/microb2010/sensorboard/.config b/projects/microb2010/sensorboard/.config deleted file mode 100644 index bc9e293..0000000 --- a/projects/microb2010/sensorboard/.config +++ /dev/null @@ -1,279 +0,0 @@ -# -# 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_SCHEDULER=y -# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set -# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set -# CONFIG_MODULE_SCHEDULER_TIMER0 is not set -CONFIG_MODULE_SCHEDULER_MANUAL=y -CONFIG_MODULE_TIME=y -# CONFIG_MODULE_TIME_CREATE_CONFIG is not set -# CONFIG_MODULE_TIME_EXT is not set -# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set - -# -# Communication modules -# - -# -# 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 -# -# CONFIG_MODULE_ROBOT_SYSTEM is not set -# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set -# CONFIG_MODULE_POSITION_MANAGER is not set -# CONFIG_MODULE_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/sensorboard/Makefile b/projects/microb2010/sensorboard/Makefile deleted file mode 100644 index 3013479..0000000 --- a/projects/microb2010/sensorboard/Makefile +++ /dev/null @@ -1,46 +0,0 @@ -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_sensorboard.c commands.c commands_scan.c -SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c -SRC += beacon.c -SRC += img_processing.c -SRC += scanner.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/sensorboard/actuator.c b/projects/microb2010/sensorboard/actuator.c deleted file mode 100644 index 2895c5a..0000000 --- a/projects/microb2010/sensorboard/actuator.c +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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 -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "main.h" - -#define PICKUP_WHEEL_L_ON 2000 -#define PICKUP_WHEEL_R_ON -2000 -#define PICKUP_WHEEL_L_OFF 0 -#define PICKUP_WHEEL_R_OFF 0 - -void pickup_wheels_on(void) -{ - pwm_ng_set(PICKUP_WHEEL_L_PWM, PICKUP_WHEEL_L_ON); - pwm_ng_set(PICKUP_WHEEL_R_PWM, PICKUP_WHEEL_R_ON); -} - -void pickup_wheels_off(void) -{ - pwm_ng_set(PICKUP_WHEEL_L_PWM, PICKUP_WHEEL_L_OFF); - pwm_ng_set(PICKUP_WHEEL_R_PWM, PICKUP_WHEEL_R_OFF); -} - diff --git a/projects/microb2010/sensorboard/actuator.h b/projects/microb2010/sensorboard/actuator.h deleted file mode 100644 index a1a535d..0000000 --- a/projects/microb2010/sensorboard/actuator.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * 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 $ - * - */ - -void pickup_wheels_on(void); -void pickup_wheels_off(void); - - diff --git a/projects/microb2010/sensorboard/adc_config.h b/projects/microb2010/sensorboard/adc_config.h deleted file mode 100644 index e69de29..0000000 diff --git a/projects/microb2010/sensorboard/ax12_config.h b/projects/microb2010/sensorboard/ax12_config.h deleted file mode 100755 index 072e8fb..0000000 --- a/projects/microb2010/sensorboard/ax12_config.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef _AX12_CONFIG_H_ -#define _AX12_CONFIG_H_ - -#define AX12_MAX_PARAMS 32 - - -#endif/*_AX12_CONFIG_H_*/ diff --git a/projects/microb2010/sensorboard/ax12_user.c b/projects/microb2010/sensorboard/ax12_user.c deleted file mode 100644 index 231495e..0000000 --- a/projects/microb2010/sensorboard/ax12_user.c +++ /dev/null @@ -1,169 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#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< 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< - * - * 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/sensorboard/beacon.c b/projects/microb2010/sensorboard/beacon.c deleted file mode 100755 index 34f1686..0000000 --- a/projects/microb2010/sensorboard/beacon.c +++ /dev/null @@ -1,397 +0,0 @@ - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "sensor.h" - -#include "../common/i2c_commands.h" -#include "main.h" -#include "beacon.h" - -struct beacon beacon; - -#define BEACON_PWM_VALUE 1000 -#define IR_SENSOR() (!!(PINK&(1<<2))) -#define MODULO_TIMER (1023L) -#define COEFF_TIMER (2) -#define COEFF_MULT (100L) -#define COEFF_MULT2 (1000L) -#define BEACON_SIZE (9) -#define BEACON_MAX_SAMPLE (3) - -#define OPPONENT_POS_X (11) -#define OPPONENT_POS_Y (22) - -#define BEACON_DEBUG(args...) DEBUG(E_USER_BEACON, args) -#define BEACON_NOTICE(args...) NOTICE(E_USER_BEACON, args) -#define BEACON_ERROR(args...) ERROR(E_USER_BEACON, args) - -static volatile int32_t rising = -1; -static volatile int32_t falling = -1; - -static int32_t get_dist(float size); -static int32_t get_angle(int32_t middle, int32_t ref); - -static int32_t pos_ref = 0; -static int32_t invalid_count = 0; - -static volatile int32_t beacon_speed; -static volatile int32_t beacon_save_count = 0; -static volatile int32_t beacon_prev_save_count = 0; -static volatile int32_t count = 0; -static volatile int32_t count_diff_rising = 0; -static volatile int32_t count_diff_falling = 0; -static int32_t beacon_coeff = 0; - -static volatile int8_t valid_beacon = 0; - -static volatile int32_t beacon_pos; - -//static int32_t beacon_sample_size[BEACON_MAX_SAMPLE]; - -int32_t encoders_spi_get_value_beacon(void *number) -{ - int32_t ret; - - ret = encoders_spi_get_value(number); - return ret*4; -} - - -void encoders_spi_set_value_beacon(void * number, int32_t v) -{ - encoders_spi_set_value(number, v/4); -} - -int32_t encoders_spi_update_beacon_speed(void * number) -{ - int32_t ret; - uint8_t flags; - - IRQ_LOCK(flags); - ret = encoders_spi_get_value_beacon(number); - beacon_speed = ret - beacon_pos; - beacon_pos = ret; - beacon_prev_save_count = beacon_save_count; - beacon_save_count = TCNT3; - IRQ_UNLOCK(flags); - - beacon_coeff = COEFF_TIMER * COEFF_MULT;//beacon_speed * COEFF_MULT / ((beacon_prev_save_count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER); - - return beacon_speed; -} - - -void beacon_init(void) -{ - //int8_t i; - - beacon_reset_pos(); - pos_ref = encoders_spi_get_value_beacon(BEACON_ENCODER); - - memset(&beacon, 0, sizeof(struct beacon)); - beacon.opponent_x = I2C_OPPONENT_NOT_THERE; - - beacon_speed = 0; - - /*for(i=0;i falling, so invert both and recalculate size and middle */ - if(local_falling < local_rising){ - temp = local_rising; - local_rising = local_falling; - local_falling = temp; - size = BEACON_STEP_TOUR - local_falling + local_rising; - middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR); - edge = local_falling; - } - /* else rising > falling */ - else{ - size = local_falling - local_rising; - middle = local_rising + (size / 2); - edge = local_rising; - } - - //for(i=BEACON_MAX_SAMPLE-1;i>0;i--){ - // beacon_sample_size[i] = beacon_sample_size[i-1]; - // total_size += beacon_sample_size[i]; - //} - //beacon_sample_size[0] = size; - //total_size += size; - //total_size /= BEACON_MAX_SAMPLE; - - //BEACON_DEBUG("rising2= %ld\t",local_rising); - //BEACON_DEBUG("falling2= %ld\r\n",local_falling); - /* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */ - BEACON_DEBUG("size= %f\r\n",size); - //BEACON_DEBUG("middle= %ld\r\n",middle); - - local_angle = get_angle(middle,0); - BEACON_NOTICE("opponent angle= %ld\t",local_angle); - - local_dist = get_dist(size); - BEACON_NOTICE("opponent dist= %ld\r\n",local_dist); - - beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y); - - IRQ_LOCK(flags); - beacon.opponent_x = result_x; - beacon.opponent_y = result_y; - beacon.opponent_angle = local_angle; - beacon.opponent_dist = local_dist; - /* for I2C test */ - //beacon.opponent_x = OPPONENT_POS_X; - //beacon.opponent_y = OPPONENT_POS_Y; - IRQ_UNLOCK(flags); - - BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x); - BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y); - } - else { - BEACON_NOTICE("non valid\r\n\n"); - } - - falling = -1; -} - -static int32_t get_dist(float size) -{ - int32_t dist=0; - //int32_t alpha=0; - - //alpha = (size*2*M_PI*COEFF_MULT2); - //dist = ((2*BEACON_SIZE*BEACON_STEP_TOUR*COEFF_MULT2)/alpha)/2; - - /* function found by measuring points up to 80cm */ - //dist = ((size - 600)*(size - 600)) / 2400 +28; - - /* new function */ - /* dist = a0 + a1*x + a2*x² + a3x³ */ - dist = 1157.3 + (1.4146*size) + (-0.013508*size*size) + (0.00001488*size*size*size); - - return dist; - -} - -static int32_t get_angle(int32_t middle, int32_t ref) -{ - int32_t ret_angle; - - ret_angle = (middle - ref) * 360 / BEACON_STEP_TOUR; - - if(ret_angle > 360) - ret_angle -= 360; - - return ret_angle; -} - -void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y) -{ - uint8_t flags; - - int32_t local_x; - int32_t local_y; - int32_t x_opponent; - int32_t y_opponent; - int32_t local_robot_angle; - - IRQ_LOCK(flags); - local_x = beacon.robot_x; - local_y = beacon.robot_y; - local_robot_angle = beacon.robot_angle; - IRQ_UNLOCK(flags); - - if (local_robot_angle < 0) - local_robot_angle += 360; - - x_opponent = cos((local_robot_angle + angle)* 2 * M_PI / 360)* dist; - y_opponent = sin((local_robot_angle + angle)* 2 * M_PI / 360)* dist; - - //BEACON_DEBUG("x_op= %ld\t",x_opponent); - //BEACON_DEBUG("y_op= %ld\r\n",y_opponent); - //BEACON_NOTICE("robot_x= %ld\t",local_x); - //BEACON_NOTICE("robot_y= %ld\t",local_y); - //BEACON_NOTICE("robot_angle= %ld\r\n",local_robot_angle); - - *x = local_x + x_opponent; - *y = local_y + y_opponent; - -} diff --git a/projects/microb2010/sensorboard/beacon.h b/projects/microb2010/sensorboard/beacon.h deleted file mode 100755 index 21d7dd5..0000000 --- a/projects/microb2010/sensorboard/beacon.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * 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: beacon.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $ - * - */ - -struct beacon { - int32_t beacon_speed; - - int32_t opponent_angle; - int32_t opponent_dist; - int32_t prev_opponent_angle; - int32_t prev_opponent_dist; - int32_t robot_x; - int32_t robot_y; - int32_t robot_angle; - int32_t opponent_x; - int32_t opponent_y; -}; - -/* real encoder value: 3531.75 so, multiple by 4 to have round - * value */ -#define BEACON_STEP_TOUR (14127L) -#define BEACON_OFFSET_CALIBRE 40 - -extern struct beacon beacon; - -void beacon_dump(void); -void beacon_init(void); -void beacon_calibre_pos(void); -void beacon_start(void); -void beacon_stop(void); -void beacon_calc(void *dummy); -void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y); - -void beacon_reset_pos(void); -void beacon_set_consign(int32_t val); - -int32_t encoders_spi_get_beacon_speed(void *dummy); -int32_t encoders_spi_update_beacon_speed(void *number); -int32_t encoders_spi_get_value_beacon(void *number); - - diff --git a/projects/microb2010/sensorboard/cmdline.c b/projects/microb2010/sensorboard/cmdline.c deleted file mode 100644 index ceb3efe..0000000 --- a/projects/microb2010/sensorboard/cmdline.c +++ /dev/null @@ -1,149 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#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; ierr_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), "sensorboard > "); - 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/sensorboard/cmdline.h b/projects/microb2010/sensorboard/cmdline.h deleted file mode 100644 index 57dfa49..0000000 --- a/projects/microb2010/sensorboard/cmdline.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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/sensorboard/commands.c b/projects/microb2010/sensorboard/commands.c deleted file mode 100644 index 539c96d..0000000 --- a/projects/microb2010/sensorboard/commands.c +++ /dev/null @@ -1,128 +0,0 @@ -/* - * 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 - */ - -#include -#include -#include - -/* 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_sensorboard.c */ -extern parse_pgm_inst_t cmd_event; -extern parse_pgm_inst_t cmd_test; - -/* commands_scan.c */ -extern parse_pgm_inst_t cmd_sample; -extern parse_pgm_inst_t cmd_sadc; -extern parse_pgm_inst_t cmd_scan_params; -extern parse_pgm_inst_t cmd_scan_calibre; -extern parse_pgm_inst_t cmd_scan_do; -extern parse_pgm_inst_t cmd_scan_img; - - -/* 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_sensorboard.c */ - (parse_pgm_inst_t *)&cmd_event, - (parse_pgm_inst_t *)&cmd_test, - - /* commands_scan.c */ - (parse_pgm_inst_t *)&cmd_sample, - (parse_pgm_inst_t *)&cmd_sadc, - (parse_pgm_inst_t *)&cmd_scan_params, - (parse_pgm_inst_t *)&cmd_scan_calibre, - (parse_pgm_inst_t *)&cmd_scan_do, - (parse_pgm_inst_t *)&cmd_scan_img, - - NULL, -}; diff --git a/projects/microb2010/sensorboard/commands_ax12.c b/projects/microb2010/sensorboard/commands_ax12.c deleted file mode 100644 index 771fb30..0000000 --- a/projects/microb2010/sensorboard/commands_ax12.c +++ /dev/null @@ -1,368 +0,0 @@ -/* - * 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 - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#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/sensorboard/commands_cs.c b/projects/microb2010/sensorboard/commands_cs.c deleted file mode 100644 index 690922a..0000000 --- a/projects/microb2010/sensorboard/commands_cs.c +++ /dev/null @@ -1,670 +0,0 @@ -/* - * 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 - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "main.h" -#include "cs.h" -#include "cmdline.h" - -struct csb_list { - const prog_char *name; - struct cs_block *csb; -}; - -prog_char csb_beacon_str[] = "beacon"; -prog_char csb_scanner_str[] = "scanner"; -struct csb_list csb_list[] = { - { .name = csb_beacon_str, .csb = &sensorboard.beacon }, - { .name = csb_scanner_str, .csb = &sensorboard.scanner }, -}; - -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[] = "beacon#scanner"; -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/sensorboard/commands_gen.c b/projects/microb2010/sensorboard/commands_gen.c deleted file mode 100644 index 9f7cdd6..0000000 --- a/projects/microb2010/sensorboard/commands_gen.c +++ /dev/null @@ -1,573 +0,0 @@ -/* - * 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 - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#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; iarg1, PSTR("loop_show"))) - loop = 1; - - do { - printf_P(PSTR("SENSOR values: ")); - for (i=0; iarg1, 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; iarg3, PSTR("off"))) { - for (i=0; i - */ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include "main.h" -#include "cmdline.h" -#include "../common/i2c_commands.h" -#include "i2c_protocol.h" -#include "actuator.h" - -#include -#include -#include - -#include "img_processing.h" -#include "scanner.h" - - - -/**********************************************************/ -/* sample ADC */ -/* -extern uint16_t sample_i; -extern float scan_offset_a; -extern float scan_offset_b; -*/ -extern struct scan_params scan_params; - -//extern uint16_t sample_tab[MAX_SAMPLE]; -/* this structure is filled when cmd_sample is parsed successfully */ -struct cmd_sample_result { - fixed_string_t arg0; - fixed_string_t arg1; - uint16_t offset_b; - float offset_a; - uint16_t dump_speed; - uint8_t filter; -}; - -extern int32_t pos_start_scan; -/* function called when cmd_sample is parsed successfully */ - -#define MAX_OBJECTS 10 -Object_bb sac_obj[MAX_OBJECTS]; -static void cmd_sample_parsed(void * parsed_result, void * data) -{ - struct cmd_sample_result * res = parsed_result; - uint16_t i; - - printf_P(PSTR("cmd sample called\r\n")); - printf_P(PSTR("arg %s %d\r\n"), res->arg1, res->offset_b); - - quadramp_set_1st_order_vars(&sensorboard.scanner.qr, res->dump_speed, res->dump_speed); /* set speed */ - - - scan_params.offset_b = (((float)res->offset_b)*M_PI/180.); - scan_params.offset_a = (((float)res->offset_a)*M_PI/180.); - scan_params.filter = res->filter; - - if (!strcmp_P(res->arg1, PSTR("start"))) { - scan_params.sample_i = MAX_SAMPLE; - scan_params.pos_start_scan = encoders_spi_get_value_scanner(SCANNER_ENC); - //printf_P(PSTR("start scan at pos %ld\r\n"), scan_params.pos_start_scan); - - memset(scan_params.sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t)); - - - cs_set_consign(&sensorboard.scanner.cs, scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L); - //printf_P(PSTR("scan dst %ld\r\n"), scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L); - - scan_params.last_col_n = 0; - scan_params.last_row_n = 0; - scan_params.last_sample = 0; - - } - else if (!strcmp_P(res->arg1, PSTR("dump"))) { - printf_P(PSTR("start object detection\r\n")); - for (i=0;ispeed; - scan_params.debug = res->debug; - -} - -prog_char str_scan_params_arg0[] = "scan_params"; -parse_pgm_token_string_t cmd_scan_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_params_result, arg0, str_scan_params_arg0); -parse_pgm_token_num_t cmd_scan_params_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, speed, INT16); -parse_pgm_token_num_t cmd_scan_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, debug, UINT8); - -prog_char help_scan_params[] = "Set scanner params (speed, debug)"; -parse_pgm_inst_t cmd_scan_params = { - .f = cmd_scan_params_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_sample, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_params_arg0, - (prog_void *)&cmd_scan_params_arg1, - (prog_void *)&cmd_scan_params_arg2, - NULL, - }, -}; - - - -/**********************************************************/ -/* set scanner calibration */ - -/* this structure is filled when cmd_scan_calibre is parsed successfully */ -struct cmd_scan_calibre_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_scan_calibre is parsed successfully */ -static void cmd_scan_calibre_parsed(void * parsed_result, void * data) -{ - struct cmd_scan_calibre_result * res = parsed_result; - - printf_P(PSTR("Starting scanner autocalibration\r\n")); - - - if (!strcmp_P(res->arg1, PSTR("mirror"))) { - scanner_calibre_mirror(); - } - else{ - scanner_calibre_laser(); - } -} - -prog_char str_scan_calibre_arg0[] = "scan_calibre"; - -prog_char str_scan_calibre_what_arg1[] = "mirror#laser"; -parse_pgm_token_string_t cmd_scan_calibre_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg0, str_scan_calibre_arg0); -parse_pgm_token_string_t cmd_scan_calibre_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg1, str_scan_calibre_what_arg1); - - -prog_char help_scan_calibre[] = "Scanner auto calibration (mirror|laser)"; -parse_pgm_inst_t cmd_scan_calibre = { - .f = cmd_scan_calibre_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_calibre, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_calibre_arg0, - (prog_void *)&cmd_scan_calibre_arg1, - NULL, - }, -}; - - - -/**********************************************************/ -/* start scan */ - -/* this structure is filled when cmd_scan_do is parsed successfully */ -struct cmd_scan_do_result { - fixed_string_t arg0; -}; - -/* function called when cmd_scan_do is parsed successfully */ -static void cmd_scan_do_parsed(void * parsed_result, void * data) -{ - printf_P(PSTR("Starting scan\r\n")); - scanner_scan_autonomous(); -} - -prog_char str_scan_do_arg0[] = "scan_do"; -parse_pgm_token_string_t cmd_scan_do_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_do_result, arg0, str_scan_do_arg0); - -prog_char help_scan_do[] = "Scan zone"; -parse_pgm_inst_t cmd_scan_do = { - .f = cmd_scan_do_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_do, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_do_arg0, - NULL, - }, -}; - - - - -/**********************************************************/ -/* set scanner img params */ - -/* this structure is filled when cmd_scan_img is parsed successfully */ -struct cmd_scan_img_result { - fixed_string_t arg0; - uint8_t algo; - uint8_t h; - int16_t x; - int16_t y; - -}; - -/* function called when cmd_scan_img is parsed successfully */ -static void cmd_scan_img_parsed(void * parsed_result, void * data) -{ - struct cmd_scan_img_result * res = parsed_result; - - scan_params.algo = res->algo; - if (res->algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE) { - scan_params.drop_zone.working_zone = res->h; - scan_params.drop_zone.center_x = res->x; - scan_params.drop_zone.center_y = res->y; - } else if (res->algo == I2C_SCANNER_ALGO_CHECK_TEMPLE) { - scan_params.check_temple.level = res->h; - scan_params.check_temple.temple_x = res->x; - scan_params.check_temple.temple_y = res->y; - } else if (res->algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE) { - scan_params.drop_zone.working_zone = res->h; - scan_params.drop_zone.center_x = res->x; - scan_params.drop_zone.center_y = res->y; - } - - - -} - -prog_char str_scan_img_arg0[] = "scan_img"; -parse_pgm_token_string_t cmd_scan_img_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_img_result, arg0, str_scan_img_arg0); -parse_pgm_token_num_t cmd_scan_img_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, algo, UINT8); -parse_pgm_token_num_t cmd_scan_img_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, h, UINT8); -parse_pgm_token_num_t cmd_scan_img_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, x, INT16); -parse_pgm_token_num_t cmd_scan_img_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, y, INT16); - - -prog_char help_scan_img[] = "Set scanner img processing params (algo, H, x, y)"; -parse_pgm_inst_t cmd_scan_img = { - .f = cmd_scan_img_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_img, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_img_arg0, - (prog_void *)&cmd_scan_img_arg1, - (prog_void *)&cmd_scan_img_arg2, - (prog_void *)&cmd_scan_img_arg3, - (prog_void *)&cmd_scan_img_arg4, - NULL, - }, -}; - - diff --git a/projects/microb2010/sensorboard/commands_sensorboard.c b/projects/microb2010/sensorboard/commands_sensorboard.c deleted file mode 100644 index 9b90bd1..0000000 --- a/projects/microb2010/sensorboard/commands_sensorboard.c +++ /dev/null @@ -1,197 +0,0 @@ -/* - * 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_sensorboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#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"))) - sensorboard.flags |= bit; - else if (!strcmp_P(res->arg2, PSTR("off"))) - sensorboard.flags &= bit; - else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), - (DO_ENCODERS & sensorboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), - (DO_CS & sensorboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), - (DO_BD & sensorboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), - (DO_POWER & sensorboard.flags) ? "on":"off"); - } - return; - } - - if (!strcmp_P(res->arg1, PSTR("encoders"))) - bit = DO_ENCODERS; - else if (!strcmp_P(res->arg1, PSTR("cs"))) { - //strat_hardstop(); - bit = DO_CS; - } - else if (!strcmp_P(res->arg1, PSTR("bd"))) - bit = DO_BD; - else if (!strcmp_P(res->arg1, PSTR("power"))) - bit = DO_POWER; - - - if (!strcmp_P(res->arg2, PSTR("on"))) - sensorboard.flags |= bit; - else if (!strcmp_P(res->arg2, PSTR("off"))) { - if (!strcmp_P(res->arg1, PSTR("cs"))) { - pwm_ng_set(BEACON_PWM, 0); - pwm_ng_set(SCANNER_PWM, 0); - } - sensorboard.flags &= (~bit); - } - printf_P(PSTR("%s is %s\r\n"), res->arg1, - (bit & sensorboard.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"))) { - sensorboard.our_color = I2C_COLOR_RED; - } - else if (!strcmp_P(res->color, PSTR("green"))) { - sensorboard.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/sensorboard/cs.c b/projects/microb2010/sensorboard/cs.c deleted file mode 100644 index 010b49e..0000000 --- a/projects/microb2010/sensorboard/cs.c +++ /dev/null @@ -1,145 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "main.h" -#include "beacon.h" -#include "scanner.h" -#include "actuator.h" - -/* called every 5 ms */ -static void do_cs(void *dummy) -{ - /* read encoders */ - if (sensorboard.flags & DO_ENCODERS) { - encoders_spi_manage(NULL); - } - /* control system */ - if (sensorboard.flags & DO_CS) { - if (sensorboard.beacon.on) - cs_manage(&sensorboard.beacon.cs); - if (sensorboard.scanner.on) - cs_manage(&sensorboard.scanner.cs); - } - if (sensorboard.flags & DO_BD) { - bd_manage_from_cs(&sensorboard.beacon.bd, &sensorboard.beacon.cs); - bd_manage_from_cs(&sensorboard.scanner.bd, &sensorboard.scanner.cs); - } - if (sensorboard.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 beacon */ - /* PID */ - pid_init(&sensorboard.beacon.pid); - pid_set_gains(&sensorboard.beacon.pid, 80, 80, 250); - pid_set_maximums(&sensorboard.beacon.pid, 0, 10000, 2000); - pid_set_out_shift(&sensorboard.beacon.pid, 6); - pid_set_derivate_filter(&sensorboard.beacon.pid, 6); - - /* CS */ - cs_init(&sensorboard.beacon.cs); - cs_set_correct_filter(&sensorboard.beacon.cs, pid_do_filter, &sensorboard.beacon.pid); - cs_set_process_in(&sensorboard.beacon.cs, pwm_ng_set, BEACON_PWM); - cs_set_process_out(&sensorboard.beacon.cs, encoders_spi_update_beacon_speed, BEACON_ENCODER); - cs_set_consign(&sensorboard.beacon.cs, 0); - - /* ---- CS scanner */ - /* PID */ - pid_init(&sensorboard.scanner.pid); - pid_set_gains(&sensorboard.scanner.pid, 200, 5, 250); - pid_set_maximums(&sensorboard.scanner.pid, 0, 10000, 2047); - pid_set_out_shift(&sensorboard.scanner.pid, 6); - pid_set_derivate_filter(&sensorboard.scanner.pid, 6); - - /* QUADRAMP */ - quadramp_init(&sensorboard.scanner.qr); - quadramp_set_1st_order_vars(&sensorboard.scanner.qr, 200, 200); /* set speed */ - quadramp_set_2nd_order_vars(&sensorboard.scanner.qr, 20, 20); /* set accel */ - - /* CS */ - cs_init(&sensorboard.scanner.cs); - cs_set_consign_filter(&sensorboard.scanner.cs, quadramp_do_filter, &sensorboard.scanner.qr); - cs_set_correct_filter(&sensorboard.scanner.cs, pid_do_filter, &sensorboard.scanner.pid); - cs_set_process_in(&sensorboard.scanner.cs, pwm_ng_set, SCANNER_PWM); - cs_set_process_out(&sensorboard.scanner.cs, encoders_spi_update_scanner, SCANNER_ENCODER); - cs_set_consign(&sensorboard.scanner.cs, 0); - - /* Blocking detection */ - bd_init(&sensorboard.scanner.bd); - bd_set_speed_threshold(&sensorboard.scanner.bd, 150); - bd_set_current_thresholds(&sensorboard.scanner.bd, 500, 8000, 1000000, 40); - - /* set them on !! */ - sensorboard.beacon.on = 0; - sensorboard.scanner.on = 1; - - - scheduler_add_periodical_event_priority(do_cs, NULL, - 5000L / SCHEDULER_UNIT, - CS_PRIO); -} diff --git a/projects/microb2010/sensorboard/cs.h b/projects/microb2010/sensorboard/cs.h deleted file mode 100644 index fe50c80..0000000 --- a/projects/microb2010/sensorboard/cs.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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/sensorboard/diagnostic_config.h b/projects/microb2010/sensorboard/diagnostic_config.h deleted file mode 100644 index e4ebb75..0000000 --- a/projects/microb2010/sensorboard/diagnostic_config.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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/sensorboard/encoders_spi_config.h b/projects/microb2010/sensorboard/encoders_spi_config.h deleted file mode 100644 index 6a4a68b..0000000 --- a/projects/microb2010/sensorboard/encoders_spi_config.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * 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/sensorboard/error_config.h b/projects/microb2010/sensorboard/error_config.h deleted file mode 100644 index 15ca2fc..0000000 --- a/projects/microb2010/sensorboard/error_config.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * 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/sensorboard/gen_scan_tab.c b/projects/microb2010/sensorboard/gen_scan_tab.c deleted file mode 100644 index 7b94bfd..0000000 --- a/projects/microb2010/sensorboard/gen_scan_tab.c +++ /dev/null @@ -1,680 +0,0 @@ -#include -#include -#include -#include - -#include - - -#include "scanner.h" - -#define ADC_REF_AVCC 0 -#define MUX_ADC13 0 - - - -double TELEMETRE_A = TELEMETRE_A_INIT; -double TELEMETRE_B = TELEMETRE_B_INIT; - - -#define printf_P printf -#define PSTR(a) (a) -int32_t motor_angle = 0; -int32_t scan_dist = 0; - - -int32_t H_fin, L_fin; -double L, H; - -#define nop() - -#define ABS(val) ({ \ - __typeof(val) __val = (val); \ - if (__val < 0) \ - __val = - __val; \ - __val; \ - }) - -struct scan_params scan_params; - -int32_t encoders_spi_get_value_scanner_interpolation(void *a) -{ - return motor_angle; -} - - -int16_t adc_get_value(uint8_t a) -{ - return scan_dist; -} - - - -/* get motor angle in radian; return mirror angle in radian, cos a sin a */ -void ang2_a_mirror(double b, double * c_a, double* s_a, double* a) -{ - double x2, y2; - double A, DELTA, B, D; - - b+=scan_params.offset_b; - x2 = X + l1*cos(b); - y2 = Y + l1*sin(b); - - A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3); - - DELTA = -(A*A - x2*x2 - y2*y2); - B = sqrt(DELTA); - - D = x2*x2 + y2*y2; - - *c_a = (x2*A + y2*B)/D; - *s_a = -(x2*B - y2*A)/D; - - *a = atan2(*s_a, *c_a); - - *a += scan_params.offset_a; - // *s_a = sin(*a); - // *c_a = cos(*a); - -} - -/* get telemeter dist , cos a, sin a, a and return H, L of scanned point */ -void ang2_H_L(double l_telemetre, double c_a, double s_a, double a, double *H, double *L) -{ - double d; - d = h_mirror*c_a/s_a; - *H = (l_telemetre - l_mirror - d)*sin(2*a); - *L = l_mirror + d + *H/tan(2*a); - - //*H+= 8*sin(a-scan_params.offset_a); -} - - - -//int32_t last_col_n; -//int32_t last_row_n; -//uint8_t last_sample; - -//uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123}; -//uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123}; - -/* last high is dummy, to deal higher columns */ -//uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250}; - - -//uint8_t h_limit[] = {60, 72, 85, 98, 112, 129, 250}; - -//uint8_t h_limit[] = {80, 97, 118, 134, 145, 160, 250}; - - -//uint8_t h_limit[] = {79, 94, 108, 117, 129, 144, 250}; - -uint8_t h_limit[] = {83, 95, 108, 120, 135, 147, 164, 250}; -#define H_MARGIN (-6) - -#define cs_set_consign(a, b) - - - -void do_scan(void * dummy) -{ - - unsigned int i; - int16_t a; - int32_t row_n; - int32_t col_n; - - - int32_t tour_pos; - int32_t pos, last_pos; - int32_t pos_tmp ; - int32_t mot_pos; - double dist; - uint8_t min_sample; - - double b, c_a, s_a, /*H, L,*/ m_a; - // int32_t H_fin; - - - //printf("scan\n"); - if (scan_params.sample_i==0) - return; - - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); -#if 0 - if (scan_params.sample_i==1){ - printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN); - //scanner.flags &= (~CS_ON); - - - - /* stop scan at cur pos + 10 round */ - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); - mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ; - - printf_P(PSTR("set to %ld \r\n"), mot_pos); - - cs_set_consign(&sensorboard.scanner.cs, mot_pos); - //pwm_ng_set(SCANNER_MOT_PWM, 0); - - - } - - mot_pos-= scan_params.pos_start_scan; -#endif - a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 ); - - - //dist = (a-TELEMETRE_B)/TELEMETRE_A; - //dist = TELEMETRE_A * a +TELEMETRE_B; - dist = telemetre_to_cm(a); - - //printf_P(PSTR("enc val = %ld\r\n"), encoders_microb_get_value((void *)SCANNER_ENC)); - - - //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a; - //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0; - - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); -#if 0 - /* separe scan forward/backword */ - if (row_n%2){ - row_n/=2; - } - else{ - row_n/=2; - row_n+=30; - } -#endif - - tour_pos = (mot_pos)%(SCANNER_STEP_TOUR); - - b = (2.*M_PI)*(double)tour_pos/(double)(SCANNER_STEP_TOUR); - - ang2_a_mirror(b, &c_a, &s_a, &m_a); - ang2_H_L(dist, c_a, s_a, m_a, &H, &L); - - - if (H >0){ - printf("zarb H\n"); - H = 0; - } - - if (dist> SCAN_MAX_DIST){ - H = 0; - L = 0; - } - - H = H;//m_a*180/M_PI; - L = L;//(L-100)*PIX_PER_SCAN; - - //printf_P(PSTR("polling : ADC0 = %i %f\r\n"),a, dist); - //printf_P(PSTR("%f %f %2.2f %f\r\n"), H, L, m_a*180./M_PI, dist); - - - //printf_P(PSTR("dist, b, a: %2.2f %2.2f %2.2f\r\n"), dist, b*180/M_PI, m_a*180/M_PI); - - H=(H+SCAN_H_MAX)*SCAN_H_COEF; - L-=SCAN_L_MIN; - - - /* XXX may never append */ - if (L<0) - L=0; - - - /* first filter => pixel modulo level */ -#define H_BASE 10 -#define H_MUL 14 - H_fin = H;//+SCAN_H_MAX; - //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE; - - if (scan_params.filter){ - H_fin = 11; // default is level 11 - for (i=0;iPIX_PER_SCAN) - printf("BUG!!! RECALC MAX L\r\n"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos =DIM_DIST) - j = DIM_DIST-1; - - if (i>=DIM_ANGLE) - i = DIM_ANGLE-1; - - - val.u16 = pgm_read_word_near(&array_h_l[j][i]); - - - //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - //val.u16 = pgm_read_word_near(&array_h_l[a][tp]); - H = val.h_l.h; - L = val.h_l.l; - /* - val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - - H = val.h_l.h; - L = val.h_l.l; - */ - H_fin = H; - L_fin = L; - - - - col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN); - if (col_n>PIX_PER_SCAN) - printf("BUG!!! RECALC MAX L\r\n"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos 1 && !strcmp(argv[1], "1")) - scan_params.filter = 1; - - - printf("max i max j %d %d \n", i, j); - dist_max = j; - angle_max = i; - - f_header = fopen("scan_h_l.h", "w"); - fprintf(f_header, "PROGMEM lookup_h_l array_h_l[%d][%d] = {\n", dist_max, angle_max); - for (j = 0, scan_dist = 0; j - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "actuator.h" -#include "beacon.h" -#include "scanner.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_sensorboard_status ans; - i2c_flush(); - ans.hdr.cmd = I2C_ANS_SENSORBOARD_STATUS; - ans.status = 0x55; /* XXX */ - ans.opponent_x = beacon.opponent_x; - ans.opponent_y = beacon.opponent_y; - ans.opponent_a = beacon.opponent_angle; - ans.opponent_d = beacon.opponent_dist; - - ans.scan_status = 0; - ans.scan_status |= scan_params.working ? 0 : I2C_SCAN_DONE; - ans.scan_status |= scan_params.max_column_detected ? I2C_SCAN_MAX_COLUMN : 0; - - - ans.dropzone_x = scan_params.dropzone_x; - ans.dropzone_y = scan_params.dropzone_y; - ans.dropzone_h = scan_params.dropzone_h; - - i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, - sizeof(ans), I2C_CTRL_GENERIC); -} - - -void i2c_scanner_calibre_laser(void* dummy) -{ - scanner_calibre_laser(); -} - -void i2c_scanner_end_process(void* dummy) -{ - scanner_end_process(); -} - -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; - sensorboard.our_color = cmd->color; - break; - } - - case I2C_CMD_SENSORBOARD_SET_BEACON: - { - struct i2c_cmd_sensorboard_start_beacon *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - if (cmd->enable) - beacon_start(); - else - beacon_stop(); - - break; - } - - case I2C_CMD_SENSORBOARD_SET_SCANNER: - { - struct i2c_cmd_sensorboard_scanner *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - scanner_set_mode(cmd->mode); - break; - - } - - - case I2C_CMD_SENSORBOARD_SCANNER_ALGO: - { - struct i2c_cmd_sensorboard_scanner_algo *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - scan_params.algo = cmd->algo; - - if (cmd->algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE){ - scan_params.drop_zone.working_zone = cmd->drop_zone.working_zone; - scan_params.drop_zone.center_x = cmd->drop_zone.center_x; - scan_params.drop_zone.center_y = cmd->drop_zone.center_y; - } - else if (cmd->algo == I2C_SCANNER_ALGO_CHECK_TEMPLE) { - scan_params.check_temple.level = cmd->check_temple.level; - scan_params.check_temple.temple_x = cmd->check_temple.temple_x; - scan_params.check_temple.temple_y = cmd->check_temple.temple_y; - } - else if (cmd->algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE){ - scan_params.drop_zone.working_zone = cmd->drop_zone.working_zone; - scan_params.drop_zone.center_x = cmd->drop_zone.center_x; - scan_params.drop_zone.center_y = cmd->drop_zone.center_y; - } - else{ - /* new command */ - } - - scan_params.working = 1; - scheduler_add_single_event_priority(i2c_scanner_end_process, NULL, - 1, - CS_PRIO-1); - break; - - } - - case I2C_CMD_SENSORBOARD_CALIB_SCANNER: - { - struct i2c_cmd_sensorboard_calib_scanner *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - scheduler_add_single_event_priority(i2c_scanner_calibre_laser, NULL, - 1, - CS_PRIO-1); - break; - } - - - - /* Add other commands here ...*/ - - - case I2C_REQ_SENSORBOARD_STATUS: - { - struct i2c_req_sensorboard_status *cmd = void_cmd; - if (size != sizeof (*cmd)) - goto error; - - beacon.robot_x = cmd->x; - beacon.robot_y = cmd->y; - beacon.robot_angle = cmd->a; - - if (cmd->enable_pickup_wheels) - pickup_wheels_on(); - else - pickup_wheels_off(); - - 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/sensorboard/i2c_protocol.h b/projects/microb2010/sensorboard/i2c_protocol.h deleted file mode 100644 index 6fcac74..0000000 --- a/projects/microb2010/sensorboard/i2c_protocol.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * 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 - -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/sensorboard/img_processing.c b/projects/microb2010/sensorboard/img_processing.c deleted file mode 100644 index d030478..0000000 --- a/projects/microb2010/sensorboard/img_processing.c +++ /dev/null @@ -1,1619 +0,0 @@ -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include - -#include "img_processing.h" - - -#define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) - -#ifndef HOST_VERSION - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "main.h" - -#define IMG_DEBUG(args...) DEBUG(E_USER_IMGPROCESS, args) -#define IMG_NOTICE(args...) NOTICE(E_USER_IMGPROCESS, args) -#define IMG_ERROR(args...) ERROR(E_USER_IMGPROCESS, args) - -#else - -#define IMG_DEBUG(args...) debug_printf(args) -#define IMG_NOTICE(args...) debug_printf(args) -#define IMG_ERROR(args...) debug_printf(args) - -#endif - - - - -#define OBJECT_MINIMUM_DEMI_PERIMETER (2*3) -/* store object in pool if never seen - * returns: - * 1 if new object - * 0 if already known - */ -int store_obj(Object_bb* tab_o, int total, Object_bb*o) -{ - uint8_t i; - - if (o->x_max - o->x_min + o->y_max - o->y_min < OBJECT_MINIMUM_DEMI_PERIMETER) - return 0; - - for (i=0;ix_max = 0; - o->y_max = 0; - o->x_min = x_in; - o->y_min = y_in; - - - while(1){ - if (pos_x == start_x && pos_y == start_y){ - count_stand++; - if (count_stand>4) - break; - if( v.x == vi.x && v.y == vi.y && start) - break; - } - - if (pos_xx_min) - o->x_min = pos_x; - if (pos_yy_min) - o->y_min = pos_y; - if (pos_x>o->x_max) - o->x_max = pos_x; - if (pos_y>o->y_max) - o->y_max = pos_y; - - /* is next pixel is good color */ - if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ - pos_x = pos_x+v.x; - pos_y = pos_y+v.y; - len++; - vect_rot_retro(&v); - start = 1; - continue; - } - vect_rot_trigo(&v); - } - - o->len = len; -} - - - - - -/* step around object of given color and computes its polygon */ -void object_find_poly(unsigned char* data, - int16_t x_in, int16_t y_in, - int16_t start_x, int16_t start_y, - int16_t color, int16_t color_w, Object_poly* o) -{ - int16_t pos_x = start_x; - int16_t pos_y = start_y; - int16_t start =0; - int16_t len = 0; - int16_t count_stand = 0; - uint16_t pt_step, pt_num; - vect_t v, vi; - - v.x = 1; - v.y = 0; - - vi = v; - - pt_step = o->len/POLY_MAX_PTS + 1; - - pt_num = 0; - - while(1){ - if (pos_x == start_x && pos_y == start_y){ - count_stand++; - if (count_stand>4) - break; - if( v.x == vi.x && v.y == vi.y && start) - break; - } - - /* is next pixel is good color */ - if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ - pos_x = pos_x+v.x; - pos_y = pos_y+v.y; - len++; - - if (len >pt_num*pt_step){ - o->pts[pt_num].x = pos_x; - o->pts[pt_num].y = pos_y; - pt_num+=1; - if (pt_num>=POLY_MAX_PTS) - break; - } - - vect_rot_retro(&v); - start = 1; - continue; - } - vect_rot_trigo(&v); - } - - o->pts_num = pt_num; - -} - -#define PT_LEFT 0 -#define PT_RIGHT 2 -#define PT_TOP 1 -#define PT_DOWN 3 - -/* return most left/right/top/down pts indexes of given polygon */ -void object_find_extrem_points_index(Object_poly* o, - unsigned int *pts) -{ - unsigned int i; - for (i=0;i<4;i++) - pts[i] = 0; - - - for (i=1;ipts_num;i++){ - if (o->pts[i].x < o->pts[pts[PT_LEFT]].x) - pts[PT_LEFT] = i; - if (o->pts[i].x > o->pts[pts[PT_RIGHT]].x) - pts[PT_RIGHT] = i; - if (o->pts[i].y < o->pts[pts[PT_TOP]].y) - pts[PT_TOP] = i; - if (o->pts[i].y > o->pts[pts[PT_DOWN]].y) - pts[PT_DOWN] = i; - } - -} - - -#define NUM_CALIPERS 4 -/* for debug purpose: display a vector on image */ -void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in, - vect_t *v, point_t p) -{ - unsigned int i; - float n; - int16_t x, y; - float coef=1.0; - - if (!v->x && !v->y) - return; - - n = vect_norm(v); - coef = 1/n; - for (i=0;i<5;i++){ - x = p.x; - y = p.y; - - x+=(float)((v->x)*(float)i*(float)coef); - y+=(float)((v->y)*(float)i*(float)coef); - if ((x== p.x) && (y == p.y)) - buf[y*x_in+x] = 0x0; - else - buf[y*x_in+x] = 0x0; - } - -} - -#define CAL_X 3 -#define CAL_Y 0 - - -/* compute minimum rectangle area including the given convex polygon */ -void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out, - vect_t *v_out, vect_t *r1, vect_t*r2) -{ - vect_t calipers[NUM_CALIPERS]; - vect_t edges[NUM_CALIPERS]; - - unsigned int i; - unsigned int calipers_pts_index[NUM_CALIPERS]; - float angles[NUM_CALIPERS]; - float min_angle; - float total_rot_angle = 0; - int caliper_result_index; - - vect_t res1, res2; - float ps, n1, n2, caliper_n; - - float aera_tmp; - /* XXX hack sould be max*/ - float aera_min=0x100000; - - object_find_extrem_points_index(o, calipers_pts_index); - - calipers[0].x = 0; - calipers[0].y = 1; - - calipers[1].x = -1; - calipers[1].y = 0; - - calipers[2].x = 0; - calipers[2].y = -1; - - calipers[3].x = 1; - calipers[3].y = 0; - - - while (total_rot_angle <= M_PI/2){ - - for (i=0;ipts[(calipers_pts_index[i] + 1)%o->pts_num].x - - o->pts[calipers_pts_index[i]].x; - edges[i].y = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].y - - o->pts[calipers_pts_index[i]].y; - - /* compute angle between caliper and polygon edge */ - angles[i] = vect_get_angle(&edges[i], &calipers[i]); - } - - /* find min angle */ - min_angle = angles[0]; - caliper_result_index = 0; - for (i=1;ipts_num; i++){ - calipers[(i+1) % NUM_CALIPERS] = calipers[i % NUM_CALIPERS]; - vect_rot_trigo(&calipers[(i+1) % NUM_CALIPERS]); - } - - /* update calipers point */ - for (i=0;ipts_num; - } - - res1.x = o->pts[calipers_pts_index[2]].x - o->pts[calipers_pts_index[0]].x; - res1.y = o->pts[calipers_pts_index[2]].y - o->pts[calipers_pts_index[0]].y; - - res2.x = o->pts[calipers_pts_index[3]].x - o->pts[calipers_pts_index[1]].x; - res2.y = o->pts[calipers_pts_index[3]].y - o->pts[calipers_pts_index[1]].y; - - ps = vect_pscal(&res1, &calipers[CAL_X]); - n1 = vect_norm(&res1); - caliper_n = vect_norm(&calipers[CAL_X]); - caliper_n*=caliper_n; - - res1 = calipers[CAL_X]; - - res1.x *= ps/(caliper_n); - res1.y *= ps/(caliper_n); - - - ps = vect_pscal(&res2, &calipers[CAL_Y]); - n1 = vect_norm(&res2); - - res2 = calipers[CAL_Y]; - - res2.x *= ps/(caliper_n); - res2.y *= ps/(caliper_n); - - n1 = vect_norm(&res1); - n2 = vect_norm(&res2); - - aera_tmp = n1*n2; - - if (aera_min >aera_tmp){ - aera_min = aera_tmp; - for (i=0;ipts[pts_index_out[i]].x; - p1.y = o->pts[pts_index_out[i]].y; - - p2.x = p1.x+COEF_CALIP*caliper_tmp.x; - p2.y = p1.y+COEF_CALIP*caliper_tmp.y; - - pts2line(&p1, &p2, &l1); - - vect_rot_trigo(&caliper_tmp); - - p1.x = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].x; - p1.y = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].y; - - p2.x = p1.x+COEF_CALIP*caliper_tmp.x; - p2.y = p1.y+COEF_CALIP*caliper_tmp.y; - - pts2line(&p1, &p2, &l2); - - ret = intersect_line(&l1, &l2, &p_int1); - if (ret!=1) - return 0; - //IMG_DEBUG("int1 (%d): %" PRIi32 " %" PRIi32 " ", ret, p_int1.x, p_int1.y); - - mp_x+=p_int1.x; - mp_y+=p_int1.y; - - } - - - - p->x = lround(mp_x/NUM_CALIPERS); - p->y = lround(mp_y/NUM_CALIPERS); - - - return 1; - -} - -#define OBJECT_DIM 5 -/* split zone in many column's sized area */ -int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color) -{ - int n1, n2; - int i, j; - int index=0; - int r1_s, r2_s; - point_t ptmp; - - - n1 = vect_norm(r1); - n2 = vect_norm(r2); - - r1_s = n1/OBJECT_DIM; - r2_s = n2/OBJECT_DIM; - - if (!r1_s || ! r2_s) - return 0; - - ptmp.x = p->x - r1->x/2 - r2->x/2 + (r1->x/(r1_s*2))+(r2->x/(r2_s*2)); - ptmp.y = p->y - r1->y/2 - r2->y/2 + (r1->y/(r1_s*2))+(r2->y/(r2_s*2)); - - for(i=0;ix)/r1_s+(j*r2->x)/r2_s; - zones[index].p.y = ptmp.y + (i*r1->y)/r1_s+(j*r2->y)/r2_s; - zones[index].h = color; - zones[index].valid = 1; - - index++; - if (index>=max_zone) - return index; - - - } - } - - return index; -} - -#define OBJECT_SEMI_DIM (OBJECT_DIM/2) -#define MIN_SURFACE_PERCENT 50 -#define HIGHER_MAX_PIXEL 5 - - -int zone_has_enought_pixels(unsigned char* data, int16_t x_in, int16_t y_in, zone* z) -{ - int x, y; - uint16_t count, total_pix, higher_pixels; - - count = 0; - total_pix=0; - higher_pixels = 0; - - for (x = -OBJECT_SEMI_DIM; - (x <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); - x++){ - for (y = -OBJECT_SEMI_DIM; - (y <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); - y++){ - total_pix++; - if (data[x_in * (y + z->p.y) + x + z->p.x] == z->h) - count++; - - if (data[x_in * (y + z->p.y) + x + z->p.x] > z->h) - higher_pixels++; - - } - - } - - IMG_DEBUG("has enougth pixel (h: %d x %"PRIi32": y:%"PRIi32") total: %d/%d (tt: %d, hmax: %d)", z->h, z->p.x, z->p.y, - count, (total_pix * MIN_SURFACE_PERCENT) / 100, total_pix, higher_pixels); - - if ((count > (total_pix * MIN_SURFACE_PERCENT) / 100) && - (higher_pixels (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin)) - continue; - - p[i].valid = 0; - - } - - return zones_num; -} - -#define MAX_DIST_TO_ZONE 2 - -unsigned int zone_filter_zone_rect(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y , uint8_t working_zone) -{ - int i; - - for (i = 0; i < zones_num; i++){ - - IMG_DEBUG("rct x:%"PRIi32" y:%"PRIi32" (centerx: %d)",p[i].p.x , p[i].p.y, center_x); - - if ((p[i].p.x > center_x - MAX_DIST_TO_ZONE) && (p[i].h > working_zone)) - continue; - - p[i].valid = 0; - } - - return zones_num; -} - - -/* delete point to render polygon convex */ -int object_poly_to_convex(Object_poly *o) -{ - unsigned int i, j; - vect_t v, w; - int16_t z; - unsigned int del_pts_num = 0; - - for (i=0;ipts_num;){ - v.x = o->pts[(i + o->pts_num - 1)%o->pts_num].x - o->pts[i].x; - v.y = o->pts[(i + o->pts_num - 1)%o->pts_num].y - o->pts[i].y; - - w.x = o->pts[(i+1)%o->pts_num].x - o->pts[i].x; - w.y = o->pts[(i+1)%o->pts_num].y - o->pts[i].y; - - z = vect_pvect(&v, &w); - if (z>0){ - i+=1; - continue; - } - - /* found a convex angle (or colinear points) */ - for (j = i; j < o->pts_num-1; j++){ - o->pts[j] = o->pts[j+1]; - } - if (i!=0) - i-=1; - o->pts_num--; - del_pts_num++; - } - - return del_pts_num; -} - - - - - -#define DEFAULT_COLOR 255 -/* scan all image and find objects*/ -unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in, - Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj) -{ - int16_t i, obj_num; - - uint8_t in_color=0; - int16_t etat; - - Object_bb o; - int ret; - - obj_num = 0; - /* - first, delete borders - */ - for (i=0;i= MAX_SISTER_PER_ZONE) - break; - - - if (!zones[i].valid) - continue; - - - if (i == z_n) - continue; - - /* twin tower must have same high */ - if (zones[i].h != zones[z_n].h) - continue; - - IMG_DEBUG("test sisters (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - zones[z_n].p.x, zones[z_n].p.y, - zones[i].p.x, zones[i].p.y); - - - dist = sqrt( (zones[i].p.x - zones[z_n].p.x) * (zones[i].p.x - zones[z_n].p.x) + - (zones[i].p.y - zones[z_n].p.y) * (zones[i].p.y - zones[z_n].p.y) ); - - - IMG_DEBUG("sister space is %2.2f may be near %d", dist, SPACE_INTER_TWIN_TOWER); - - /* - twin tower must be close/far enought to drop lintel - */ - if (ABS(dist - SPACE_INTER_TWIN_TOWER) > SPACE_INTER_TWIN_TOWER_TOLERENCE) - continue; - - - pts2line(&zones[i].p, &zones[z_n].p, &l); - - /* - test the paralelism of the temple: - zone may be on same distance from center - */ - - - v1.x = zones[z_n].p.x - center_x; - v1.y = zones[z_n].p.y - center_y; - - dist = vect_norm(&v1); - - v2.x = zones[i].p.x - center_x; - v2.y = zones[i].p.y - center_y; - - dist2 = vect_norm(&v2); - - IMG_DEBUG("zone dist %2.2f %2.2f", dist, dist2); - if (ABS(dist-dist2) > 3){ - IMG_DEBUG("bad parallelism %2.2f", ABS(dist-dist2)); - continue; - } - - - - - /* no other aligned tower to avoid dropping on a lintel - * (3 aligned zone may mean lintel) - */ - - good_zone = 1; - - for (j = 0; j < zones_num; j++){ - if (j==i ||j == z_n) - continue; - - /* if third zone, but lower */ - if (zones[j].h <= zones[i].h) - continue; - - /* - check distance from dropping zone to - line formed by twin towers - */ - - proj_pt_line(&zones[j].p, &l, &p); - - - /* test if projected point is in the segement */ - - - v.x = zones[z_n].p.x - zones[i].p.x; - v.y = zones[z_n].p.y - zones[i].p.y; - - v1.x = p.x - zones[i].p.x; - v1.y = p.y - zones[i].p.y; - - n1 = vect_pscal_sign(&v, &v1); - - v.x = -v.x; - v.y = -v.y; - - v1.x = p.x - zones[z_n].p.x; - v1.y = p.y - zones[z_n].p.y; - - - n2 =vect_pscal_sign(&v, &v1); - - v.x = p.x - zones[j].p.x; - v.y = p.y - zones[j].p.y; - - dist = vect_norm(&v); - IMG_DEBUG("dist pt h %d n: (%d %d) (%"PRIi32" %"PRIi32") to line %2.2f", zones[j].h, n1, n2, zones[j].p.x, zones[j].p.y, dist); - - - if ((n1>=0 && n2>=0) && dist < OBJECT_DIM+2.){ - good_zone = 0; - break; - } - - - /* test if zone is far from points*/ - - v1.x = zones[j].p.x - zones[z_n].p.x; - v1.y = zones[j].p.y - zones[z_n].p.y; - - dist = vect_norm(&v1); - IMG_DEBUG("dist pt to z1 %2.2f", dist); - - if (dist < OBJECT_DIM){ - good_zone = 0; - break; - } - - v2.x = zones[j].p.x - zones[i].p.x; - v2.y = zones[j].p.y - zones[i].p.y; - - - dist = vect_norm(&v2); - IMG_DEBUG("dist pt to z2 %2.2f", dist); - - if (dist < OBJECT_DIM){ - good_zone = 0; - break; - } - - - - z1 = i; - z2 = z_n; - z3 = j; - - - - - - /* - XXX may be a lintel on lintel !! - */ - - } - - if (!good_zone) - continue; - - IMG_DEBUG("sisters ok (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - zones[z_n].p.x, zones[z_n].p.y, - zones[i].p.x, zones[i].p.y); - - - sisters[z_n][current_sister] = i; - current_sister++; - } - } -} - -/* test if a higher zone is too close */ -int test_close_zone(uint8_t zones_num, zone* zones, unsigned int z_n) -{ - uint8_t i; - vect_t v; - double dist; - - for (i = 0; i < zones_num; i++){ - if (i == z_n) - continue; - if (zones[i].h <= zones[z_n].h) - continue; - - v.x = zones[i].p.x - zones[z_n].p.x; - v.y = zones[i].p.y - zones[z_n].p.y; - - dist = vect_norm(&v); - //IMG_DEBUG("dist pt to pt %2.2f", dist); - - if (dist < OBJECT_DIM){ - return 1; - } - - } - - return 0; -} - -#define MAX_COLUMN 4 -#define MAX_LINTEL 2 - -drop_column_zone drop_c[MAX_COLUMN]; -drop_lintel_zone drop_l[MAX_LINTEL]; - - -void reset_drop_zone(void) -{ - memset(drop_c, 0, sizeof(drop_c)); - memset(drop_l, 0, sizeof(drop_l)); - -} - - -void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones) -{ - unsigned int i; - - for (i=0;i(b)?(a):(b)) - - -#if 0 -#define MAX_DROP_HIGH 8 - - -/* - recursive function to maximize points during object - dropping, given lintel/column number - working zone may be 1, 2 or 3 - */ -unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max, - uint8_t n_columns, uint8_t n_lintels, - uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone) -{ - - uint8_t i, j; - unsigned int points_calc; - //unsigned int points_added = 0; - int sister; - int ret; - - - /* if no more objects, return points */ - if (n_columns == 0 && n_lintels == 0) - return MY_MAX(points, points_max); - - /* start by putting columns if so */ - for (i = 0; i < zones_num; i++){ - if (zones[i].h >= MAX_DROP_HIGH) - continue; - - if (n_columns){ - - ret = test_close_zone(zones_num, zones, i); - if (ret) - continue; - - zones[i].h++; - points_calc = solve_objects_dropping(points + zones[i].h, points_max, - n_columns - 1, n_lintels, - zones_num, zones, sisters, working_zone); - - if (points_calc > points_max){ - points_max = points_calc; - drop_c[n_columns - 1].z = i; - drop_c[n_columns - 1].h = zones[i].h; - drop_c[n_columns - 1].valid = 1; - - } - zones[i].h--; - } - /* we must depose all columns before dropping lintels */ - else if (n_lintels){ - - /* dont drop lintel on ground */ - if (zones[i].h <= working_zone) - continue; - - /* XXX todo need get second zone near selected one */ - //ret = find_twin_tower(zones_num, zones, i, &sister); - - for (j = 0; j < MAX_SISTER_PER_ZONE; j++){ - sister = sisters[i][j]; - if (sister == -1) - break; - if (zones[i].h != zones[sister].h){ - sister = -1; - } - - } - - if (sister == -1) - continue; - //IMG_DEBUG("sister found: %d %d (h=%d %p)", i, sister, zones[i].h, &zones[i].h); - - zones[i].h++; - zones[sister].h++; - - - points_calc = solve_objects_dropping(points + zones[i].h * 3, points_max, - n_columns, n_lintels - 1, - zones_num, zones, sisters, working_zone); - - if (points_calc > points_max){ - points_max = points_calc; - - drop_l[n_lintels - 1].z1 = i; - drop_l[n_lintels - 1].z2 = sister; - drop_l[n_lintels - 1].h = zones[i].h; - drop_l[n_lintels - 1].valid = 1; - - } - - - zones[sister].h--; - zones[i].h--; - } - } - - return MY_MAX(points, points_max); -} -#endif - - -/* */ -int find_column_dropzone(uint8_t zones_num, zone* zones) -{ - uint8_t i; - - uint8_t z_n = 0; - - if (zones_num <= 0) - return -1; - - for (i = 0; i < zones_num; i++){ - if (!zones[i].valid) - continue; - if (zones[i].h > zones[z_n].h) - z_n = i; - } - - - /* - now, chose dropzone closest to robot - meaning little x, big y - so maximise y-x - */ - for (i = 0; i < zones_num; i++){ - if (zones[i].h != zones[z_n].h) - continue; - if (!zones[i].valid) - continue; - if (zones[i].p.y - zones[i].p.x > zones[z_n].p.y - zones[z_n].p.x) - z_n = i; - } - - - - return z_n; -} - - - -uint8_t color2h(uint8_t color) -{ - return (0x100-color)/0x20; -} - -uint8_t h2color(uint8_t color) -{ - return color*0x20; -} - - -#define NUM_ZONE_GENERATE 8 -#define DIST_ZONE_GEN 9 - -/* - remove zone at ground level, and generate zones on - a circle at X cm from center - */ -unsigned int generate_center_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, - zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y) -{ - double c_a, s_a; - uint8_t i, j; - double px1, py1, px2, py2; - - - /* first del zone at level 2 */ - for (i = 0; i < zones_num; ){ - if (zones[i].h!=2){ - i++; - continue; - } - - for (j = i; j < zones_num-1; j++) - zones[j] = zones[j+1]; - - zones_num--; - - } - - /* generate zones around circle */ - - c_a = cos(2*M_PI/NUM_ZONE_GENERATE); - s_a = sin(2*M_PI/NUM_ZONE_GENERATE); - - px1 = DIST_ZONE_GEN; - py1 = 0; - - for (i = 0; i < NUM_ZONE_GENERATE; i++){ - - zones[zones_num].p.x = center_x + px1; - zones[zones_num].p.y = center_y + py1; - zones[zones_num].h = 2; - zones[zones_num].valid = 1; - - - px2 = px1*c_a + py1*s_a; - py2 = -px1*s_a + py1*c_a; - - px1 = px2; - py1 = py2; - - /* skip zone if it is not in img */ - if (zones[zones_num].p.x < 0 || zones[zones_num].p.y < 0 || - zones[zones_num].p.x >= x_in || zones[zones_num].p.y > y_in) - continue; - - /* skip zone if not enougth pixels */ - if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) - continue; - - zones_num++; - if (zones_num >= max_zones) - break; - - } - - return zones_num; - - -} - - -/* - remove zone at ground level, and generate zones on - a line at X cm from robot - */ -unsigned int generate_rectangle_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, - zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y, - uint8_t working_zone) -{ - uint8_t i, j; - uint8_t y; - - /* first del zone at level i */ - for (i = 0; i < zones_num; ){ - if (zones[i].h != working_zone ){ - i++; - continue; - } - - for (j = i; j < zones_num-1; j++) - zones[j] = zones[j+1]; - - zones_num--; - } - - - /* generate zones on a line */ - for (y = OBJECT_DIM; y < y_in; y+=OBJECT_DIM){ - - zones[zones_num].p.x = center_x; - zones[zones_num].p.y = y; - zones[zones_num].h = working_zone ; - zones[zones_num].valid = 1; - - if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) - continue; - zones_num++; - - if (zones_num >= max_zones) - break; - - } - return zones_num; - -} - - -#define MAX_DECAL_LINE 5 -#define ENOUGHT_ZONE_PIXEL 2 - -void recal_img_y(unsigned char* buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone) -{ - uint8_t i, j; - uint8_t cpt; - - /* recal img only for central zone */ - if (working_zone !=2) - return; - - for (i = 0; i < MAX_DECAL_LINE; i++){ - cpt = 0; - for (j = 0; j < x_in; j++){ - if (buffer[i*x_in + j] ==2) - cpt++; - } - - if (cpt >= ENOUGHT_ZONE_PIXEL) - break; - } - - memmove(buffer, &buffer[i * x_in], x_in * y_in - i*x_in); - memset(&buffer[x_in * y_in - i * x_in], 0, i*x_in); -} - - -#define MAX_OBJECTS 20 - -#define MAX_ZONES_PER_OBJECT 20 - - -uint8_t g_zone_num; -zone g_all_zones[MAX_ZONES]; - -uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in, - zone * all_zones, uint8_t max_zones) -{ - - int ret; - int i, j; - - zone zones[MAX_ZONES_PER_OBJECT]; - - vect_t caliper; - unsigned int pts_cal[4]; - point_t ptmp; - vect_t r1, r2; - int zone_len; - - - uint8_t zone_num = 0; - - Object_bb sac_obj[MAX_OBJECTS]; - Object_poly sac_obj_poly[MAX_OBJECTS]; - - - - /* - XXX fix: only decal for working zone 2/(1?) - but we dont have info yet - */ - recal_img_y(buffer, x_in, y_in, 2); - - memset(sac_obj, 0, sizeof(sac_obj)); - memset(sac_obj_poly, 0, sizeof(sac_obj_poly)); - - - /* first, find polygons*/ - parcour_img(buffer, x_in, y_in, sac_obj, sac_obj_poly, MAX_OBJECTS); - - /* enclose each poygon in the min area polygon - then, split each rectangle in dropping zone - */ - for (i=0;i 0; i--){ - z.h = i; - z.valid = 1; - ret = zone_has_enought_pixels(buffer, x_in, y_in, &z); - IMG_NOTICE("test z3:(h=%d) %d", i, ret); - - if (ret) - return 1; - } - - return 0; - -} - - -int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone, int16_t center_x, int16_t center_y, - int16_t * dropzone_x, int16_t * dropzone_y) -{ - uint8_t zone_num; - int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE]; - int8_t z_n = -1; - uint8_t i, j; - - - /* find all drop zone */ - zone_num = filter_zones(buffer, x_in, y_in, - g_all_zones, g_zone_num, MAX_ZONES, - working_zone, center_x, center_y, - -2); - - /* precompute possible twin towers */ - find_twin_tower(zone_num, g_all_zones, sisters, center_x, center_y); - - - for (i=0; i< zone_num; i++){ - IMG_DEBUG("all sisters: %d", i); - for (j=0;j g_all_zones[i].h) - continue; - - z_n = i; - } - - IMG_NOTICE("twin tower found :z_n=%d", z_n); - if (z_n == -1) - return -1; - - IMG_NOTICE("(%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", - g_all_zones[z_n].p.x, g_all_zones[z_n].p.y, - g_all_zones[sisters[z_n][0]].p.x, g_all_zones[sisters[z_n][0]].p.y); - - - *dropzone_x = ((double)(g_all_zones[z_n].p.x + g_all_zones[sisters[z_n][0]].p.x)*PIXEL2CM ) / 2; - *dropzone_y = ((double)(g_all_zones[z_n].p.y + g_all_zones[sisters[z_n][0]].p.y)*PIXEL2CM ) / 2 + 30; - - - return g_all_zones[z_n].h; - -} - diff --git a/projects/microb2010/sensorboard/img_processing.h b/projects/microb2010/sensorboard/img_processing.h deleted file mode 100644 index 4c06237..0000000 --- a/projects/microb2010/sensorboard/img_processing.h +++ /dev/null @@ -1,111 +0,0 @@ - - - -typedef struct _Object_bb -{ - uint8_t x_min; - uint8_t x_max; - uint8_t y_min; - uint8_t y_max; - uint16_t len; -}Object_bb; - -#define POLY_MAX_PTS 12 -typedef struct _Object_poly -{ - uint16_t pixels_perim; - uint16_t pts_num; - point_t pts[POLY_MAX_PTS]; - uint16_t len; - uint8_t color; -}Object_poly; - - -typedef struct _zone -{ - point_t p; - uint8_t h:7; - uint8_t valid:1; -}zone; - - -typedef struct _drop_column_zone -{ - uint8_t valid; - uint8_t z; - uint8_t h; -}drop_column_zone; - -typedef struct _drop_lintel_zone -{ - uint8_t valid; - uint8_t z1; - uint8_t h; - uint8_t z2; -}drop_lintel_zone; - - - - -#define MAX_ZONES 30 - -#define MAX_SISTER_PER_ZONE 1 - - - -unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in, Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj); - -float vect_get_angle(vect_t*v, vect_t* w); - -void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out, vect_t *v_out, vect_t *r1, vect_t*r2); - -void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in, vect_t *v, point_t p); - -void vect_rot_trigo(vect_t* v); - -int object_poly_caliper_to_rectangle(Object_poly *o, - unsigned int *pts_index_out, vect_t* caliper, - vect_t *r1, vect_t*r2, point_t *p); - -int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color); - - - -int zone_filtre_min_surface(unsigned char* data, int16_t x_in, int16_t y_in, - uint8_t color, unsigned int num_zone, zone* p); - -void reset_drop_zone(void); -void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones); - -unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max, - uint8_t n_columns, uint8_t n_lintels, - uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone); - -uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in, - zone * all_zones, uint8_t max_zones); - - -uint8_t color2h(uint8_t color); -uint8_t h2color(uint8_t color); - -int8_t get_column_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone, int16_t center_x, int16_t center_y, - int16_t * dropzone_x, int16_t * dropzone_y); - - -uint8_t is_temple_there(unsigned char * buffer, int16_t x_in, int16_t y_in, - uint8_t h, int16_t center_x, int16_t center_y); - - - -int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, - uint8_t working_zone, int16_t center_x, int16_t center_y, - int16_t * dropzone_x, int16_t * dropzone_y); - -void process_img_to_zone(unsigned char *buffer, int16_t x_in, int16_t y_in); - -#define PIXEL2CM (300./27.) - - -extern uint8_t g_zone_num; -extern zone g_all_zones[MAX_ZONES]; diff --git a/projects/microb2010/sensorboard/main.c b/projects/microb2010/sensorboard/main.c deleted file mode 100755 index 82fa2a5..0000000 --- a/projects/microb2010/sensorboard/main.c +++ /dev/null @@ -1,279 +0,0 @@ -/* - * Copyright Droids Corporation - * Olivier Matz - * - * 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 -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#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 sensorboard sensorboard; - -/***********************/ - -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(&sensorboard, 0, sizeof(sensorboard)); - sensorboard.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_SENSORBOARD); - /* check eeprom to avoid to run the bad program */ - if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != - EEPROM_MAGIC_SENSORBOARD) { - 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_SENSORBOARD_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(); - - /* beacon */ - beacon_init(); - scheduler_add_periodical_event_priority(beacon_calc, NULL, - 20000L / SCHEDULER_UNIT, - BEACON_PRIO); - - - - /* scan */ - - - scanner_init(); - scheduler_add_periodical_event_priority(do_scan, NULL, - (1024L*1L) / SCHEDULER_UNIT, - CS_PRIO-1); - - sei(); - - scan_params.speed = SCAN_DEFAULT_SPEED; - scan_params.debug = 0; - - scanner_calibre_mirror(); - scanner_calibre_laser(); - - beacon_calibre_pos(); - - - 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/sensorboard/main.h b/projects/microb2010/sensorboard/main.h deleted file mode 100755 index 3a4ff83..0000000 --- a/projects/microb2010/sensorboard/main.h +++ /dev/null @@ -1,154 +0,0 @@ -/* - * 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 BEACON_ENCODER ((void *)0) -#define SCANNER_ENCODER ((void *)1) - -#define BEACON_PWM ((void *)&gen.pwm1_4A) -#define SCANNER_PWM ((void *)&gen.pwm2_4B) -#define PICKUP_WHEEL_L_PWM ((void *)&gen.pwm3_1A) -#define PICKUP_WHEEL_R_PWM ((void *)&gen.pwm4_1B) - -#define BEACON_POS_SENSOR 2 -#define SCANNER_POS_SENSOR 7 -#define SCANNER_MAXCOLUMN_SENSOR 1 - - - -#define SCANNER_POS_OUT 179 -#define SCANNER_POS_CALIBRE 370 -#define SCANNER_POS_IN 400 - - -/** 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 BEACON_PRIO 80 -#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; -}; - -/* sensorboard specific */ -struct sensorboard { -#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 beacon; - struct cs_block scanner; - - /* robot status */ - uint8_t our_color; -}; - -extern struct genboard gen; -extern struct sensorboard sensorboard; - -/* 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/sensorboard/pid_config.h b/projects/microb2010/sensorboard/pid_config.h deleted file mode 100755 index fa95f08..0000000 --- a/projects/microb2010/sensorboard/pid_config.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * 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/sensorboard/rdline_config.h b/projects/microb2010/sensorboard/rdline_config.h deleted file mode 100644 index e69de29..0000000 diff --git a/projects/microb2010/sensorboard/scanner.c b/projects/microb2010/sensorboard/scanner.c deleted file mode 100644 index dfc869e..0000000 --- a/projects/microb2010/sensorboard/scanner.c +++ /dev/null @@ -1,898 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include "sensor.h" - -#include - -#include "main.h" -#include "scanner.h" - -#include "cmdline.h" - - -#include "scan_h_l.h" - - -#include -#include "img_processing.h" - - -#include "../common/i2c_commands.h" - -#define SCANNER_DEBUG(args...) DEBUG(E_USER_SCANNER, args) -#define SCANNER_NOTICE(args...) NOTICE(E_USER_SCANNER, args) -#define SCANNER_ERROR(args...) ERROR(E_USER_SCANNER, args) - - -#define MODULO_TIMER (1023L) - -#define COEFF_TIMER (2) -#define COEFF_MULT (1000L) -#define COEFF_MULT2 (1000L) - -double TELEMETRE_A = TELEMETRE_A_INIT; -double TELEMETRE_B = TELEMETRE_B_INIT; - - - - -struct scan_params scan_params; - -static volatile int32_t scan_delta_pos; -static volatile int32_t scan_tick_cur = 0; -static volatile int32_t scan_tick_prev = 0; -/*static volatile int32_t count = 0; -static volatile int32_t count_diff_rising = 0; -static volatile int32_t count_diff_falling = 0; -*/ -static int32_t scanner_coeff = 0; - -//static volatile int8_t valid_scanner = 0; - -static volatile int32_t scan_pos_cur = 0; - -static int32_t pos_ref = 0; - - -int32_t encoders_spi_get_value_scanner(void *number) -{ - int32_t ret; - - ret = encoders_spi_get_value(number); - return ret*4; -} - - -void encoders_spi_set_value_scanner(void * number, int32_t v) -{ - encoders_spi_set_value(number, v/4); -} - -int32_t encoders_spi_update_scanner(void * number) -{ - int32_t ret; - uint8_t flags; - - IRQ_LOCK(flags); - ret = encoders_spi_get_value_scanner(number); - scan_delta_pos = ret - scan_pos_cur; - scan_pos_cur = ret; - scan_tick_prev = scan_tick_cur; - scan_tick_cur = TCNT3; - - scanner_coeff = (scan_delta_pos * COEFF_MULT) / - ((scan_tick_cur - scan_tick_prev + MODULO_TIMER + 1) & MODULO_TIMER); - - IRQ_UNLOCK(flags); - - - return ret; -} - -int32_t encoders_spi_get_value_scanner_interpolation(void * number) -{ - uint8_t flags; - int32_t pos; - - IRQ_LOCK(flags); - pos = scan_pos_cur; - pos += (scanner_coeff * ((TCNT3 - scan_tick_cur + MODULO_TIMER + 1)& MODULO_TIMER ))/ - COEFF_MULT; - - IRQ_UNLOCK(flags); - - return pos; -} - - -void scanner_reset_pos(void) -{ - pwm_ng_set(SCANNER_PWM, 0); - encoders_spi_set_value_scanner(SCANNER_ENCODER, 0); -} - -void scanner_init(void) -{ - - scan_params.working = 0; - scan_params.must_stop = 0; - - scanner_reset_pos(); - pos_ref = encoders_spi_get_value_scanner(SCANNER_ENCODER); - - //memset(&scanner, 0, sizeof(struct scanner)); - - scan_delta_pos = 0; - - /*for(i=0;i 0){ - time_wait_ms(10); - } - - /* arm in */ - pwm_ng_set(&gen.servo3, SCANNER_POS_IN); - -} - - -/* - * called from IRQ: - * mode can be off/prepare/start, see in i2c_commands.h - */ -void scanner_set_mode(uint8_t mode) -{ - if (mode == I2C_SENSORBOARD_SCANNER_PREPARE){ - /* reset flag max pos */ - scan_params.max_column_detected = 0; - - /* arm out */ - pwm_ng_set(&gen.servo3, SCANNER_POS_OUT); - - } - else if (mode == I2C_SENSORBOARD_SCANNER_STOP){ - /* arm in */ - pwm_ng_set(&gen.servo3, SCANNER_POS_IN); - scan_params.must_stop = 1; - } - else if (mode == I2C_SENSORBOARD_SCANNER_START){ - scan_params.max_column_detected = 0; - scan_params.must_stop = 0; - - - /* start scan in background */ - scanner_do_scan(); - } - - -} - -/* -void scanner_stop(void) -{ - sensorboard.scanner.on = 0; - pwm_ng_set(SCANNER_PWM, 0); -} -*/ - - -/* -int32_t encoders_spi_get_scanner_speed(void * dummy) -{ - return scanner_speed; -} -*/ - - -//uint8_t sample_tab[MAX_SAMPLE]; -//uint16_t sample_i = 0; - - -//#define offset_a (75.*M_PI/180.) -//float offset_a; -//float offset_b; - -//int32_t pos_start_scan; - - -/* get motor angle in radian; return mirror angle in radian, cos a sin a */ -void ang2_a_mirror(float b, float * c_a, float* s_a, float* a) -{ - float x2, y2; - float A, DELTA, B, D; - - b+=scan_params.offset_b; - x2 = X + l1*cos(b); - y2 = Y + l1*sin(b); - - A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3); - - DELTA = -(A*A - x2*x2 - y2*y2); - B = sqrt(DELTA); - - D = x2*x2 + y2*y2; - - *c_a = (x2*A + y2*B)/D; - *s_a = -(x2*B - y2*A)/D; - - *a = atan2(*s_a, *c_a); - - *a += scan_params.offset_a; - // *s_a = sin(*a); - // *c_a = cos(*a); - -} - -/* get telemeter dist , cos a, sin a, a and return H, L of scanned point */ -void ang2_H_L(float l_telemetre, float c_a, float s_a, float a, float *H, float *L) -{ - float d; - d = h_mirror*c_a/s_a; - *H = (l_telemetre - l_mirror - d)*sin(2*a); - *L = l_mirror + d + *H/tan(2*a); - - //*H+= 8*sin(a-scan_params.offset_a); -} - - - -//int32_t last_col_n; -//int32_t last_row_n; -//uint8_t last_sample; - -//uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123}; -//uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123}; - -/* last high is dummy, to deal higher columns */ -uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250}; -#define H_MARGIN 7 - - -#if 0 -void do_scan(void * dummy) -{ - - unsigned int i; - int16_t a; - int32_t row_n; - int32_t col_n; - - - int32_t tour_pos; - int32_t pos, last_pos; - int32_t pos_tmp ; - int32_t mot_pos; - float dist; - uint8_t min_sample; - - float b, c_a, s_a, H, L, m_a; - int32_t H_fin; - - - if (scan_params.sample_i==0) - return; - - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); - - if (scan_params.sample_i==1){ - SCANNER_DEBUG("dump end enc %ld %d ", mot_pos, PIX_PER_SCAN); - //scanner.flags &= (~CS_ON); - - - - /* stop scan at cur pos + 10 round */ - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); - mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ; - - SCANNER_DEBUG("set to %ld ", mot_pos); - - cs_set_consign(&sensorboard.scanner.cs, mot_pos); - //pwm_ng_set(SCANNER_MOT_PWM, 0); - - - } - - mot_pos-= scan_params.pos_start_scan; - - a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 ); - - - //dist = (a-TELEMETRE_B)/TELEMETRE_A; - dist = TELEMETRE_A * a +TELEMETRE_B; - - //SCANNER_DEBUG("enc val = %ld", encoders_microb_get_value((void *)SCANNER_ENC)); - - - //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a; - //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0; - - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); -#if 0 - /* separe scan forward/backword */ - if (row_n%2){ - row_n/=2; - } - else{ - row_n/=2; - row_n+=30; - } -#endif - - tour_pos = (mot_pos)%(SCANNER_STEP_TOUR); - - b = (2.*M_PI)*(float)tour_pos/(float)(SCANNER_STEP_TOUR); - - ang2_a_mirror(b, &c_a, &s_a, &m_a); - ang2_H_L(dist, c_a, s_a, m_a, &H, &L); - - - SCANNER_DEBUG("%ld %d", tour_pos, a); - - if (H >0){ - printf("zarb H\n"); - H = 0; - } - - if (dist> SCAN_MAX_DIST){ - H = 0; - L = 0; - } - - H = H;//m_a*180/M_PI; - L = L;//(L-100)*PIX_PER_SCAN; - - //SCANNER_DEBUG("polling : ADC0 = %i %f",a, dist); - //SCANNER_DEBUG("%f %f %2.2f %f", H, L, m_a*180./M_PI, dist); - - - //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI); - - H=(H+SCAN_H_MAX)*SCAN_H_COEF; - L-=SCAN_L_MIN; - - - /* XXX may never append */ - if (L<0) - L=0; - - - /* first filter => pixel modulo level */ -#define H_BASE 10 -#define H_MUL 14 - H_fin = H;//+SCAN_H_MAX; - //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE; - - if (scan_params.filter){ - H_fin = 11; // default is level 11 - for (i=0;iPIX_PER_SCAN) - printf("BUG!!! RECALC MAX L"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos =DIM_DIST) - j = DIM_DIST-1; - - if (i>=DIM_ANGLE) - i = DIM_ANGLE-1; - - - val.u16 = pgm_read_word_near(&array_h_l[j][i]); - - - //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - //val.u16 = pgm_read_word_near(&array_h_l[a][tp]); - H = val.h_l.h; - L = val.h_l.l; - /* - val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - - H = val.h_l.h; - L = val.h_l.l; - */ - H_fin = H; - L_fin = L; - - - if (L<=0) - L = 0; - - col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN); - if (col_n>=PIX_PER_SCAN) { - //printf("BUG!!! RECALC MAX L"); - col_n = PIX_PER_SCAN-1; - } - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos - * - * 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 - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#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_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/sensorboard/sensor.h b/projects/microb2010/sensorboard/sensor.h deleted file mode 100644 index c2b18a7..0000000 --- a/projects/microb2010/sensorboard/sensor.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright Droids Corporation (2009) - * Olivier MATZ - * - * 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/sensorboard/spi_config.h b/projects/microb2010/sensorboard/spi_config.h deleted file mode 100644 index 76697c3..0000000 --- a/projects/microb2010/sensorboard/spi_config.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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/sensorboard/time_config.h b/projects/microb2010/sensorboard/time_config.h deleted file mode 100755 index e0f7f2a..0000000 --- a/projects/microb2010/sensorboard/time_config.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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/sensorboard/timer_config.h b/projects/microb2010/sensorboard/timer_config.h deleted file mode 100755 index 810525c..0000000 --- a/projects/microb2010/sensorboard/timer_config.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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/sensorboard/uart_config.h b/projects/microb2010/sensorboard/uart_config.h deleted file mode 100644 index 2a27e07..0000000 --- a/projects/microb2010/sensorboard/uart_config.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: uart_config.h,v 1.3 2009-05-27 20:04:07 zer0 Exp $ - * - */ - -/* Droids-corp 2004 - Zer0 - * config for uart module - */ - -#ifndef UART_CONFIG_H -#define UART_CONFIG_H - -/* - * UART1 definitions - */ - -/* compile uart1 fonctions, undefine it to pass compilation */ -#define UART1_COMPILE - -/* enable uart1 if == 1, disable if == 0 */ -#define UART1_ENABLED 1 - -/* enable uart1 interrupts if == 1, disable if == 0 */ -#define UART1_INTERRUPT_ENABLED 1 - -#define UART1_BAUDRATE 57600 - -/* - * if you enable this, the maximum baudrate you can reach is - * higher, but the precision is lower. - */ -#define UART1_USE_DOUBLE_SPEED 1 - -#define UART1_RX_FIFO_SIZE 64 -#define UART1_TX_FIFO_SIZE 64 -#define UART1_NBITS 8 - -#define UART1_PARITY UART_PARTITY_NONE - -#define UART1_STOP_BIT UART_STOP_BITS_1 - - -/* - * UART3 definitions - */ - -/* compile uart3 fonctions, undefine it to pass compilation */ -#define UART3_COMPILE - -/* enable uart3 if == 1, disable if == 0 */ -#define UART3_ENABLED 1 - -/* enable uart3 interrupts if == 1, disable if == 0 */ -#define UART3_INTERRUPT_ENABLED 1 - -#define UART3_BAUDRATE 57600 - -/* - * if you enable this, the maximum baudrate you can reach is - * higher, but the precision is lower. - */ -#define UART3_USE_DOUBLE_SPEED 1 -//#define UART3_USE_DOUBLE_SPEED 1 - -#define UART3_RX_FIFO_SIZE 64 -#define UART3_TX_FIFO_SIZE 64 -//#define UART3_NBITS 5 -//#define UART3_NBITS 6 -//#define UART3_NBITS 7 -#define UART3_NBITS 8 -//#define UART3_NBITS 9 - -#define UART3_PARITY UART_PARTITY_NONE -//#define UART3_PARITY UART_PARTITY_ODD -//#define UART3_PARITY UART_PARTITY_EVEN - -#define UART3_STOP_BIT UART_STOP_BITS_1 -//#define UART3_STOP_BIT UART_STOP_BITS_2 - - - - -/* .... same for uart 1, 2, 3 ... */ - -#endif - diff --git a/projects/microb2010/tests/beacon_tsop/Makefile b/projects/microb2010/tests/beacon_tsop/Makefile index 8b3940d..d8d5b40 100755 --- a/projects/microb2010/tests/beacon_tsop/Makefile +++ b/projects/microb2010/tests/beacon_tsop/Makefile @@ -3,7 +3,7 @@ TARGET = beacon_tsop AVERSIVE_DIR = ../../../.. # List C source files here. (C dependencies are automatically generated.) -SRC = main.c commands.c cmdline.c trigo.c +SRC = main.c commands.c cmdline.c trigo.c uart_proto.c # List Assembler source files here. # Make them always end in a capital .S. Files ending in a lowercase .s diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c index 0cf5319..855eb05 100755 --- a/projects/microb2010/tests/beacon_tsop/main.c +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -1,5 +1,5 @@ /* - * Copyright Droids Corporation (2009) + * Copyright Droids Corporation (2010) * Olivier Matz * * This program is free software; you can redistribute it and/or modify @@ -20,6 +20,8 @@ * */ +#include + #include #include @@ -28,69 +30,39 @@ #include #include #include +#include +#include +#include #include "cmdline.h" +#include "uart_proto.h" +#include "trigo.h" #include "main.h" - -/* beacon identifier: must be odd, 3 bits */ -#define BEACON_ID_MASK 0x7 -#define BEACON_ID_SHIFT 0 -#define FRAME_DATA_MASK 0xFF8 -#define FRAME_DATA_SHIFT 3 +#include "board2006.h" +//#include "board2010.h" /******************* TSOP */ -#define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */ -#ifdef BOARD2006 -#define INTx_TSOP INT6 -#define ISCx0_TSOP ISC60 -#define ISCx1_TSOP ISC61 -#define SIG_TSOP SIG_INTERRUPT6 -#define TSOP_READ() (!(PINE & 0x40)) -#else -#define INTx_TSOP INT4 -#define ISCx0_TSOP ISC40 -#define ISCx1_TSOP ISC41 -#define SIG_TSOP SIG_INTERRUPT4 -#define TSOP_READ() (!(PINE & 0x10)) -#endif - -//#define MODUL_455KHZ -#define MODUL_38KHZ - -#if (defined MODUL_455KHZ) -#define TSOP_FREQ_MHZ 0.455 -#define N_PERIODS 10. -#else -#define TSOP_FREQ_MHZ 0.038 -#define N_PERIODS 15. -#endif - -#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ) - -#define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US) -#define TSOP_TIME_LONG_US (2.5 * N_PERIODS * TSOP_PERIOD_US) - -#define TSOP_TIME_SHORT ((uint16_t)(TSOP_TIME_SHORT_US*2)) -#define TSOP_TIME_LONG ((uint16_t)(TSOP_TIME_LONG_US*2)) - -#define FRAME_LEN 16 -#define FRAME_MASK ((1UL << FRAME_LEN) - 1) - struct detected_frame { uint16_t frame; + uint16_t ref_time; uint16_t time; + uint16_t tick; }; /* frame */ struct frame_status { uint8_t led_cpt; - uint16_t start_angle_time; + uint16_t ref_time; + uint16_t start_time; uint16_t frame; uint16_t mask; uint16_t prev_time; + uint16_t time_long; + uint16_t time_short; uint8_t prev_tsop; uint8_t len; + uint8_t frame_len; uint8_t val; #define FRAME_RING_ORDER 4 #define FRAME_RING_SIZE (1<prev_time; /* first rising edge */ - if (status->len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) { + if (status->len == 0 && cur_tsop && diff_time > status->time_long) { status->len = 1; status->val = 1; status->frame = 0; - status->start_angle_time = cur_time - ref_time; + status->start_time = cur_time; + status->ref_time = ref_time; status->mask = 1; } /* any short edge */ - else if (status->len != 0 && diff_time < TSOP_TIME_SHORT) { + else if (status->len != 0 && diff_time < status->time_short) { if (status->len & 1) { if (status->val) status->frame |= status->mask; @@ -200,7 +186,7 @@ static inline void decode_frame(struct frame_status *status, status->len ++; } /* any long edge */ - else if (status->len != 0 && diff_time < TSOP_TIME_LONG) { + else if (status->len != 0 && diff_time < status->time_long) { status->val = !status->val; if (status->val) status->frame |= status->mask; @@ -213,12 +199,17 @@ static inline void decode_frame(struct frame_status *status, } /* end of frame */ - if (status->len == FRAME_LEN*2) { + if (status->len == status->frame_len*2) { uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK; + uint16_t frame_mask; + + frame_mask = (1 << status->frame_len) - 1; if (tail_next != status->head) { - status->ring[status->tail].frame = (status->frame & FRAME_MASK); - status->ring[status->tail].time = status->start_angle_time; + status->ring[status->tail].frame = (status->frame & frame_mask); + status->ring[status->tail].ref_time = status->ref_time; + status->ring[status->tail].time = status->start_time; + status->ring[status->tail].tick = tick; status->tail = tail_next; if ((status->led_cpt & 0x7) == 0) LED3_TOGGLE(); @@ -232,7 +223,7 @@ static inline void decode_frame(struct frame_status *status, } /* decode frame */ -SIGNAL(SIG_TSOP) { +SIGNAL(SIG_TSOP_STA) { static uint8_t running = 0; /* tsop status */ @@ -242,7 +233,7 @@ SIGNAL(SIG_TSOP) { ref_time = ICR3; cur_time = TCNT3; - cur_tsop = TSOP_READ(); + cur_tsop = TSOP_STA_READ(); /* avoid interruption stacking */ if (running) @@ -260,6 +251,35 @@ SIGNAL(SIG_TSOP) { running = 0; } +/* decode frame */ +SIGNAL(SIG_TSOP_OPP) { + static uint8_t running = 0; + + /* tsop status */ + uint8_t cur_tsop; + uint16_t ref_time; + uint16_t cur_time; + + ref_time = ICR3; + cur_time = TCNT3; + cur_tsop = TSOP_OPP_READ(); + + /* avoid interruption stacking */ + if (running) + return; + running = 1; + sei(); + +/* if (cur_tsop) */ +/* LED2_ON(); */ +/* else */ +/* LED2_OFF(); */ + + decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop); + + running = 0; +} + /* absolute value */ static inline int32_t AbS(int32_t x) { @@ -304,39 +324,192 @@ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff) } } - /* real time difference in 1/2 us */ + /* real time difference in timer unit (resolution 4us) */ diff = (best_cpt * 16384L) + (icr_diff & 0x3fff); - return 2000000000L/diff; + current_motor_period = diff; /* save it in global var */ + return 250000000L/diff; } /* process the received frame ring */ -void process_ring(struct frame_status *status) +static void process_sta_ring(struct frame_status *status) { - uint8_t head_next; - uint32_t frame; + uint8_t head, head_next; + uint16_t frame, frametick; + uint8_t found = 0; + uint8_t beacon_id; - /* after CS, check if we have a new frame in ring */ + /* beacon 0 */ + uint16_t data0, time0, ref_time0; + double angle0; + double dist0; + + /* beacon 1 */ + uint16_t data1, time1, ref_time1; + double angle1; + double dist1; + + point_t pos; + double a; + + /* remove too old captures from the ring */ while (status->head != status->tail) { head_next = (status->head+1) & FRAME_RING_MASK; - frame = status->ring[status->head].frame; + frametick = status->ring[status->head].tick; + if ((uint16_t)(tick - frametick) < MAX_CAP_AGE) + break; + status->head = head_next; + } + + head = status->head; + /* after CS, check if we have a new frame in ring */ + while (head != status->tail) { + head_next = (head+1) & FRAME_RING_MASK; + frame = status->ring[head].frame; + + /* ignore bad cksum */ + if (verify_cksum(frame) == 0xFFFF) + continue; + + beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK; + if (beacon_id != TSOP_STA_BEACON_ID0 && + beacon_id != TSOP_STA_BEACON_ID1) + continue; + + /* if motor speed is not good, skip values */ + if (current_motor_period < MOTOR_PERIOD_MIN) + continue; + if (current_motor_period > MOTOR_PERIOD_MAX) + continue; /* display if needed */ if (beacon_tsop.debug_frame) { - uint8_t beacon_id; - uint16_t data; + printf("STA ID=%d time=%d\r\n", + beacon_id, status->ring[head].time); + } + + if (beacon_id == TSOP_STA_BEACON_ID0) { + found |= 0x1; + data0 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK; + time0 = status->ring[head].time; + ref_time0 = status->ring[head].ref_time; + } + else if (beacon_id == TSOP_STA_BEACON_ID1) { + found |= 0x2; + data1 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK; + time1 = status->ring[head].time; + ref_time1 = status->ring[head].ref_time; + } + + head = head_next; + } + + /* if we didn't found beacon 0 and 1, return */ + if (found != 0x3) + return; + + /* update ring head */ + status->head = head; + + /* beacon 0 */ + dist0 = data0; + dist0 /= 512.; + dist0 *= (MAX_DIST-MIN_DIST); + dist0 += MIN_DIST; + + time0 = time0 - ref_time0; + angle0 = (double)time0 / (double)current_motor_period; + if (angle0 > 1.) + angle0 -= 1.; + if (angle0 > 1.) + return; /* fail */ + angle0 *= (2 * M_PI); + if (angle0 > M_PI) + angle0 -= M_PI; + + /* beacon 1 */ + dist1 = data1; + dist1 /= 512.; + dist1 *= (MAX_DIST-MIN_DIST); + dist1 += MIN_DIST; + + time1 = time1 - ref_time1; + angle1 = (double)time1 / (double)current_motor_period; + if (angle1 > 1.) + angle1 -= 1.; + if (angle1 > 1.) + return; /* fail */ + angle1 *= (2 * M_PI); + if (angle0 > M_PI) + angle0 -= M_PI; + + if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0, + angle1, dist1) < 0) + return; + + xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a); +} + +/* process the received frame ring */ +static void process_opp_ring(struct frame_status *status) +{ + uint8_t head_next; + uint16_t frame; + uint8_t found = 0; + uint8_t beacon_id; + uint16_t data, time, ref_time; + double angle; + double dist; - /* ignore bad cksum */ - if (verify_cksum(frame) == 0xFFFF) - continue; + /* after CS, check if we have a new frame in ring */ + while (status->head != status->tail) { + head_next = (status->head+1) & FRAME_RING_MASK; + frame = status->ring[status->head].frame; + + /* ignore bad cksum */ + if (verify_cksum(frame) == 0xFFFF) + continue; + + beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK; + if (beacon_id != TSOP_OPP_BEACON_ID) + continue; + + /* if motor speed is not good, skip values */ + if (current_motor_period < MOTOR_PERIOD_MIN) + continue; + if (current_motor_period > MOTOR_PERIOD_MAX) + continue; + + found = 1; + data = (frame >> TSOP_OPP_FRAME_DATA_SHIFT) & TSOP_OPP_FRAME_DATA_MASK; + time = status->ring[status->head].time; + ref_time = status->ring[status->head].ref_time; - beacon_id = (frame >> BEACON_ID_SHIFT) & BEACON_ID_MASK; - data = (frame >> FRAME_DATA_SHIFT) & FRAME_DATA_MASK; - printf("ID=%d data=%d time=%d\r\n", + /* display if needed */ + if (beacon_tsop.debug_frame) { + printf("OPP ID=%d data=%d time=%d\r\n", beacon_id, data, status->ring[status->head].time); } status->head = head_next; } + + if (found == 0) + return; + + dist = data; + dist /= 512.; + dist *= (MAX_DIST-MIN_DIST); + dist += MIN_DIST; + + time = time - ref_time; + angle = (double)time / (double)current_motor_period; + if (angle > 1.) + angle -= 1.; + if (angle > 1.) + return; /* fail */ + angle *= 3600; /* angle in 1/10 deg */ + + xmit_opp((uint16_t)dist, (uint16_t)angle); } int main(void) @@ -351,6 +524,14 @@ int main(void) uint16_t tcnt3; uint8_t x = 0; + opp_beacon.frame_len = TSOP_OPP_FRAME_LEN; + opp_beacon.time_long = TSOP_OPP_TIME_LONG; + opp_beacon.time_short = TSOP_OPP_TIME_SHORT; + + static_beacon.frame_len = TSOP_STA_FRAME_LEN; + static_beacon.time_long = TSOP_STA_TIME_LONG; + static_beacon.time_short = TSOP_STA_TIME_SHORT; + /* LEDS */ LED1_DDR |= _BV(LED1_BIT); LED2_DDR |= _BV(LED2_BIT); @@ -375,8 +556,8 @@ int main(void) debug_serial(); /* configure external interrupt for TSOP */ - EICRx_TSOP |= _BV(ISCx0_TSOP); - EIMSK |= _BV(INTx_TSOP); + EICRx_TSOP |= _BV(ISCx0_TSOP_STA) | _BV(ISCx0_TSOP_OPP); + EIMSK |= _BV(INTx_TSOP_STA) | _BV(INTx_TSOP_OPP); /* pwm for motor */ PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, @@ -390,10 +571,10 @@ int main(void) TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); OCR0 = 80; /* f = 100 khz at 16 Mhz */ - /* configure timer 3: CLK/8 + /* configure timer 3: CLK/64 * it is used as a reference time * enable noise canceller for ICP3 */ - TCCR3B = _BV(ICNC3) | _BV(CS11); + TCCR3B = _BV(ICNC3) | _BV(CS11) | _BV(CS10); sei(); @@ -468,7 +649,11 @@ int main(void) if (cpt < CPT_ICR_MAX) cpt ++; - process_ring(&static_beacon); + process_sta_ring(&static_beacon); + process_opp_ring(&opp_beacon); + cli(); + tick ++; /* global imprecise time reference */ + sei(); } return 0; diff --git a/projects/microb2010/tests/beacon_tsop/main.h b/projects/microb2010/tests/beacon_tsop/main.h index 0a85337..42709ee 100644 --- a/projects/microb2010/tests/beacon_tsop/main.h +++ b/projects/microb2010/tests/beacon_tsop/main.h @@ -20,52 +20,6 @@ * */ -#define BOARD2006 - -/********************** LEDs */ -#define LED_TOGGLE(port, bit) do { \ - if (port & _BV(bit)) \ - port &= ~_BV(bit); \ - else \ - port |= _BV(bit); \ - } while(0) - -#ifdef BOARD2006 -#define LED1_PORT PORTE -#define LED1_DDR DDRE -#define LED1_BIT 2 -#define LED2_PORT PORTE -#define LED2_DDR DDRE -#define LED2_BIT 3 -#define LED3_PORT PORTB -#define LED3_DDR DDRB -#define LED3_BIT 3 -#define LED1_ON() sbi(LED1_PORT, LED1_BIT) -#define LED1_OFF() cbi(LED1_PORT, LED1_BIT) -#define LED1_TOGGLE() LED1_TOGGLE(LED_PORT, LED1_BIT) -#define LED2_ON() sbi(LED2_PORT, LED2_BIT) -#define LED2_OFF() cbi(LED2_PORT, LED2_BIT) -#define LED2_TOGGLE() LED_TOGGLE(LED2_PORT, LED2_BIT) -#define LED3_ON() sbi(LED3_PORT, LED3_BIT) -#define LED3_OFF() cbi(LED3_PORT, LED3_BIT) -#define LED3_TOGGLE() LED_TOGGLE(LED3_PORT, LED3_BIT) -#else -#define LED_PORT PORTD -#define LED_DDR DDRD -#define LED1_BIT 5 -#define LED2_BIT 6 -#define LED3_BIT 7 -#define LED1_ON() sbi(LED_PORT, LED1_BIT) -#define LED1_OFF() cbi(LED_PORT, LED1_BIT) -#define LED1_TOGGLE() LED_TOGGLE(LED_PORT, LED1_BIT) -#define LED2_ON() sbi(LED_PORT, LED2_BIT) -#define LED2_OFF() cbi(LED_PORT, LED2_BIT) -#define LED2_TOGGLE() LED_TOGGLE(LED_PORT, LED2_BIT) -#define LED3_ON() sbi(LED_PORT, LED3_BIT) -#define LED3_OFF() cbi(LED_PORT, LED3_BIT) -#define LED3_TOGGLE() LED_TOGGLE(LED_PORT, LED3_BIT) -#endif - struct beacon_tsop { struct rdline rdl; char prompt[RDLINE_PROMPT_SIZE]; diff --git a/projects/microb2010/tests/beacon_tsop/trigo.c b/projects/microb2010/tests/beacon_tsop/trigo.c index 391043c..df45edf 100644 --- a/projects/microb2010/tests/beacon_tsop/trigo.c +++ b/projects/microb2010/tests/beacon_tsop/trigo.c @@ -1,5 +1,5 @@ /* - * Copyright Droids Corporation (2010) + * 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 @@ -15,7 +15,8 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Olivier MATZ + * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $ + * */ #include @@ -37,10 +38,23 @@ static int dprint = 0; #define dprintf(args...) if (dprint) printf(args) +#define AREA_X 3000 +#define AREA_Y 2100 + +#define ANGLE_EPSILON 0.005 + const point_t beacon0 = { 0, 1050 }; const point_t beacon1 = { 3000, 0 }; const point_t beacon2 = { 3000, 2100 }; +static inline double abs_dbl(double x) +{ + if (x > 0) + return x; + else + return -x; +} + /* Fill the 2 circles pointer given as parameter, each of those cross * both beacons b1 and b2. From any point of these circles (except b1 * and b2), we see b1 and b2 with the angle of a_rad (which must be @@ -79,8 +93,8 @@ int8_t angle_to_circles(circle_t *c1, circle_t *c2, vect_t v; float l, d; - /* reject negative or too small angles */ - if (a_rad <= 0.01) + /* reject too small angles */ + if (a_rad <= 0.01 && a_rad >= -0.01) return -1; /* get position of O */ @@ -159,22 +173,190 @@ int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20) return 0; } -/* get the angles of beacons from xy pos */ -int8_t posxy_to_angles(point_t pos, double *a01, double *a12, - double *a20, int err_num, float err_val) +static inline int8_t is_pt_in_area(point_t pt) { - double a0, a1, a2; + if (pt.x < 0 || pt.y < 0) + return 0; + if (pt.x > AREA_X || pt.y > AREA_Y) + return 0; + return 1; +} - a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x); - a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x); - a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x); +/* intersect the circle formed by one distance info with the circle + * crossing the 2 beacons */ +static int8_t +ad_to_posxy_one(point_t *pos, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd, const circle_t *c01, /* circles to intersect */ + double a01) /* seen angle of beacons */ +{ + double tmp_a01_p1, tmp_a01_p2; + point_t p1, p2; + uint8_t p1_ok=0, p2_ok=0; + + if (circle_intersect(c01, cd, &p1, &p2) == 0) + return -1; + + dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y); + dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y); + + dprintf("real a01: %2.2f\n", a01); + + tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) - + atan2(b0->y-p1.y, b0->x-p1.x); + if (tmp_a01_p1 > M_PI) + tmp_a01_p1 -= 2*M_PI; + if (tmp_a01_p1 < -M_PI) + tmp_a01_p1 += 2*M_PI; + dprintf("a01-1: %2.2f\n", tmp_a01_p1); + + tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) - + atan2(b0->y-p2.y, b0->x-p2.x); + if (tmp_a01_p2 > M_PI) + tmp_a01_p2 -= 2*M_PI; + if (tmp_a01_p2 < -M_PI) + tmp_a01_p2 += 2*M_PI; + dprintf("a01-2: %2.2f\n", tmp_a01_p2); + + /* in some conditions, we already know the position of the + * robot at this step */ + p1_ok = is_pt_in_area(p1) && + abs_dbl(tmp_a01_p1 - a01) < ANGLE_EPSILON; + + p2_ok = is_pt_in_area(p2) && + abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON; + if (!p1_ok && p2_ok) { + *pos = p2; + return 0; + } + if (p1_ok && !p2_ok) { + *pos = p1; + return 0; + } + return -1; +} + +static int8_t +ad_to_posxy_algo(point_t *pos, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd0, const circle_t *cd1, const circle_t *c01, /* circles to intersect */ + double a01, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) + +{ + switch (algo) { + + /* take the closer beacon first */ + case 0: + if (d0 < d1) { + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) + return 0; + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) + return 0; + return -1; + } + else { + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) + return 0; + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) + return 0; + return -1; + } + break; + case 1: + /* b0 only */ + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) + return 0; + break; + case 2: + /* b0 only */ + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) + return 0; + break; + default: + break; + } + /* not found */ + return -1; +} + +/* get the position and angle of the robot from the angle of the 2 + * beacons, and the distance of 2 beacons */ +int8_t +ad_to_posxya(point_t *pos, double *a, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + double a0, double a1, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) +{ + circle_t cd0, cd1, c01; + double a01; + + dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n", + algo, a0, a1, d0, d1); + + /* the first 2 circles depends on distance between robot and + * beacons */ + cd0.x = b0->x; + cd0.y = b0->y; + cd0.r = d0; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r); + cd1.x = b1->x; + cd1.y = b1->y; + cd1.r = d1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r); + + /* the third circle depends on angle between b0 and b1 */ + a01 = a1-a0; + if (a01 < -M_PI) + a01 += 2*M_PI; + if (a01 > M_PI) + a01 -= 2*M_PI; + + if (angle_to_circles(&c01, NULL, b0, b1, a01)) + return -1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r); + + /* get the xy position, depending on algo */ + if (ad_to_posxy_algo(pos, algo, b0, b1, &cd0, &cd1, &c01, a01, d0, d1) < 0) + return -1; + + /* now, we have the xy position, we can get angle thanks to + * the angles a0 and a1. Take the far beacon. */ + if (d0 < d1) + *a = atan2(beacon1.y - pos->y, beacon1.x - pos->x) - a1; + else + *a = atan2(beacon0.y - pos->y, beacon0.x - pos->x) - a0; + if (*a < -M_PI) + *a += 2*M_PI; + if (*a > M_PI) + *a -= 2*M_PI; + return 0; +} + +/* get the angles of beacons from xy pos */ +int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1, + double *a2, int err_num, float err_val) +{ + *a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x); + *a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x); + *a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x); if (err_num == 0 || err_num == 3) - a0 += (err_val * M_PI/180.); + *a0 += (err_val * M_PI/180.); if (err_num == 1 || err_num == 3) - a1 += (err_val * M_PI/180.); + *a1 += (err_val * M_PI/180.); if (err_num == 2 || err_num == 3) - a2 += (err_val * M_PI/180.); + *a2 += (err_val * M_PI/180.); + + return 0; +} + +/* get the angles of beacons from xy pos */ +int8_t posxy_to_angles(point_t pos, double *a01, double *a12, + double *a20, int err_num, float err_val) +{ + double a0, a1, a2; + posxy_to_abs_angles(pos, &a0, &a1, &a2, err_num, err_val); *a01 = a1-a0; if (*a01 < 0) @@ -245,3 +427,185 @@ int8_t process_move_error(double x, double y, double speed, *err = 50.; return 0; } + +#if 0 +/* whole process is around 3ms on atmega128 at 16Mhz */ +int main(int argc, char **argv) +{ + double a0, a1, a2; + double a01, a12, a20; + point_t pos, tmp; + const char *mode = "nothing"; + +#ifdef HOST_VERSION + if (argc < 2) { + printf("bad args\n"); + return -1; + } + mode = argv[1]; +#else + mode = "angle2pos"; + argc = 5; + a0 = -1.; + a1 = 1.65; + a2 = 3.03; +#endif + + /* + * angle2pos a0 a1 a2: + * (angles in radian, -pi < a < pi) + */ + if (argc == 5 && strcmp(mode, "angle2pos") == 0) { +#ifdef HOST_VERSION + dprint = 1; + a0 = atof(argv[2]); + a1 = atof(argv[3]); + a2 = atof(argv[4]); +#endif + + /* */ + a01 = a1-a0; + if (a01 < 0) + a01 += 2*M_PI; + a12 = a2-a1; + if (a12 < 0) + a12 += 2*M_PI; + a20 = a0-a2; + if (a20 < 0) + a20 += 2*M_PI; + + if (angles_to_posxy(&pos, a01, a12, a20) < 0) + return -1; + printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y); + return 0; + } + + /* + * ad2pos a0 a1 d0 d1: + * (angles in radian, -pi < a < pi) + * (distances in mm) + */ + if (argc == 6 && strcmp(mode, "ad2pos") == 0) { + double a, d0, d1; + + dprint = 1; + a0 = atof(argv[2]); + a1 = atof(argv[3]); + d0 = atof(argv[4]); + d1 = atof(argv[5]); + + /* */ + if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, + a0, a1, d0, d1) < 0) + return -1; + printf("p0: x=%2.2f y=%2.2f a=%2.2f\n", pos.x, pos.y, a); + return 0; + } + + if (argc == 4 && strcmp(mode, "simple_error") == 0) { + int x, y; + int err_num; + double err_val_deg; + double err; + + err_num = atof(argv[2]); /* which beacon sees an error */ + err_val_deg = atof(argv[3]); /* how many degrees of error */ + + for (x=0; x<300; x++) { + for (y=0; y<210; y++) { + + pos.x = x*10; + pos.y = y*10; + posxy_to_angles(pos, &a01, &a12, &a20, + err_num, err_val_deg); + if (angles_to_posxy(&tmp, a01, a12, a20)) + continue; + err = pt_norm(&tmp, &pos); + if (err > 50.) /* saturate error to 5cm */ + err = 50.; + printf("%d %d %2.2f\n", x, y, err); + } + } + return 0; + } + + /* da_error algo errpercent errdeg */ + if ((argc == 5 && strcmp(mode, "da_error") == 0) || + (argc == 5 && strcmp(mode, "da_error_mm") == 0)) { + int x, y, algo; + double err_val_deg; + double err_val_percent; + double err_val_mm; + double err, d0, d1, a; + + algo = atoi(argv[2]); + err_val_percent = atof(argv[3]); /* how many % of error for dist */ + err_val_mm = atof(argv[3]); /* how many mm of error for dist */ + err_val_deg = atof(argv[4]); /* how many degrees of error */ + + for (x=0; x<300; x++) { + for (y=0; y<210; y++) { + + pos.x = x*10; + pos.y = y*10; + posxy_to_abs_angles(pos, &a0, &a1, &a2, + 0, err_val_deg); + d0 = pt_norm(&pos, &beacon0); + d1 = pt_norm(&pos, &beacon1); + if (strcmp(mode, "da_error") == 0) { + d0 += d0 * err_val_percent / 100.; + d1 += d1 * err_val_percent / 100.; + } + else { + d0 += err_val_mm; + d1 += err_val_mm; + } + + if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1, + a0, a1, d0, d1) < 0) + continue; + + err = pt_norm(&tmp, &pos); + if (err > 50.) /* saturate error to 5cm */ + err = 50.; + printf("%d %d %2.2f\n", x, y, err); + } + } + return 0; + } + + if ((argc == 5 || argc == 7) + && strcmp(argv[1], "move_error") == 0) { + int x, y; + double angle, speed, period, err; + + speed = atof(argv[2]); /* speed in m/s ( = mm/ms) */ + period = atof(argv[3]); /* period of turret in ms */ + angle = atof(argv[4]); /* direction of moving */ + if (argc == 7) { + dprint = 1; + process_move_error(atof(argv[5]), atof(argv[6]), + speed, period, angle, &err); + printf("%2.2f %2.2f %2.2f\n", atof(argv[5]), + atof(argv[6]), err); + return 0; + } + + for (x=0; x<300; x++) { + for (y=0; y<210; y++) { + pos.x = x*10; + pos.y = y*10; + if (process_move_error(pos.x, pos.y, + speed, period, angle, + &err) < 0) + continue; + printf("%d %d %2.2f\n", x, y, err); + } + } + return 0; + } + + printf("bad args\n"); + return -1; +} +#endif diff --git a/projects/microb2010/tests/beacon_tsop/trigo.h b/projects/microb2010/tests/beacon_tsop/trigo.h index 1c3178f..8ac82f8 100644 --- a/projects/microb2010/tests/beacon_tsop/trigo.h +++ b/projects/microb2010/tests/beacon_tsop/trigo.h @@ -18,5 +18,16 @@ * Olivier MATZ */ +extern const point_t beacon0; +extern const point_t beacon1; +extern const point_t beacon2; + /* get the position of the robot from the angle of the 3 beacons */ int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20); + +/* get the position and angle of the robot from the angle of the 2 + * beacons, and the distance of 2 beacons */ +int8_t ad_to_posxya(point_t *pos, double *a, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + double a0, double a1, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ); diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c index d2accb3..caf0934 100755 --- a/projects/microb2010/tests/static_beacon/static_beacon.c +++ b/projects/microb2010/tests/static_beacon/static_beacon.c @@ -358,6 +358,23 @@ int main(void) while (1); #endif +#if 1 + /* test freq ~ 400khz */ + ICR1 = 38; + OCR1A = 19; + TCCR1B = _BV(WGM13) | _BV(WGM12); + TCNT1 = 37; + TCCR1A = _BV(COM1A1) | _BV(WGM11); + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10); + + /* motor PWM 50%, 8khz */ + DDRB |= 0x08; + OCR2 = 50; + TCCR2 = _BV(WGM21) | _BV(WGM20) | _BV(COM21) | _BV(CS21) ; + + while (1); +#endif + /* configure timer 0, prescaler = 64 */ TCCR0 = _BV(CS01) | _BV(CS00);