#ifndef TXC \r
#define TXC TXC0\r
#endif\r
+#ifndef RXC \r
+#define RXC RXC0\r
+#endif\r
#ifndef RXB8\r
#define RXB8 RXB80\r
#endif\r
--- /dev/null
+#
+# 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
--- /dev/null
+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) ;\
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: actuator.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+
+#include "main.h"
+
--- /dev/null
+/*
+ * 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 $
+ *
+ */
+
--- /dev/null
+#ifndef _AX12_CONFIG_H_
+#define _AX12_CONFIG_H_
+
+#define AX12_MAX_PARAMS 32
+
+
+#endif/*_AX12_CONFIG_H_*/
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: ax12_user.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/error.h>
+
+#include <i2c.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+
+/*
+ * Cmdline interface for AX12. Use the PC to command a daisy-chain of
+ * AX12 actuators with a nice command line interface.
+ *
+ * The circuit should be as following:
+ *
+ * |----------|
+ * | uart3|------->--- PC (baudrate=57600)
+ * | |-------<---
+ * | atmega128|
+ * | |
+ * | uart0|---->---+-- AX12 (baudrate 115200)
+ * | |----<---|
+ * |----------|
+ *
+ * Note that RX and TX pins of UART1 are connected together to provide
+ * a half-duplex UART emulation.
+ *
+ */
+
+#define UART_AX12_NUM 0
+#define UCSRxB UCSR0B
+#define AX12_TIMEOUT 5000UL /* in us */
+
+/********************************* AX12 commands */
+
+/*
+ * We use synchronous access (not interrupt driven) to the hardware
+ * UART, because we have to be sure that the transmission/reception is
+ * really finished when we return from the functions.
+ *
+ * We don't use the CM-5 circuit as described in the AX12
+ * documentation, we simply connect TX and RX and use TXEN + RXEN +
+ * DDR to manage the port directions.
+ */
+
+static volatile uint8_t ax12_state = AX12_STATE_READ;
+extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
+static volatile uint8_t ax12_nsent = 0;
+
+/* Called by ax12 module to send a character on serial line. Count the
+ * number of transmitted bytes. It will be used in ax12_recv_char() to
+ * drop the bytes that we transmitted. */
+static int8_t ax12_send_char(uint8_t c)
+{
+ uart_send(UART_AX12_NUM, c);
+ ax12_nsent++;
+ return 0;
+}
+
+/* for atmega256 */
+#ifndef TXEN
+#define TXEN TXEN0
+#endif
+
+/* called by uart module when the character has been written in
+ * UDR. It does not mean that the byte is physically transmitted. */
+static void ax12_send_callback(char c)
+{
+ if (ax12_state == AX12_STATE_READ) {
+ /* disable TX when last byte is pushed. */
+ if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+ UCSRxB &= ~(1<<TXEN);
+ }
+}
+
+/* Called by ax12 module when we want to receive a char. Note that we
+ * also receive the bytes we sent ! So we need to drop them. */
+static int16_t ax12_recv_char(void)
+{
+ microseconds t = time_get_us2();
+ int c;
+ while (1) {
+ c = uart_recv_nowait(UART_AX12_NUM);
+ if (c != -1) {
+ if (ax12_nsent == 0)
+ return c;
+ ax12_nsent --;
+ }
+
+ /* 5 ms timeout */
+ if ((time_get_us2() - t) > AX12_TIMEOUT)
+ return -1;
+ }
+ return c;
+}
+
+/* called by ax12 module when we want to switch serial line. As we
+ * work in interruption mode, this function can be called to switch
+ * back in read mode even if the bytes are not really transmitted on
+ * the line. That's why in this case we do nothing, we will fall back
+ * in read mode in any case when xmit is finished -- see in
+ * ax12_send_callback() -- */
+static void ax12_switch_uart(uint8_t state)
+{
+ uint8_t flags;
+
+ if (state == AX12_STATE_WRITE) {
+ IRQ_LOCK(flags);
+ ax12_nsent=0;
+ while (uart_recv_nowait(UART_AX12_NUM) != -1);
+ UCSRxB |= (1<<TXEN);
+ ax12_state = AX12_STATE_WRITE;
+ IRQ_UNLOCK(flags);
+ }
+ else {
+ IRQ_LOCK(flags);
+ if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+ UCSRxB &= ~(1<<TXEN);
+ ax12_state = AX12_STATE_READ;
+ IRQ_UNLOCK(flags);
+ }
+}
+
+
+void ax12_user_init(void)
+{
+ /* AX12 */
+ AX12_init(&gen.ax12);
+ AX12_set_hardware_send(&gen.ax12, ax12_send_char);
+ AX12_set_hardware_recv(&gen.ax12, ax12_recv_char);
+ AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart);
+ uart_register_tx_event(UART_AX12_NUM, ax12_send_callback);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: ax12_user.h,v 1.2 2009-04-07 20:03:48 zer0 Exp $
+ *
+ */
+
+/* This is the ax12 user interface. It initializes the aversive AX12
+ * module so that it works in background, using interrupt driver uart.
+ *
+ * Be carreful, a call to AX12 module is synchronous and uses
+ * interruptions, so interrupts must be enabled. On the other side, a
+ * call _must not_ interrupt another one. That's why all calls to the
+ * module are done either in init() functions or in a scheduler event
+ * with prio=ARM_PRIO.
+ */
+
+/* XXX do a safe_ax12() function that will retry once or twice if we
+ * see some problems. */
+
+void ax12_user_init(void);
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include "main.h"
+#include "cmdline.h"
+
+
+/******** See in commands.c for the list of commands. */
+extern parse_pgm_ctx_t main_ctx[];
+
+static void write_char(char c)
+{
+ uart_send(CMDLINE_UART, c);
+}
+
+static void
+valid_buffer(const char *buf, uint8_t size)
+{
+ int8_t ret;
+ ret = parse(main_ctx, buf);
+ if (ret == PARSE_AMBIGUOUS)
+ printf_P(PSTR("Ambiguous command\r\n"));
+ else if (ret == PARSE_NOMATCH)
+ printf_P(PSTR("Command not found\r\n"));
+ else if (ret == PARSE_BAD_ARGS)
+ printf_P(PSTR("Bad arguments\r\n"));
+}
+
+static int8_t
+complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
+ int16_t *state)
+{
+ return complete(main_ctx, buf, state, dstbuf, dstsize);
+}
+
+
+/* sending "pop" on cmdline uart resets the robot */
+void emergency(char c)
+{
+ static uint8_t i = 0;
+
+ if ((i == 0 && c == 'p') ||
+ (i == 1 && c == 'o') ||
+ (i == 2 && c == 'p'))
+ i++;
+ else if ( !(i == 1 && c == 'p') )
+ i = 0;
+ if (i == 3)
+ reset();
+}
+
+/* log function, add a command to configure
+ * it dynamically */
+void mylog(struct error * e, ...)
+{
+ va_list ap;
+ u16 stream_flags = stdout->flags;
+ uint8_t i;
+ time_h tv;
+
+ if (e->severity > ERROR_SEVERITY_ERROR) {
+ if (gen.log_level < e->severity)
+ return;
+
+ for (i=0; i<NB_LOGS+1; i++)
+ if (gen.logs[i] == e->err_num)
+ break;
+ if (i == NB_LOGS+1)
+ return;
+ }
+
+ va_start(ap, e);
+ tv = time_get_time();
+ printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
+ vfprintf_P(stdout, e->text, ap);
+ printf_P(PSTR("\r\n"));
+ va_end(ap);
+ stdout->flags = stream_flags;
+}
+
+int cmdline_interact(void)
+{
+ const char *history, *buffer;
+ int8_t ret, same = 0;
+ int16_t c;
+
+ rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
+ snprintf(gen.prompt, sizeof(gen.prompt), "ballboard > ");
+ rdline_newline(&gen.rdl, gen.prompt);
+
+ while (1) {
+ c = uart_recv_nowait(CMDLINE_UART);
+ if (c == -1)
+ continue;
+ ret = rdline_char_in(&gen.rdl, c);
+ if (ret != 2 && ret != 0) {
+ buffer = rdline_get_buffer(&gen.rdl);
+ history = rdline_get_history_item(&gen.rdl, 0);
+ if (history) {
+ same = !memcmp(buffer, history, strlen(history)) &&
+ buffer[strlen(history)] == '\n';
+ }
+ else
+ same = 0;
+ if (strlen(buffer) > 1 && !same)
+ rdline_add_history(&gen.rdl, buffer);
+ rdline_newline(&gen.rdl, gen.prompt);
+ }
+ }
+
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#define CMDLINE_UART 1
+
+/* uart rx callback for reset() */
+void emergency(char c);
+
+/* log function */
+void mylog(struct error * e, ...);
+
+/* launch cmdline */
+int cmdline_interact(void);
+
+static inline uint8_t cmdline_keypressed(void) {
+ return (uart_recv_nowait(CMDLINE_UART) != -1);
+}
+
+static inline uint8_t cmdline_getchar(void) {
+ return uart_recv_nowait(CMDLINE_UART);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands.c,v 1.2 2009-05-27 20:04:07 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdlib.h>
+#include <aversive/pgmspace.h>
+#include <parse.h>
+
+/* commands_gen.c */
+extern parse_pgm_inst_t cmd_reset;
+extern parse_pgm_inst_t cmd_bootloader;
+extern parse_pgm_inst_t cmd_encoders;
+extern parse_pgm_inst_t cmd_pwm;
+extern parse_pgm_inst_t cmd_adc;
+extern parse_pgm_inst_t cmd_sensor;
+extern parse_pgm_inst_t cmd_log;
+extern parse_pgm_inst_t cmd_log_show;
+extern parse_pgm_inst_t cmd_log_type;
+extern parse_pgm_inst_t cmd_stack_space;
+extern parse_pgm_inst_t cmd_scheduler;
+
+/* commands_ax12.c */
+extern parse_pgm_inst_t cmd_baudrate;
+extern parse_pgm_inst_t cmd_uint16_read;
+extern parse_pgm_inst_t cmd_uint16_write;
+extern parse_pgm_inst_t cmd_uint8_read;
+extern parse_pgm_inst_t cmd_uint8_write;
+
+/* commands_cs.c */
+extern parse_pgm_inst_t cmd_gain;
+extern parse_pgm_inst_t cmd_gain_show;
+extern parse_pgm_inst_t cmd_speed;
+extern parse_pgm_inst_t cmd_speed_show;
+extern parse_pgm_inst_t cmd_derivate_filter;
+extern parse_pgm_inst_t cmd_derivate_filter_show;
+extern parse_pgm_inst_t cmd_consign;
+extern parse_pgm_inst_t cmd_maximum;
+extern parse_pgm_inst_t cmd_maximum_show;
+extern parse_pgm_inst_t cmd_quadramp;
+extern parse_pgm_inst_t cmd_quadramp_show;
+extern parse_pgm_inst_t cmd_cs_status;
+extern parse_pgm_inst_t cmd_blocking_i;
+extern parse_pgm_inst_t cmd_blocking_i_show;
+
+/* commands_ballboard.c */
+extern parse_pgm_inst_t cmd_event;
+extern parse_pgm_inst_t cmd_test;
+
+/* in progmem */
+parse_pgm_ctx_t main_ctx[] = {
+
+ /* commands_gen.c */
+ (parse_pgm_inst_t *)&cmd_reset,
+ (parse_pgm_inst_t *)&cmd_bootloader,
+ (parse_pgm_inst_t *)&cmd_encoders,
+ (parse_pgm_inst_t *)&cmd_pwm,
+ (parse_pgm_inst_t *)&cmd_adc,
+ (parse_pgm_inst_t *)&cmd_sensor,
+ (parse_pgm_inst_t *)&cmd_log,
+ (parse_pgm_inst_t *)&cmd_log_show,
+ (parse_pgm_inst_t *)&cmd_log_type,
+ (parse_pgm_inst_t *)&cmd_stack_space,
+ (parse_pgm_inst_t *)&cmd_scheduler,
+
+ /* commands_ax12.c */
+ (parse_pgm_inst_t *)&cmd_baudrate,
+ (parse_pgm_inst_t *)&cmd_uint16_read,
+ (parse_pgm_inst_t *)&cmd_uint16_write,
+ (parse_pgm_inst_t *)&cmd_uint8_read,
+ (parse_pgm_inst_t *)&cmd_uint8_write,
+
+ /* commands_cs.c */
+ (parse_pgm_inst_t *)&cmd_gain,
+ (parse_pgm_inst_t *)&cmd_gain_show,
+ (parse_pgm_inst_t *)&cmd_speed,
+ (parse_pgm_inst_t *)&cmd_speed_show,
+ (parse_pgm_inst_t *)&cmd_consign,
+ (parse_pgm_inst_t *)&cmd_derivate_filter,
+ (parse_pgm_inst_t *)&cmd_derivate_filter_show,
+ (parse_pgm_inst_t *)&cmd_maximum,
+ (parse_pgm_inst_t *)&cmd_maximum_show,
+ (parse_pgm_inst_t *)&cmd_quadramp,
+ (parse_pgm_inst_t *)&cmd_quadramp_show,
+ (parse_pgm_inst_t *)&cmd_cs_status,
+ (parse_pgm_inst_t *)&cmd_blocking_i,
+ (parse_pgm_inst_t *)&cmd_blocking_i_show,
+
+ /* commands_ballboard.c */
+ (parse_pgm_inst_t *)&cmd_event,
+ (parse_pgm_inst_t *)&cmd_test,
+
+ NULL,
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_ax12.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+
+uint8_t addr_from_string(const char *s)
+{
+ /* 16 bits */
+ if (!strcmp_P(s, PSTR("cw_angle_limit")))
+ return AA_CW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("ccw_angle_limit")))
+ return AA_CCW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("max_torque")))
+ return AA_MAX_TORQUE_L;
+ if (!strcmp_P(s, PSTR("down_calibration")))
+ return AA_DOWN_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("up_calibration")))
+ return AA_UP_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("torque_limit")))
+ return AA_TORQUE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("position")))
+ return AA_PRESENT_POSITION_L;
+ if (!strcmp_P(s, PSTR("speed")))
+ return AA_PRESENT_SPEED_L;
+ if (!strcmp_P(s, PSTR("load")))
+ return AA_PRESENT_LOAD_L;
+ if (!strcmp_P(s, PSTR("moving_speed")))
+ return AA_MOVING_SPEED_L;
+ if (!strcmp_P(s, PSTR("model")))
+ return AA_MODEL_NUMBER_L;
+ if (!strcmp_P(s, PSTR("goal_pos")))
+ return AA_GOAL_POSITION_L;
+ if (!strcmp_P(s, PSTR("punch")))
+ return AA_PUNCH_L;
+
+ /* 8 bits */
+ if (!strcmp_P(s, PSTR("firmware")))
+ return AA_FIRMWARE;
+ if (!strcmp_P(s, PSTR("id")))
+ return AA_ID;
+ if (!strcmp_P(s, PSTR("baudrate")))
+ return AA_BAUD_RATE;
+ if (!strcmp_P(s, PSTR("delay")))
+ return AA_DELAY_TIME;
+ if (!strcmp_P(s, PSTR("high_lim_temp")))
+ return AA_HIGHEST_LIMIT_TEMP;
+ if (!strcmp_P(s, PSTR("low_lim_volt")))
+ return AA_LOWEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("high_lim_volt")))
+ return AA_HIGHEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("status_return")))
+ return AA_STATUS_RETURN_LEVEL;
+ if (!strcmp_P(s, PSTR("alarm_led")))
+ return AA_ALARM_LED;
+ if (!strcmp_P(s, PSTR("alarm_shutdown")))
+ return AA_ALARM_SHUTDOWN;
+ if (!strcmp_P(s, PSTR("torque_enable")))
+ return AA_TORQUE_ENABLE;
+ if (!strcmp_P(s, PSTR("led")))
+ return AA_LED;
+ if (!strcmp_P(s, PSTR("cw_comp_margin")))
+ return AA_CW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("ccw_comp_margin")))
+ return AA_CCW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("cw_comp_slope")))
+ return AA_CW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("ccw_comp_slope")))
+ return AA_CCW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("voltage")))
+ return AA_PRESENT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("temp")))
+ return AA_PRESENT_TEMP;
+ if (!strcmp_P(s, PSTR("reginst")))
+ return AA_PRESENT_REGINST;
+ if (!strcmp_P(s, PSTR("moving")))
+ return AA_MOVING;
+ if (!strcmp_P(s, PSTR("lock")))
+ return AA_LOCK;
+
+ return 0;
+}
+
+/**********************************************************/
+/* Ax12_Stress */
+
+/* this structure is filled when cmd_ax12_stress is parsed successfully */
+struct cmd_ax12_stress_result {
+ fixed_string_t arg0;
+ uint8_t id;
+};
+
+/* function called when cmd_ax12_stress is parsed successfully */
+static void cmd_ax12_stress_parsed(void *parsed_result, void *data)
+{
+ struct cmd_ax12_stress_result *res = parsed_result;
+ int i, nb_errs = 0;
+ uint8_t val;
+ microseconds t = time_get_us2();
+
+ for (i=0; i<1000; i++) {
+ if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
+ nb_errs ++;
+ }
+
+ printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
+ t = (time_get_us2() - t) / 1000;
+ printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
+}
+
+prog_char str_ax12_stress_arg0[] = "ax12_stress";
+parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
+parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
+
+prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands";
+parse_pgm_inst_t cmd_ax12_stress = {
+ .f = cmd_ax12_stress_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ax12_stress,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ax12_stress_arg0,
+ (prog_void *)&cmd_ax12_stress_id,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_baudrate is parsed successfully */
+struct cmd_baudrate_result {
+ fixed_string_t arg0;
+ uint32_t arg1;
+};
+
+/* function called when cmd_baudrate is parsed successfully */
+static void cmd_baudrate_parsed(void * parsed_result, void * data)
+{
+ struct cmd_baudrate_result *res = parsed_result;
+ struct uart_config c;
+
+ printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+ uart_getconf(1, &c);
+ c.baudrate = res->arg1;
+ uart_setconf(1, &c);
+ printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+}
+
+prog_char str_baudrate_arg0[] = "baudrate";
+parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
+parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
+
+prog_char help_baudrate[] = "Change ax12 baudrate";
+parse_pgm_inst_t cmd_baudrate = {
+ .f = cmd_baudrate_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_baudrate,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_baudrate_arg0,
+ (prog_void *)&cmd_baudrate_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint16 */
+
+
+/* this structure is filled when cmd_uint16_read is parsed successfully */
+struct cmd_uint16_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint16_t val;
+};
+
+/* function called when cmd_uint16_read is parsed successfully */
+static void cmd_uint16_read_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint16_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+ ret = AX12_read_int(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint16_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
+prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#down_calibration#up_calibration#torque_limit#"
+ "position#speed#load#punch";
+parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
+parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
+
+prog_char help_uint16_read[] = "Read uint16 value (type, num)";
+parse_pgm_inst_t cmd_uint16_read = {
+ .f = cmd_uint16_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0,
+ (prog_void *)&cmd_uint16_arg1,
+ (prog_void *)&cmd_uint16_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint16_write is parsed successfully */
+static void cmd_uint16_write_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t addr = addr_from_string(res->arg1);
+ printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = AX12_write_int(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint16_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
+prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#torque_limit#punch";
+parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
+parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
+
+prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
+parse_pgm_inst_t cmd_uint16_write = {
+ .f = cmd_uint16_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0_w,
+ (prog_void *)&cmd_uint16_arg1_w,
+ (prog_void *)&cmd_uint16_num,
+ (prog_void *)&cmd_uint16_val,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint8 */
+
+
+/* this structure is filled when cmd_uint8_read is parsed successfully */
+struct cmd_uint8_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint8_t val;
+};
+
+/* function called when cmd_uint8_read is parsed successfully */
+static void cmd_uint8_read_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+
+ ret = AX12_read_byte(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint8_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
+prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "voltage#temp#reginst#moving#lock";
+parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
+parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
+
+prog_char help_uint8_read[] = "Read uint8 value (type, num)";
+parse_pgm_inst_t cmd_uint8_read = {
+ .f = cmd_uint8_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0,
+ (prog_void *)&cmd_uint8_arg1,
+ (prog_void *)&cmd_uint8_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint8_write is parsed successfully */
+static void cmd_uint8_write_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t addr = addr_from_string(res->arg1);
+ uint8_t ret;
+ printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint8_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
+prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "reginst#lock";
+parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
+parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
+
+prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
+parse_pgm_inst_t cmd_uint8_write = {
+ .f = cmd_uint8_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0_w,
+ (prog_void *)&cmd_uint8_arg1_w,
+ (prog_void *)&cmd_uint8_num,
+ (prog_void *)&cmd_uint8_val,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_ballboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
+#include "actuator.h"
+
+struct cmd_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+
+/* function called when cmd_event is parsed successfully */
+static void cmd_event_parsed(void *parsed_result, void *data)
+{
+ u08 bit=0;
+
+ struct cmd_event_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("all"))) {
+ bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ ballboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ ballboard.flags &= bit;
+ else { /* show */
+ printf_P(PSTR("encoders is %s\r\n"),
+ (DO_ENCODERS & ballboard.flags) ? "on":"off");
+ printf_P(PSTR("cs is %s\r\n"),
+ (DO_CS & ballboard.flags) ? "on":"off");
+ printf_P(PSTR("bd is %s\r\n"),
+ (DO_BD & ballboard.flags) ? "on":"off");
+ printf_P(PSTR("power is %s\r\n"),
+ (DO_POWER & ballboard.flags) ? "on":"off");
+ }
+ return;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("encoders")))
+ bit = DO_ENCODERS;
+ else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ bit = DO_CS;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("bd")))
+ bit = DO_BD;
+ else if (!strcmp_P(res->arg1, PSTR("power")))
+ bit = DO_POWER;
+
+
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ ballboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off"))) {
+ if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ pwm_ng_set(ROLLER_PWM, 0);
+ pwm_ng_set(FORKTRANS_PWM, 0);
+ pwm_ng_set(FORKROT_PWM, 0);
+ }
+ ballboard.flags &= (~bit);
+ }
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ (bit & ballboard.flags) ? "on":"off");
+}
+
+prog_char str_event_arg0[] = "event";
+parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
+prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
+parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
+prog_char str_event_arg2[] = "on#off#show";
+parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
+
+prog_char help_event[] = "Enable/disable events";
+parse_pgm_inst_t cmd_event = {
+ .f = cmd_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_event_arg0,
+ (prog_void *)&cmd_event_arg1,
+ (prog_void *)&cmd_event_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Color */
+
+/* this structure is filled when cmd_color is parsed successfully */
+struct cmd_color_result {
+ fixed_string_t arg0;
+ fixed_string_t color;
+};
+
+/* function called when cmd_color is parsed successfully */
+static void cmd_color_parsed(void *parsed_result, void *data)
+{
+ struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
+ if (!strcmp_P(res->color, PSTR("red"))) {
+ ballboard.our_color = I2C_COLOR_RED;
+ }
+ else if (!strcmp_P(res->color, PSTR("green"))) {
+ ballboard.our_color = I2C_COLOR_GREEN;
+ }
+ printf_P(PSTR("Done\r\n"));
+}
+
+prog_char str_color_arg0[] = "color";
+parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
+prog_char str_color_color[] = "green#red";
+parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
+
+prog_char help_color[] = "Set our color";
+parse_pgm_inst_t cmd_color = {
+ .f = cmd_color_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_color,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_color_arg0,
+ (prog_void *)&cmd_color_color,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(void *parsed_result, void *data)
+{
+ //struct cmd_test_result *res = parsed_result;
+
+}
+
+prog_char str_test_arg0[] = "test";
+parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
+
+prog_char help_test[] = "Test function";
+parse_pgm_inst_t cmd_test = {
+ .f = cmd_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_test_arg0,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_cs.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cs.h"
+#include "cmdline.h"
+
+struct csb_list {
+ const prog_char *name;
+ struct cs_block *csb;
+};
+
+prog_char csb_roller_str[] = "roller";
+prog_char csb_forktrans_str[] = "forktrans";
+prog_char csb_forkrot_str[] = "forkrot";
+struct csb_list csb_list[] = {
+ { .name = csb_roller_str, .csb = &ballboard.roller },
+ { .name = csb_forktrans_str, .csb = &ballboard.forktrans },
+ { .name = csb_forkrot_str, .csb = &ballboard.forkrot },
+};
+
+struct cmd_cs_result {
+ fixed_string_t cmdname;
+ fixed_string_t csname;
+};
+
+/* token to be used for all cs-related commands */
+prog_char str_csb_name[] = "roller#forktrans#forkrot";
+parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
+
+struct cs_block *cs_from_name(const char *name)
+{
+ int i;
+
+ for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
+ if (!strcmp_P(name, csb_list[i].name))
+ return csb_list[i].csb;
+ }
+ return NULL;
+}
+
+/**********************************************************/
+/* Gains for control system */
+
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_result {
+ struct cmd_cs_result cs;
+ int16_t p;
+ int16_t i;
+ int16_t d;
+};
+
+/* function called when cmd_gain is parsed successfully */
+static void cmd_gain_parsed(void * parsed_result, void *show)
+{
+ struct cmd_gain_result *res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_gains(&csb->pid, res->p, res->i, res->d);
+
+ printf_P(PSTR("%s %s %d %d %d\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ pid_get_gain_P(&csb->pid),
+ pid_get_gain_I(&csb->pid),
+ pid_get_gain_D(&csb->pid));
+}
+
+prog_char str_gain_arg0[] = "gain";
+parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
+parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
+parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
+parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
+
+prog_char help_gain[] = "Set gain values for PID";
+parse_pgm_inst_t cmd_gain = {
+ .f = cmd_gain_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_gain,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_p,
+ (prog_void *)&cmd_gain_i,
+ (prog_void *)&cmd_gain_d,
+ NULL,
+ },
+};
+
+/* show */
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_gain_show_arg[] = "show";
+parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
+
+prog_char help_gain_show[] = "Show gain values for PID";
+parse_pgm_inst_t cmd_gain_show = {
+ .f = cmd_gain_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_gain_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Speeds for control system */
+
+/* this structure is filled when cmd_speed is parsed successfully */
+struct cmd_speed_result {
+ struct cmd_cs_result cs;
+ uint16_t s;
+};
+
+/* function called when cmd_speed is parsed successfully */
+static void cmd_speed_parsed(void *parsed_result, void *show)
+{
+ struct cmd_speed_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+#if notyet
+ if (!show)
+ ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
+
+ printf_P(PSTR("%s %lu\r\n"),
+ res->cs.csname,
+ ext.r_b.var_pos);
+#else
+ printf_P(PSTR("TODO\r\n"));
+#endif
+}
+
+prog_char str_speed_arg0[] = "speed";
+parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
+parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
+
+prog_char help_speed[] = "Set speed values for ramp filter";
+parse_pgm_inst_t cmd_speed = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_speed,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_s,
+ NULL,
+ },
+};
+
+/* show */
+struct cmd_speed_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
+
+prog_char help_speed_show[] = "Show speed values for ramp filter";
+parse_pgm_inst_t cmd_speed_show = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_speed_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Derivate_Filters for control system */
+
+/* this structure is filled when cmd_derivate_filter is parsed successfully */
+struct cmd_derivate_filter_result {
+ struct cmd_cs_result cs;
+ uint8_t size;
+};
+
+/* function called when cmd_derivate_filter is parsed successfully */
+static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
+{
+ struct cmd_derivate_filter_result * res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_derivate_filter(&csb->pid, res->size);
+
+ printf_P(PSTR("%s %s %u\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ pid_get_derivate_filter(&csb->pid));
+}
+
+prog_char str_derivate_filter_arg0[] = "derivate_filter";
+parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
+parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
+
+prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
+parse_pgm_inst_t cmd_derivate_filter = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_derivate_filter,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_size,
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_derivate_filter_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_derivate_filter_show_arg[] = "show";
+parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
+
+prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
+parse_pgm_inst_t cmd_derivate_filter_show = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_derivate_filter_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_show_arg,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Consign for control system */
+
+/* this structure is filled when cmd_consign is parsed successfully */
+struct cmd_consign_result {
+ struct cmd_cs_result cs;
+ int32_t p;
+};
+
+/* function called when cmd_consign is parsed successfully */
+static void cmd_consign_parsed(void * parsed_result, void *data)
+{
+ struct cmd_consign_result * res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ cs_set_consign(&csb->cs, res->p);
+}
+
+prog_char str_consign_arg0[] = "consign";
+parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
+parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
+
+prog_char help_consign[] = "Set consign value";
+parse_pgm_inst_t cmd_consign = {
+ .f = cmd_consign_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_consign,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_consign_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_consign_p,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Maximums for control system */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_result {
+ struct cmd_cs_result cs;
+ uint32_t in;
+ uint32_t i;
+ uint32_t out;
+};
+
+/* function called when cmd_maximum is parsed successfully */
+static void cmd_maximum_parsed(void *parsed_result, void *show)
+{
+ struct cmd_maximum_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_maximums(&csb->pid, res->in, res->i, res->out);
+
+ printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
+ res->cs.csname,
+ pid_get_max_in(&csb->pid),
+ pid_get_max_I(&csb->pid),
+ pid_get_max_out(&csb->pid));
+}
+
+prog_char str_maximum_arg0[] = "maximum";
+parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
+parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
+parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
+parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
+
+prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
+parse_pgm_inst_t cmd_maximum = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_maximum,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_in,
+ (prog_void *)&cmd_maximum_i,
+ (prog_void *)&cmd_maximum_out,
+ NULL,
+ },
+};
+
+/* show */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+prog_char str_maximum_show_arg[] = "show";
+parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
+
+prog_char help_maximum_show[] = "Show maximum values for PID";
+parse_pgm_inst_t cmd_maximum_show = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_maximum_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Quadramp for control system */
+
+/* this structure is filled when cmd_quadramp is parsed successfully */
+struct cmd_quadramp_result {
+ struct cmd_cs_result cs;
+ uint32_t ap;
+ uint32_t an;
+ uint32_t sp;
+ uint32_t sn;
+};
+
+/* function called when cmd_quadramp is parsed successfully */
+static void cmd_quadramp_parsed(void *parsed_result, void *show)
+{
+ struct cmd_quadramp_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show) {
+ quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
+ quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
+ }
+
+ printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
+ res->cs.csname,
+ csb->qr.var_2nd_ord_pos,
+ csb->qr.var_2nd_ord_neg,
+ csb->qr.var_1st_ord_pos,
+ csb->qr.var_1st_ord_neg);
+}
+
+prog_char str_quadramp_arg0[] = "quadramp";
+parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+
+prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
+parse_pgm_inst_t cmd_quadramp = {
+ .f = cmd_quadramp_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_quadramp,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_ap,
+ (prog_void *)&cmd_quadramp_an,
+ (prog_void *)&cmd_quadramp_sp,
+ (prog_void *)&cmd_quadramp_sn,
+
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_quadramp_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_quadramp_show_arg[] = "show";
+parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
+
+prog_char help_quadramp_show[] = "Get quadramp values for control system";
+parse_pgm_inst_t cmd_quadramp_show = {
+ .f = cmd_quadramp_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_quadramp_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_show_arg,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* cs_status show for control system */
+
+/* this structure is filled when cmd_cs_status is parsed successfully */
+struct cmd_cs_status_result {
+ struct cmd_cs_result cs;
+ fixed_string_t arg;
+};
+
+/* function called when cmd_cs_status is parsed successfully */
+static void cmd_cs_status_parsed(void *parsed_result, void *data)
+{
+ struct cmd_cs_status_result *res = parsed_result;
+ struct cs_block *csb;
+ uint8_t loop = 0;
+ uint8_t print_pid = 0, print_cs = 0;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+ if (strcmp_P(res->arg, PSTR("on")) == 0) {
+ csb->on = 1;
+ printf_P(PSTR("%s is on\r\n"), res->cs.csname);
+ return;
+ }
+ else if (strcmp_P(res->arg, PSTR("off")) == 0) {
+ csb->on = 0;
+ printf_P(PSTR("%s is off\r\n"), res->cs.csname);
+ return;
+ }
+ else if (strcmp_P(res->arg, PSTR("show")) == 0) {
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
+ loop = 1;
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
+ print_pid = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
+ print_pid = 1;
+ loop = 1;
+ }
+
+ printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
+ do {
+ if (print_cs)
+ dump_cs(res->cs.csname, &csb->cs);
+ if (print_pid)
+ dump_pid(res->cs.csname, &csb->pid);
+ wait_ms(100);
+ } while(loop && !cmdline_keypressed());
+}
+
+prog_char str_cs_status_arg0[] = "cs_status";
+parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
+prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
+parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
+
+prog_char help_cs_status[] = "Show cs status";
+parse_pgm_inst_t cmd_cs_status = {
+ .f = cmd_cs_status_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_cs_status,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_cs_status_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_cs_status_arg,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Blocking_I for control system */
+
+/* this structure is filled when cmd_blocking_i is parsed successfully */
+struct cmd_blocking_i_result {
+ struct cmd_cs_result cs;
+ int32_t k1;
+ int32_t k2;
+ uint32_t i;
+ uint16_t cpt;
+};
+
+/* function called when cmd_blocking_i is parsed successfully */
+static void cmd_blocking_i_parsed(void *parsed_result, void *show)
+{
+ struct cmd_blocking_i_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
+ res->i, res->cpt);
+
+ printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ csb->bd.k1,
+ csb->bd.k2,
+ csb->bd.i_thres,
+ csb->bd.cpt_thres);
+}
+
+prog_char str_blocking_i_arg0[] = "blocking";
+parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
+parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
+parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
+parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
+parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
+
+prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
+parse_pgm_inst_t cmd_blocking_i = {
+ .f = cmd_blocking_i_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_blocking_i,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_k1,
+ (prog_void *)&cmd_blocking_i_k2,
+ (prog_void *)&cmd_blocking_i_i,
+ (prog_void *)&cmd_blocking_i_cpt,
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_blocking_i_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_blocking_i_show_arg[] = "show";
+parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg);
+
+prog_char help_blocking_i_show[] = "Show blocking detection values";
+parse_pgm_inst_t cmd_blocking_i_show = {
+ .f = cmd_blocking_i_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_blocking_i_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_show_arg,
+ NULL,
+ },
+};
+
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_gen.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <encoders_spi.h>
+#include <adc.h>
+
+#include <scheduler.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include <diagnostic.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "sensor.h"
+
+/**********************************************************/
+/* Reset */
+
+/* this structure is filled when cmd_reset is parsed successfully */
+struct cmd_reset_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_reset is parsed successfully */
+static void cmd_reset_parsed(void * parsed_result, void * data)
+{
+ reset();
+}
+
+prog_char str_reset_arg0[] = "reset";
+parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
+
+prog_char help_reset[] = "Reset the board";
+parse_pgm_inst_t cmd_reset = {
+ .f = cmd_reset_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_reset,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_reset_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Bootloader */
+
+/* this structure is filled when cmd_bootloader is parsed successfully */
+struct cmd_bootloader_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_bootloader is parsed successfully */
+static void cmd_bootloader_parsed(void *parsed_result, void *data)
+{
+ bootloader();
+}
+
+prog_char str_bootloader_arg0[] = "bootloader";
+parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
+
+prog_char help_bootloader[] = "Launch the bootloader";
+parse_pgm_inst_t cmd_bootloader = {
+ .f = cmd_bootloader_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_bootloader,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_bootloader_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Encoders tests */
+
+/* this structure is filled when cmd_encoders is parsed successfully */
+struct cmd_encoders_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_encoders is parsed successfully */
+static void cmd_encoders_parsed(void *parsed_result, void *data)
+{
+ struct cmd_encoders_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ encoders_spi_set_value((void *)0, 0);
+ encoders_spi_set_value((void *)1, 0);
+ encoders_spi_set_value((void *)2, 0);
+ encoders_spi_set_value((void *)3, 0);
+ return;
+ }
+
+ /* show */
+ while(!cmdline_keypressed()) {
+ printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
+ encoders_spi_get_value((void *)0),
+ encoders_spi_get_value((void *)1),
+ encoders_spi_get_value((void *)2),
+ encoders_spi_get_value((void *)3));
+ wait_ms(100);
+ }
+}
+
+prog_char str_encoders_arg0[] = "encoders";
+parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
+prog_char str_encoders_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
+
+prog_char help_encoders[] = "Show encoders values";
+parse_pgm_inst_t cmd_encoders = {
+ .f = cmd_encoders_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_encoders,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_encoders_arg0,
+ (prog_void *)&cmd_encoders_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Scheduler show */
+
+/* this structure is filled when cmd_scheduler is parsed successfully */
+struct cmd_scheduler_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_scheduler is parsed successfully */
+static void cmd_scheduler_parsed(void *parsed_result, void *data)
+{
+ scheduler_dump_events();
+}
+
+prog_char str_scheduler_arg0[] = "scheduler";
+parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
+prog_char str_scheduler_arg1[] = "show";
+parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
+
+prog_char help_scheduler[] = "Show scheduler events";
+parse_pgm_inst_t cmd_scheduler = {
+ .f = cmd_scheduler_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_scheduler,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_scheduler_arg0,
+ (prog_void *)&cmd_scheduler_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Pwms tests */
+
+/* this structure is filled when cmd_pwm is parsed successfully */
+struct cmd_pwm_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t arg2;
+};
+
+/* function called when cmd_pwm is parsed successfully */
+static void cmd_pwm_parsed(void * parsed_result, void * data)
+{
+ void * pwm_ptr = NULL;
+ struct cmd_pwm_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("1(4A)")))
+ pwm_ptr = &gen.pwm1_4A;
+ else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
+ pwm_ptr = &gen.pwm2_4B;
+ else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
+ pwm_ptr = &gen.pwm3_1A;
+ else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
+ pwm_ptr = &gen.pwm4_1B;
+
+ else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
+ pwm_ptr = &gen.servo1;
+ else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
+ pwm_ptr = &gen.servo2;
+ else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
+ pwm_ptr = &gen.servo3;
+ else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
+ pwm_ptr = &gen.servo4;
+
+ if (pwm_ptr)
+ pwm_ng_set(pwm_ptr, res->arg2);
+
+ printf_P(PSTR("done\r\n"));
+}
+
+prog_char str_pwm_arg0[] = "pwm";
+parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
+prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)";
+parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
+parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
+
+prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
+parse_pgm_inst_t cmd_pwm = {
+ .f = cmd_pwm_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pwm,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pwm_arg0,
+ (prog_void *)&cmd_pwm_arg1,
+ (prog_void *)&cmd_pwm_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Adcs tests */
+
+/* this structure is filled when cmd_adc is parsed successfully */
+struct cmd_adc_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_adc is parsed successfully */
+static void cmd_adc_parsed(void *parsed_result, void *data)
+{
+ struct cmd_adc_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("ADC values: "));
+ for (i=0; i<ADC_MAX; i++) {
+ printf_P(PSTR("%.4d "), sensor_get_adc(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_adc_arg0[] = "adc";
+parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
+prog_char str_adc_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
+
+prog_char help_adc[] = "Show adc values";
+parse_pgm_inst_t cmd_adc = {
+ .f = cmd_adc_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_adc,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_adc_arg0,
+ (prog_void *)&cmd_adc_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Sensors tests */
+
+/* this structure is filled when cmd_sensor is parsed successfully */
+struct cmd_sensor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_sensor is parsed successfully */
+static void cmd_sensor_parsed(void *parsed_result, void *data)
+{
+ struct cmd_sensor_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("SENSOR values: "));
+ for (i=0; i<SENSOR_MAX; i++) {
+ printf_P(PSTR("%d "), !!sensor_get(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_sensor_arg0[] = "sensor";
+parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
+prog_char str_sensor_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
+
+prog_char help_sensor[] = "Show sensor values";
+parse_pgm_inst_t cmd_sensor = {
+ .f = cmd_sensor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sensor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_sensor_arg0,
+ (prog_void *)&cmd_sensor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+ fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const prog_char uart_log[] = "uart";
+static const prog_char i2c_log[] = "i2c";
+static const prog_char i2cproto_log[] = "i2cproto";
+static const prog_char sensor_log[] = "sensor";
+static const prog_char block_log[] = "bd";
+static const prog_char beacon_log[] = "beacon";
+static const prog_char scanner_log[] = "scanner";
+static const prog_char imgprocess_log[] = "imgprocess";
+
+struct log_name_and_num {
+ const prog_char * name;
+ uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+ { uart_log, E_UART },
+ { i2c_log, E_I2C },
+ { i2cproto_log, E_USER_I2C_PROTO },
+ { sensor_log, E_USER_SENSOR },
+ { block_log, E_BLOCKING_DETECTION_MANAGER },
+ { beacon_log, E_USER_BEACON },
+ { scanner_log, E_USER_SCANNER },
+ { imgprocess_log, E_USER_IMGPROCESS },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (!strcmp_P(s, log_name_and_num[i].name)) {
+ return log_name_and_num[i].num;
+ }
+ }
+ return 0;
+}
+
+const prog_char *
+log_num2name(uint8_t num)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (num == log_name_and_num[i].num) {
+ return log_name_and_num[i].name;
+ }
+ }
+ return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+ uint8_t i, empty=1;
+ const prog_char * name;
+
+ printf_P(PSTR("log level is %d\r\n"), gen.log_level);
+ for (i=0; i<NB_LOGS; i++) {
+ name = log_num2name(gen.logs[i]);
+ if (name) {
+ printf_P(PSTR("log type %S is on\r\n"), name);
+ empty = 0;
+ }
+ }
+ if (empty)
+ printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, void * data)
+{
+ struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("level"))) {
+ gen.log_level = res->arg2;
+ }
+
+ /* else it is a show */
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg0[] = "log";
+parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
+prog_char str_log_arg1[] = "level";
+parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
+parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
+
+prog_char help_log[] = "Set log options: level (0 -> 5)";
+parse_pgm_inst_t cmd_log = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1,
+ (prog_void *)&cmd_log_arg2,
+ NULL,
+ },
+};
+
+prog_char str_log_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
+
+prog_char help_log_show[] = "Show configured logs";
+parse_pgm_inst_t cmd_log_show = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_show,
+ NULL,
+ },
+};
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_type_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_type_parsed(void * parsed_result, void * data)
+{
+ struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
+ uint8_t lognum;
+ uint8_t i;
+
+ lognum = log_name2num(res->arg2);
+ if (lognum == 0) {
+ printf_P(PSTR("Cannot find log num\r\n"));
+ return;
+ }
+
+ if (!strcmp_P(res->arg3, PSTR("on"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ printf_P(PSTR("Already on\r\n"));
+ return;
+ }
+ }
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == 0) {
+ gen.logs[i] = lognum;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("no more room\r\n"));
+ }
+ }
+ else if (!strcmp_P(res->arg3, PSTR("off"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ gen.logs[i] = 0;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("already off\r\n"));
+ }
+ }
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg1_type[] = "type";
+parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#beacon#scanner#imgprocess";
+parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+prog_char str_log_arg3[] = "on#off";
+parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+prog_char help_log_type[] = "Set log type";
+parse_pgm_inst_t cmd_log_type = {
+ .f = cmd_log_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_type,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_type,
+ (prog_void *)&cmd_log_arg2_type,
+ (prog_void *)&cmd_log_arg3,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(void *parsed_result, void *data)
+{
+ printf("res stack: %d\r\n", min_stack_space_available());
+
+}
+
+prog_char str_stack_space_arg0[] = "stack_space";
+parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+prog_char help_stack_space[] = "Display remaining stack space";
+parse_pgm_inst_t cmd_stack_space = {
+ .f = cmd_stack_space_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_stack_space,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_stack_space_arg0,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cs.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "actuator.h"
+
+int32_t encoders_spi_update_roller_speed(void *number)
+{
+ static volatile int32_t roller_pos;
+ int32_t tmp, speed;
+ tmp = encoders_spi_get_value(number);
+ speed = tmp - roller_pos;
+ roller_pos = tmp;
+ return speed;
+}
+
+/* called every 5 ms */
+static void do_cs(void *dummy)
+{
+ /* read encoders */
+ if (ballboard.flags & DO_ENCODERS) {
+ encoders_spi_manage(NULL);
+ }
+ /* control system */
+ if (ballboard.flags & DO_CS) {
+ if (ballboard.roller.on)
+ cs_manage(&ballboard.roller.cs);
+ if (ballboard.forktrans.on)
+ cs_manage(&ballboard.forktrans.cs);
+ if (ballboard.forkrot.on)
+ cs_manage(&ballboard.forkrot.cs);
+ }
+ if (ballboard.flags & DO_BD) {
+ bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs);
+ bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
+ bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs);
+ }
+ if (ballboard.flags & DO_POWER)
+ BRAKE_OFF();
+ else
+ BRAKE_ON();
+}
+
+void dump_cs(const char *name, struct cs *cs)
+{
+ printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
+ "in=% .5ld out=% .5ld\r\n"),
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_pid(const char *name, struct pid_filter *pid)
+{
+ printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ name,
+ pid_get_value_in(pid) * pid_get_gain_P(pid),
+ pid_get_value_I(pid) * pid_get_gain_I(pid),
+ pid_get_value_D(pid) * pid_get_gain_D(pid),
+ pid_get_value_out(pid));
+}
+
+void microb_cs_init(void)
+{
+ /* ---- CS roller */
+ /* PID */
+ pid_init(&ballboard.roller.pid);
+ pid_set_gains(&ballboard.roller.pid, 80, 80, 250);
+ pid_set_maximums(&ballboard.roller.pid, 0, 10000, 2000);
+ pid_set_out_shift(&ballboard.roller.pid, 6);
+ pid_set_derivate_filter(&ballboard.roller.pid, 6);
+
+ /* CS */
+ cs_init(&ballboard.roller.cs);
+ cs_set_correct_filter(&ballboard.roller.cs, pid_do_filter, &ballboard.roller.pid);
+ cs_set_process_in(&ballboard.roller.cs, pwm_ng_set, ROLLER_PWM);
+ cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER);
+ cs_set_consign(&ballboard.roller.cs, 0);
+
+ /* ---- CS forktrans */
+ /* PID */
+ pid_init(&ballboard.forktrans.pid);
+ pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250);
+ pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047);
+ pid_set_out_shift(&ballboard.forktrans.pid, 6);
+ pid_set_derivate_filter(&ballboard.forktrans.pid, 6);
+
+ /* QUADRAMP */
+ quadramp_init(&ballboard.forktrans.qr);
+ quadramp_set_1st_order_vars(&ballboard.forktrans.qr, 200, 200); /* set speed */
+ quadramp_set_2nd_order_vars(&ballboard.forktrans.qr, 20, 20); /* set accel */
+
+ /* CS */
+ cs_init(&ballboard.forktrans.cs);
+ cs_set_consign_filter(&ballboard.forktrans.cs, quadramp_do_filter, &ballboard.forktrans.qr);
+ cs_set_correct_filter(&ballboard.forktrans.cs, pid_do_filter, &ballboard.forktrans.pid);
+ cs_set_process_in(&ballboard.forktrans.cs, pwm_ng_set, FORKTRANS_PWM);
+ cs_set_process_out(&ballboard.forktrans.cs, encoders_spi_get_value, FORKTRANS_ENCODER);
+ cs_set_consign(&ballboard.forktrans.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&ballboard.forktrans.bd);
+ bd_set_speed_threshold(&ballboard.forktrans.bd, 150);
+ bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40);
+
+ /* ---- CS forkrot */
+ /* PID */
+ pid_init(&ballboard.forkrot.pid);
+ pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250);
+ pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047);
+ pid_set_out_shift(&ballboard.forkrot.pid, 6);
+ pid_set_derivate_filter(&ballboard.forkrot.pid, 6);
+
+ /* QUADRAMP */
+ quadramp_init(&ballboard.forkrot.qr);
+ quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */
+ quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */
+
+ /* CS */
+ cs_init(&ballboard.forkrot.cs);
+ cs_set_consign_filter(&ballboard.forkrot.cs, quadramp_do_filter, &ballboard.forkrot.qr);
+ cs_set_correct_filter(&ballboard.forkrot.cs, pid_do_filter, &ballboard.forkrot.pid);
+ cs_set_process_in(&ballboard.forkrot.cs, pwm_ng_set, FORKROT_PWM);
+ cs_set_process_out(&ballboard.forkrot.cs, encoders_spi_get_value, FORKROT_ENCODER);
+ cs_set_consign(&ballboard.forkrot.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&ballboard.forkrot.bd);
+ bd_set_speed_threshold(&ballboard.forkrot.bd, 150);
+ bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40);
+
+ /* set them on !! */
+ ballboard.roller.on = 0;
+ ballboard.forktrans.on = 1;
+ ballboard.forkrot.on = 1;
+
+
+ scheduler_add_periodical_event_priority(do_cs, NULL,
+ 5000L / SCHEDULER_UNIT,
+ CS_PRIO);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cs.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
--- /dev/null
+/*
+ * 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_
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * Copyright Droids Corporation (2007)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_protocol.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/error.h>
+
+#include <scheduler.h>
+
+#include <i2c.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "actuator.h"
+
+void i2c_protocol_init(void)
+{
+}
+
+/*** LED CONTROL ***/
+void i2c_led_control(uint8_t l, uint8_t state)
+{
+ switch(l) {
+ case 1:
+ state? LED1_ON():LED1_OFF();
+ break;
+ case 2:
+ state? LED2_ON():LED2_OFF();
+ break;
+ default:
+ break;
+ }
+}
+
+void i2c_send_status(void)
+{
+ struct i2c_ans_ballboard_status ans;
+ i2c_flush();
+ ans.hdr.cmd = I2C_ANS_BALLBOARD_STATUS;
+ ans.status = 0x55; /* XXX */
+
+ i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
+ sizeof(ans), I2C_CTRL_GENERIC);
+}
+
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+ void *void_cmd = buf;
+ static uint8_t a=0;
+ a=!a;
+ if (a)
+ LED2_ON();
+ else
+ LED2_OFF();
+
+ if (size <= 0) {
+ goto error;
+ }
+
+ switch (buf[0]) {
+
+ /* Commands (no answer needed) */
+ case I2C_CMD_GENERIC_LED_CONTROL:
+ {
+ struct i2c_cmd_led_control *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+ i2c_led_control(cmd->led_num, cmd->state);
+ break;
+ }
+
+ case I2C_CMD_GENERIC_SET_COLOR:
+ {
+ struct i2c_cmd_generic_color *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+ ballboard.our_color = cmd->color;
+ break;
+ }
+
+
+ /* Add other commands here ...*/
+
+
+ case I2C_REQ_BALLBOARD_STATUS:
+ {
+ struct i2c_req_ballboard_status *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+
+ i2c_send_status();
+ break;
+ }
+
+ default:
+ goto error;
+ }
+
+ error:
+ /* log error on a led ? */
+ return;
+}
+
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
+{
+}
+
+void i2c_sendevent(int8_t size)
+{
+}
+
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2007)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_protocol.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+
+void i2c_protocol_init(void);
+
+void i2c_recvevent(uint8_t * buf, int8_t size);
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
+void i2c_sendevent(int8_t size);
+
+int debug_send(char c, FILE* f);
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: main.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <avr/eeprom.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <i2c.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "../common/eeprom_mapping.h"
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+#include "ax12_user.h"
+#include "cmdline.h"
+#include "sensor.h"
+#include "actuator.h"
+#include "cs.h"
+#include "i2c_protocol.h"
+#include "beacon.h"
+#include "scanner.h"
+
+/* 0 means "programmed"
+ * ---- with 16 Mhz quartz
+ * CKSEL 3-0 : 0111
+ * SUT 1-0 : 10
+ * CKDIV8 : 1
+ * ---- bootloader
+ * BOOTZ 1-0 : 01 (4K bootloader)
+ * BOOTRST : 0 (reset on bootloader)
+ * ---- jtag
+ * jtagen : 0
+ */
+
+struct genboard gen;
+struct ballboard ballboard;
+
+/***********************/
+
+void bootloader(void)
+{
+#define BOOTLOADER_ADDR 0x3f000
+ if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
+ printf_P(PSTR("Bootloader is not present\r\n"));
+ return;
+ }
+ cli();
+ BRAKE_ON();
+ /* ... very specific :( */
+ TIMSK0 = 0;
+ TIMSK1 = 0;
+ TIMSK2 = 0;
+ TIMSK3 = 0;
+ TIMSK4 = 0;
+ TIMSK5 = 0;
+ EIMSK = 0;
+ UCSR0B = 0;
+ UCSR1B = 0;
+ UCSR2B = 0;
+ UCSR3B = 0;
+ SPCR = 0;
+ TWCR = 0;
+ ACSR = 0;
+ ADCSRA = 0;
+
+ EIND = 1;
+ __asm__ __volatile__ ("ldi r31,0xf8\n");
+ __asm__ __volatile__ ("ldi r30,0x00\n");
+ __asm__ __volatile__ ("eijmp\n");
+
+ /* never returns */
+}
+
+void do_led_blink(__attribute__((unused)) void *dummy)
+{
+#if 1 /* simple blink */
+ static uint8_t a=0;
+
+ if(a)
+ LED1_ON();
+ else
+ LED1_OFF();
+
+ a = !a;
+#endif
+}
+
+static void main_timer_interrupt(void)
+{
+ static uint8_t cpt = 0;
+ cpt++;
+ sei();
+ if ((cpt & 0x3) == 0)
+ scheduler_interrupt();
+}
+
+int main(void)
+{
+ /* brake */
+ BRAKE_OFF();
+ BRAKE_DDR();
+
+ /* CPLD reset on PG3 */
+ DDRG |= 1<<3;
+ PORTG &= ~(1<<3); /* implicit */
+
+ /* LEDS */
+ DDRJ |= 0x0c;
+ DDRL = 0xc0;
+ LED1_OFF();
+ memset(&gen, 0, sizeof(gen));
+ memset(&ballboard, 0, sizeof(ballboard));
+ ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD
+
+ /* UART */
+ uart_init();
+#if CMDLINE_UART == 3
+ fdevopen(uart3_dev_send, uart3_dev_recv);
+ uart_register_rx_event(3, emergency);
+#elif CMDLINE_UART == 1
+ fdevopen(uart1_dev_send, uart1_dev_recv);
+ uart_register_rx_event(1, emergency);
+#else
+# error not supported
+#endif
+
+ //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD);
+ /* check eeprom to avoid to run the bad program */
+ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
+ EEPROM_MAGIC_BALLBOARD) {
+ sei();
+ printf_P(PSTR("Bad eeprom value\r\n"));
+ while(1);
+ }
+
+ /* LOGS */
+ error_register_emerg(mylog);
+ error_register_error(mylog);
+ error_register_warning(mylog);
+ error_register_notice(mylog);
+ error_register_debug(mylog);
+
+ /* SPI + ENCODERS */
+ encoders_spi_init(); /* this will also init spi hardware */
+
+ /* I2C */
+ i2c_protocol_init();
+ i2c_init(I2C_MODE_SLAVE, I2C_BALLBOARD_ADDR);
+ i2c_register_recv_event(i2c_recvevent);
+
+ /* TIMER */
+ timer_init();
+ timer0_register_OV_intr(main_timer_interrupt);
+
+ /* PWM */
+ PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
+ TIMER1_PRESCALER_DIV_1);
+ PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10,
+ TIMER4_PRESCALER_DIV_1);
+
+ PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
+ PWM_NG_MODE_SIGN_INVERTED,
+ &PORTD, 4);
+ PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
+ &PORTD, 5);
+ PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
+ &PORTD, 6);
+ PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
+ &PORTD, 7);
+
+
+ /* servos */
+ PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
+ TIMER1_PRESCALER_DIV_256);
+ PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+ PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10,
+ TIMER1_PRESCALER_DIV_256);
+ PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+ PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+ PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+
+ /* SCHEDULER */
+ scheduler_init();
+
+ scheduler_add_periodical_event_priority(do_led_blink, NULL,
+ 100000L / SCHEDULER_UNIT,
+ LED_PRIO);
+ /* all cs management */
+ microb_cs_init();
+
+ /* sensors, will also init hardware adc */
+ sensor_init();
+
+ /* TIME */
+ time_init(TIME_PRIO);
+
+ /* ax12 */
+ ax12_user_init();
+
+ sei();
+
+ printf_P(PSTR("\r\n"));
+ printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
+ cmdline_interact();
+
+ return 0;
+}
--- /dev/null
+/*
+ * 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; \
+})
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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_
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ * Olivier MATZ <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: sensor.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+#include <stdlib.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <adc.h>
+#include <scheduler.h>
+#include <ax12.h>
+#include <pwm_ng.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "sensor.h"
+
+/************ ADC */
+
+struct adc_infos {
+ uint16_t config;
+ int16_t value;
+ int16_t prev_val;
+ int16_t (*filter)(struct adc_infos *, int16_t);
+};
+
+/* reach 90% of the value in 4 samples */
+int16_t rii_light(struct adc_infos *adc, int16_t val)
+{
+ adc->prev_val = val + (int32_t)adc->prev_val / 2;
+ return adc->prev_val / 2;
+}
+
+/* reach 90% of the value in 8 samples */
+int16_t rii_medium(struct adc_infos *adc, int16_t val)
+{
+ adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4;
+ return adc->prev_val / 4;
+}
+
+/* reach 90% of the value in 16 samples */
+int16_t rii_strong(struct adc_infos *adc, int16_t val)
+{
+ adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8;
+ return adc->prev_val / 8;
+}
+
+
+#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
+
+/* define which ADC to poll, see in sensor.h */
+static struct adc_infos adc_infos[ADC_MAX] = {
+
+ [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
+ [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
+ [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
+ [ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium },
+
+ /* add adc on "cap" pins if needed */
+/* [ADC_CAP1] = { .config = ADC_CONF(10) }, */
+/* [ADC_CAP2] = { .config = ADC_CONF(11) }, */
+/* [ADC_CAP3] = { .config = ADC_CONF(12) }, */
+/* [ADC_CAP4] = { .config = ADC_CONF(13) }, */
+};
+
+static void adc_event(int16_t result);
+
+/* called every 10 ms, see init below */
+static void do_adc(void *dummy)
+{
+ /* launch first conversion */
+ adc_launch(adc_infos[0].config);
+}
+
+static void adc_event(int16_t result)
+{
+ static uint8_t i = 0;
+
+ /* filter value if needed */
+ if (adc_infos[i].filter)
+ adc_infos[i].value = adc_infos[i].filter(&adc_infos[i],
+ result);
+ else
+ adc_infos[i].value = result;
+
+ i ++;
+ if (i >= ADC_MAX)
+ i = 0;
+ else
+ adc_launch(adc_infos[i].config);
+}
+
+int16_t sensor_get_adc(uint8_t i)
+{
+ int16_t tmp;
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ tmp = adc_infos[i].value;
+ IRQ_UNLOCK(flags);
+ return tmp;
+}
+
+/************ boolean sensors */
+
+
+struct sensor_filter {
+ uint8_t filter;
+ uint8_t prev;
+ uint8_t thres_off;
+ uint8_t thres_on;
+ uint8_t cpt;
+ uint8_t invert;
+};
+
+/* pullup mapping:
+ * CAP 5,6,7,8
+ * voltage div mapping:
+ * CAP 1
+ */
+static struct sensor_filter sensor_filter[SENSOR_MAX] = {
+ [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */
+ [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
+ [S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */
+ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
+ [S_CAP5] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
+ [S_CAP6] = { 1, 0, 0, 1, 0, 0 }, /* 5 */
+ [S_CAP7] = { 1, 0, 0, 1, 0, 0 }, /* 6 */
+ [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
+ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
+ [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
+ [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */
+ [S_RESERVED4] = { 1, 0, 0, 1, 0, 0 }, /* 11 */
+ [S_RESERVED5] = { 1, 0, 0, 1, 0, 0 }, /* 12 */
+ [S_RESERVED6] = { 1, 0, 0, 1, 0, 0 }, /* 13 */
+ [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */
+ [S_RESERVED8] = { 1, 0, 0, 1, 0, 0 }, /* 15 */
+};
+
+/* value of filtered sensors */
+static uint16_t sensor_filtered = 0;
+
+/* sensor mapping :
+ * 0-3: PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
+ * 4-5: PORTL 0->1 (cap5 -> cap6)
+ * 6-7: PORTE 3->4 (cap7 -> cap8)
+ * 8-15: reserved
+ */
+
+uint16_t sensor_get_all(void)
+{
+ uint16_t tmp;
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ tmp = sensor_filtered;
+ IRQ_UNLOCK(flags);
+ return tmp;
+}
+
+uint8_t sensor_get(uint8_t i)
+{
+ uint16_t tmp = sensor_get_all();
+ return (tmp & _BV(i));
+}
+
+/* get the physical value of pins */
+static uint16_t sensor_read(void)
+{
+ uint16_t tmp = 0;
+ tmp |= (uint16_t)((PINK & (_BV(2)|_BV(3)|_BV(4)|_BV(5))) >> 2) << 0;
+ tmp |= (uint16_t)((PINL & (_BV(0)|_BV(1))) >> 0) << 4;
+ tmp |= (uint16_t)((PINE & (_BV(3)|_BV(4))) >> 3) << 6;
+ /* add reserved sensors here */
+ return tmp;
+}
+
+/* called every 10 ms, see init below */
+static void do_boolean_sensors(void *dummy)
+{
+ uint8_t i;
+ uint8_t flags;
+ uint16_t sensor = sensor_read();
+ uint16_t tmp = 0;
+
+ for (i=0; i<SENSOR_MAX; i++) {
+ if ((1 << i) & sensor) {
+ if (sensor_filter[i].cpt < sensor_filter[i].filter)
+ sensor_filter[i].cpt++;
+ if (sensor_filter[i].cpt >= sensor_filter[i].thres_on)
+ sensor_filter[i].prev = 1;
+ }
+ else {
+ if (sensor_filter[i].cpt > 0)
+ sensor_filter[i].cpt--;
+ if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
+ sensor_filter[i].prev = 0;
+ }
+
+ if (sensor_filter[i].prev) {
+ tmp |= (1UL << i);
+ }
+ }
+ IRQ_LOCK(flags);
+ sensor_filtered = tmp;
+ IRQ_UNLOCK(flags);
+}
+
+
+
+/************ global sensor init */
+#define BACKGROUND_ADC 0
+
+/* called every 10 ms, see init below */
+static void do_sensors(void *dummy)
+{
+ if (BACKGROUND_ADC)
+ do_adc(NULL);
+ do_boolean_sensors(NULL);
+}
+
+void sensor_init(void)
+{
+ adc_init();
+ if (BACKGROUND_ADC)
+ adc_register_event(adc_event);
+ /* CS EVENT */
+ scheduler_add_periodical_event_priority(do_sensors, NULL,
+ 10000L / SCHEDULER_UNIT,
+ ADC_PRIO);
+
+}
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ * Olivier MATZ <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: sensor.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
+ *
+ */
+
+/* synchronize with sensor.c */
+#define ADC_CSENSE1 0
+#define ADC_CSENSE2 1
+#define ADC_CSENSE3 2
+#define ADC_CSENSE4 3
+#define ADC_MAX 4
+
+/* synchronize with sensor.c */
+#define S_CAP1 0
+#define S_CAP2 1
+#define S_CAP3 2
+#define S_CAP4 3
+#define S_CAP5 4
+#define S_CAP6 5
+#define S_CAP7 6
+#define S_CAP8 7
+#define S_RESERVED1 8
+#define S_RESERVED2 9
+#define S_RESERVED3 10
+#define S_RESERVED4 11
+#define S_RESERVED5 12
+#define S_RESERVED6 13
+#define S_RESERVED7 14
+#define S_RESERVED8 15
+#define SENSOR_MAX 16
+
+void sensor_init(void);
+
+/* get filtered values for adc */
+int16_t sensor_get_adc(uint8_t i);
+
+/* get filtered values of boolean sensors */
+uint16_t sensor_get_all(void);
+uint8_t sensor_get(uint8_t i);
--- /dev/null
+/*
+ * 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
+
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/* \r
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
+ * \r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation; either version 2 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ *\r
+ * Revision : $Id: uart_config.h,v 1.3 2009-05-27 20:04:07 zer0 Exp $\r
+ *\r
+ */\r
+\r
+/* Droids-corp 2004 - Zer0\r
+ * config for uart module\r
+ */\r
+\r
+#ifndef UART_CONFIG_H\r
+#define UART_CONFIG_H\r
+\r
+/*\r
+ * UART1 definitions \r
+ */\r
+\r
+/* compile uart1 fonctions, undefine it to pass compilation */\r
+#define UART1_COMPILE \r
+\r
+/* enable uart1 if == 1, disable if == 0 */\r
+#define UART1_ENABLED 1\r
+\r
+/* enable uart1 interrupts if == 1, disable if == 0 */\r
+#define UART1_INTERRUPT_ENABLED 1\r
+\r
+#define UART1_BAUDRATE 57600\r
+\r
+/* \r
+ * if you enable this, the maximum baudrate you can reach is \r
+ * higher, but the precision is lower. \r
+ */\r
+#define UART1_USE_DOUBLE_SPEED 1\r
+\r
+#define UART1_RX_FIFO_SIZE 64\r
+#define UART1_TX_FIFO_SIZE 64\r
+#define UART1_NBITS 8\r
+\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+\r
+\r
+/*\r
+ * UART3 definitions \r
+ */\r
+\r
+/* compile uart3 fonctions, undefine it to pass compilation */\r
+#define UART3_COMPILE \r
+\r
+/* enable uart3 if == 1, disable if == 0 */\r
+#define UART3_ENABLED 1\r
+\r
+/* enable uart3 interrupts if == 1, disable if == 0 */\r
+#define UART3_INTERRUPT_ENABLED 1\r
+\r
+#define UART3_BAUDRATE 57600\r
+\r
+/* \r
+ * if you enable this, the maximum baudrate you can reach is \r
+ * higher, but the precision is lower. \r
+ */\r
+#define UART3_USE_DOUBLE_SPEED 1\r
+//#define UART3_USE_DOUBLE_SPEED 1\r
+\r
+#define UART3_RX_FIFO_SIZE 64\r
+#define UART3_TX_FIFO_SIZE 64\r
+//#define UART3_NBITS 5\r
+//#define UART3_NBITS 6\r
+//#define UART3_NBITS 7\r
+#define UART3_NBITS 8\r
+//#define UART3_NBITS 9\r
+\r
+#define UART3_PARITY UART_PARTITY_NONE\r
+//#define UART3_PARITY UART_PARTITY_ODD\r
+//#define UART3_PARITY UART_PARTITY_EVEN\r
+\r
+#define UART3_STOP_BIT UART_STOP_BITS_1\r
+//#define UART3_STOP_BIT UART_STOP_BITS_2\r
+\r
+\r
+\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+#endif\r
+\r
--- /dev/null
+#
+# 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
--- /dev/null
+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) ;\
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+
+#include "../common/i2c_commands.h"
+#include "actuator.h"
+#include "ax12_user.h"
+#include "main.h"
+
+void servo_carry_open(void)
+{
+}
+
+void servo_carry_close(void)
+{
+}
+
+void servo_door_open(void)
+{
+}
+
+void servo_door_close(void)
+{
+}
+
+void actuator_init(void)
+{
+
+}
--- /dev/null
+/*
+ * 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);
+
--- /dev/null
+#ifndef _AX12_CONFIG_H_
+#define _AX12_CONFIG_H_
+
+#define AX12_MAX_PARAMS 32
+
+
+#endif/*_AX12_CONFIG_H_*/
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: ax12_user.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <i2c.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "ax12_user.h"
+
+#define AX12_ERROR(args...) ERROR(E_USER_AX12, args)
+#define AX12_NOTICE(args...) NOTICE(E_USER_AX12, args)
+#define AX12_MAX_TRIES 3
+
+/*
+ * Cmdline interface for AX12. Use the PC to command a daisy-chain of
+ * AX12 actuators with a nice command line interface.
+ *
+ * The circuit should be as following:
+ *
+ * |----------|
+ * | uart3|------->--- PC (baudrate=57600)
+ * | |-------<---
+ * | atmega128|
+ * | |
+ * | uart0|---->---+-- AX12 (baudrate 115200)
+ * | |----<---|
+ * |----------|
+ *
+ * Note that RX and TX pins of UART1 are connected together to provide
+ * a half-duplex UART emulation.
+ *
+ */
+
+#define UCSRxB UCSR0B
+#define AX12_TIMEOUT 15000L /* in us */
+
+static uint32_t ax12_stats_ops = 0; /* total ops */
+static uint32_t ax12_stats_fails = 0; /* number of fails */
+static uint32_t ax12_stats_drops = 0; /* number of drops (3 fails) */
+static uint32_t ax12_dropped_logs = 0; /* error messages that were not displayed */
+static microseconds t_prev_msg = 0;
+
+/********************************* AX12 commands */
+
+/*
+ * We use synchronous access (not interrupt driven) to the hardware
+ * UART, because we have to be sure that the transmission/reception is
+ * really finished when we return from the functions.
+ *
+ * We don't use the CM-5 circuit as described in the AX12
+ * documentation, we simply connect TX and RX and use TXEN + RXEN +
+ * DDR to manage the port directions.
+ */
+
+static volatile uint8_t ax12_state = AX12_STATE_READ;
+extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
+static volatile uint8_t ax12_nsent = 0;
+
+/* Called by ax12 module to send a character on serial line. Count the
+ * number of transmitted bytes. It will be used in ax12_recv_char() to
+ * drop the bytes that we transmitted. */
+static int8_t ax12_send_char(uint8_t c)
+{
+ uart_send(UART_AX12_NUM, c);
+ ax12_nsent++;
+ return 0;
+}
+
+/* for atmega256 */
+#ifndef TXEN
+#define TXEN TXEN0
+#endif
+
+/* called by uart module when the character has been written in
+ * UDR. It does not mean that the byte is physically transmitted. */
+static void ax12_send_callback(__attribute__((unused)) char c)
+{
+ if (ax12_state == AX12_STATE_READ) {
+ /* disable TX when last byte is pushed. */
+ if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+ UCSRxB &= ~(1<<TXEN);
+ }
+}
+
+/* Called by ax12 module when we want to receive a char. Note that we
+ * also receive the bytes we sent ! So we need to drop them. */
+static int16_t ax12_recv_char(void)
+{
+ microseconds t = time_get_us2();
+ int c;
+ while (1) {
+ c = uart_recv_nowait(UART_AX12_NUM);
+ if (c != -1) {
+ if (ax12_nsent == 0)
+ return c;
+ ax12_nsent --;
+ }
+
+ /* 5 ms timeout */
+ if ((time_get_us2() - t) > AX12_TIMEOUT)
+ return -1;
+ }
+ return c;
+}
+
+/* called by ax12 module when we want to switch serial line. As we
+ * work in interruption mode, this function can be called to switch
+ * back in read mode even if the bytes are not really transmitted on
+ * the line. That's why in this case we do nothing, we will fall back
+ * in read mode in any case when xmit is finished -- see in
+ * ax12_send_callback() -- */
+static void ax12_switch_uart(uint8_t state)
+{
+ uint8_t flags;
+
+ if (state == AX12_STATE_WRITE) {
+ IRQ_LOCK(flags);
+ ax12_nsent=0;
+ while (uart_recv_nowait(UART_AX12_NUM) != -1);
+ UCSRxB |= (1<<TXEN);
+ ax12_state = AX12_STATE_WRITE;
+ IRQ_UNLOCK(flags);
+ }
+ else {
+ IRQ_LOCK(flags);
+ if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+ UCSRxB &= ~(1<<TXEN);
+ ax12_state = AX12_STATE_READ;
+ IRQ_UNLOCK(flags);
+ }
+}
+
+/* ----- */
+
+/* log rate limit */
+static void ax12_print_error(uint8_t err, uint16_t line)
+{
+ microseconds t2;
+
+ /* no more than 1 log per sec */
+ t2 = time_get_us2();
+
+ if (t2 - t_prev_msg < 1000000L) {
+ ax12_dropped_logs++;
+ return;
+ }
+ AX12_ERROR("AX12 error %x at line %d (%ld messages dropped)",
+ err, line, ax12_dropped_logs);
+ ax12_dropped_logs = 0;
+ t_prev_msg = t2;
+}
+
+uint8_t __ax12_user_write_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint8_t data, uint16_t line)
+{
+ uint8_t err, i;
+
+ ax12_stats_ops++;
+
+ for (i=0; i<AX12_MAX_TRIES ; i++) {
+ err = AX12_write_byte(ax12, id, address, data);
+ if (err == 0)
+ break;
+ wait_ms(2); /* BAD HACK XXX */
+ ax12_stats_fails++;
+ }
+ if (err == 0)
+ return 0;
+
+ ax12_print_error(err, line);
+ ax12_stats_drops++;
+ return err;
+}
+
+uint8_t __ax12_user_write_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint16_t data, uint16_t line)
+{
+ uint8_t err, i;
+
+ ax12_stats_ops++;
+
+ for (i=0; i<AX12_MAX_TRIES ; i++) {
+ err = AX12_write_int(ax12, id, address, data);
+ if (err == 0)
+ break;
+ wait_ms(2); /* BAD HACK XXX */
+ ax12_stats_fails++;
+ }
+ if (err == 0)
+ return 0;
+
+ ax12_print_error(err, line);
+ ax12_stats_drops++;
+ return err;
+}
+
+uint8_t __ax12_user_read_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint8_t *val, uint16_t line)
+{
+ uint8_t err, i;
+
+ ax12_stats_ops++;
+
+ for (i=0; i<AX12_MAX_TRIES ; i++) {
+ err = AX12_read_byte(ax12, id, address, val);
+ if (err == 0)
+ break;
+ wait_ms(2); /* BAD HACK XXX */
+ ax12_stats_fails++;
+ }
+ if (err == 0) {
+ /* XXX hack for broadcast */
+ if (id == AX12_BROADCAST_ID)
+ wait_ms(1);
+ return 0;
+ }
+
+ ax12_print_error(err, line);
+ ax12_stats_drops++;
+ return err;
+}
+
+uint8_t __ax12_user_read_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint16_t *val, uint16_t line)
+{
+ uint8_t err, i;
+
+ ax12_stats_ops++;
+
+ for (i=0; i<AX12_MAX_TRIES ; i++) {
+ err = AX12_read_int(ax12, id, address, val);
+ if (err == 0)
+ break;
+ wait_ms(2); /* BAD HACK XXX */
+ ax12_stats_fails++;
+ }
+ if (err == 0) {
+ /* XXX hack for broadcast */
+ if (id == AX12_BROADCAST_ID)
+ wait_ms(1);
+ return 0;
+ }
+
+ ax12_print_error(err, line);
+ ax12_stats_drops++;
+ return err;
+}
+
+void ax12_dump_stats(void)
+{
+ printf_P(PSTR("AX12 stats:\r\n"));
+ printf_P(PSTR(" total ops: %ld\r\n"), ax12_stats_ops);
+ printf_P(PSTR(" total fails: %ld\r\n"), ax12_stats_fails);
+ printf_P(PSTR(" total drops: %ld\r\n"), ax12_stats_drops);
+ printf_P(PSTR(" logs dropped since last message: %ld\r\n"), ax12_dropped_logs);
+}
+
+void ax12_user_init(void)
+{
+ /* AX12 */
+ AX12_init(&gen.ax12);
+ AX12_set_hardware_send(&gen.ax12, ax12_send_char);
+ AX12_set_hardware_recv(&gen.ax12, ax12_recv_char);
+ AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart);
+ uart_register_tx_event(UART_AX12_NUM, ax12_send_callback);
+ t_prev_msg = time_get_us2();
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: ax12_user.h,v 1.3 2009-04-07 20:03:48 zer0 Exp $
+ *
+ */
+
+/* This is the ax12 user interface. It initializes the aversive AX12
+ * module so that it works in background, using interrupt driver uart.
+ *
+ * Be carreful, a call to AX12 module is synchronous and uses
+ * interruptions, so interrupts must be enabled. On the other side, a
+ * call _must not_ interrupt another one. That's why all calls to the
+ * module are done either in init() functions or in a scheduler event
+ * with prio=ARM_PRIO.
+ */
+
+/* XXX do a safe_ax12() function that will retry once or twice if we
+ * see some problems. */
+
+#define UART_AX12_NUM 0
+
+void ax12_user_init(void);
+
+void ax12_dump_stats(void);
+
+#define ax12_user_write_byte(ax12, id, addr, data) \
+ __ax12_user_write_byte(ax12, id, addr, data, __LINE__)
+
+#define ax12_user_write_int(ax12, id, addr, data) \
+ __ax12_user_write_int(ax12, id, addr, data, __LINE__)
+
+#define ax12_user_read_byte(ax12, id, addr, data) \
+ __ax12_user_read_byte(ax12, id, addr, data, __LINE__)
+
+#define ax12_user_read_int(ax12, id, addr, data) \
+ __ax12_user_read_int(ax12, id, addr, data, __LINE__)
+
+/** @brief Write byte in AX-12 memory
+ * @return Error code from AX-12 (0 means okay) */
+uint8_t __ax12_user_write_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint8_t data, uint16_t line);
+
+/** @brief Write integer (2 bytes) in AX-12 memory
+ * @return Error code from AX-12 (0 means okay)
+ *
+ * address : data low
+ * address+1 : data high
+ */
+uint8_t __ax12_user_write_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint16_t data, uint16_t line);
+
+/** @brief Read byte from AX-12 memory
+ * @return Error code from AX-12 (0 means okay) */
+uint8_t __ax12_user_read_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint8_t *val, uint16_t line);
+
+/** @brief Write integer (2 bytes) from AX-12 memory
+ * @return Error code from AX-12 (0 means okay) */
+uint8_t __ax12_user_read_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
+ uint16_t *val, uint16_t line);
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include "main.h"
+#include "cmdline.h"
+
+
+/******** See in commands.c for the list of commands. */
+extern parse_pgm_ctx_t main_ctx[];
+
+static void write_char(char c)
+{
+ uart_send(CMDLINE_UART, c);
+}
+
+static void
+valid_buffer(const char *buf, __attribute__((unused)) uint8_t size)
+{
+ int8_t ret;
+ ret = parse(main_ctx, buf);
+ if (ret == PARSE_AMBIGUOUS)
+ printf_P(PSTR("Ambiguous command\r\n"));
+ else if (ret == PARSE_NOMATCH)
+ printf_P(PSTR("Command not found\r\n"));
+ else if (ret == PARSE_BAD_ARGS)
+ printf_P(PSTR("Bad arguments\r\n"));
+}
+
+static int8_t
+complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
+ int16_t *state)
+{
+ return complete(main_ctx, buf, state, dstbuf, dstsize);
+}
+
+
+/* sending "pop" on cmdline uart resets the robot */
+void emergency(char c)
+{
+ static uint8_t i = 0;
+
+ if ((i == 0 && c == 'p') ||
+ (i == 1 && c == 'o') ||
+ (i == 2 && c == 'p'))
+ i++;
+ else if ( !(i == 1 && c == 'p') )
+ i = 0;
+ if (i == 3)
+ reset();
+}
+
+/* log function, add a command to configure
+ * it dynamically */
+void mylog(struct error * e, ...)
+{
+ va_list ap;
+ u16 stream_flags = stdout->flags;
+ uint8_t i;
+ time_h tv;
+
+ if (e->severity > ERROR_SEVERITY_ERROR) {
+ if (gen.log_level < e->severity)
+ return;
+
+ for (i=0; i<NB_LOGS+1; i++)
+ if (gen.logs[i] == e->err_num)
+ break;
+ if (i == NB_LOGS+1)
+ return;
+ }
+
+ va_start(ap, e);
+ tv = time_get_time();
+ printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
+ vfprintf_P(stdout, e->text, ap);
+ printf_P(PSTR("\r\n"));
+ va_end(ap);
+ stdout->flags = stream_flags;
+}
+
+int cmdline_interact(void)
+{
+ const char *history, *buffer;
+ int8_t ret, same = 0;
+ int16_t c;
+
+ rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
+ snprintf(gen.prompt, sizeof(gen.prompt), "cobboard > ");
+ rdline_newline(&gen.rdl, gen.prompt);
+
+ while (1) {
+ c = uart_recv_nowait(CMDLINE_UART);
+ if (c == -1)
+ continue;
+ ret = rdline_char_in(&gen.rdl, c);
+ if (ret != 2 && ret != 0) {
+ buffer = rdline_get_buffer(&gen.rdl);
+ history = rdline_get_history_item(&gen.rdl, 0);
+ if (history) {
+ same = !memcmp(buffer, history, strlen(history)) &&
+ buffer[strlen(history)] == '\n';
+ }
+ else
+ same = 0;
+ if (strlen(buffer) > 1 && !same)
+ rdline_add_history(&gen.rdl, buffer);
+ rdline_newline(&gen.rdl, gen.prompt);
+ }
+ }
+
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#define CMDLINE_UART 1
+
+/* uart rx callback for reset() */
+void emergency(char c);
+
+/* log function */
+void mylog(struct error * e, ...);
+
+/* launch cmdline */
+int cmdline_interact(void);
+
+static inline uint8_t cmdline_keypressed(void) {
+ return (uart_recv_nowait(CMDLINE_UART) != -1);
+}
+
+static inline int16_t cmdline_getchar(void) {
+ return uart_recv_nowait(CMDLINE_UART);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdlib.h>
+#include <aversive/pgmspace.h>
+#include <parse.h>
+
+/* commands_gen.c */
+extern parse_pgm_inst_t cmd_reset;
+extern parse_pgm_inst_t cmd_bootloader;
+extern parse_pgm_inst_t cmd_encoders;
+extern parse_pgm_inst_t cmd_pwm;
+extern parse_pgm_inst_t cmd_adc;
+extern parse_pgm_inst_t cmd_sensor;
+extern parse_pgm_inst_t cmd_log;
+extern parse_pgm_inst_t cmd_log_show;
+extern parse_pgm_inst_t cmd_log_type;
+extern parse_pgm_inst_t cmd_stack_space;
+extern parse_pgm_inst_t cmd_scheduler;
+
+/* commands_ax12.c */
+extern parse_pgm_inst_t cmd_baudrate;
+extern parse_pgm_inst_t cmd_uint16_read;
+extern parse_pgm_inst_t cmd_uint16_write;
+extern parse_pgm_inst_t cmd_uint8_read;
+extern parse_pgm_inst_t cmd_uint8_write;
+extern parse_pgm_inst_t cmd_ax12_stress;
+extern parse_pgm_inst_t cmd_ax12_dump_stats;
+
+/* commands_cs.c */
+extern parse_pgm_inst_t cmd_gain;
+extern parse_pgm_inst_t cmd_gain_show;
+extern parse_pgm_inst_t cmd_speed;
+extern parse_pgm_inst_t cmd_speed_show;
+extern parse_pgm_inst_t cmd_derivate_filter;
+extern parse_pgm_inst_t cmd_derivate_filter_show;
+extern parse_pgm_inst_t cmd_consign;
+extern parse_pgm_inst_t cmd_maximum;
+extern parse_pgm_inst_t cmd_maximum_show;
+extern parse_pgm_inst_t cmd_quadramp;
+extern parse_pgm_inst_t cmd_quadramp_show;
+extern parse_pgm_inst_t cmd_cs_status;
+extern parse_pgm_inst_t cmd_blocking_i;
+extern parse_pgm_inst_t cmd_blocking_i_show;
+
+/* commands_cobboard.c */
+extern parse_pgm_inst_t cmd_event;
+extern parse_pgm_inst_t cmd_color;
+extern parse_pgm_inst_t cmd_state1;
+extern parse_pgm_inst_t cmd_state2;
+extern parse_pgm_inst_t cmd_state3;
+extern parse_pgm_inst_t cmd_state_debug;
+extern parse_pgm_inst_t cmd_state_machine;
+extern parse_pgm_inst_t cmd_servo_door;
+extern parse_pgm_inst_t cmd_servo_carry;
+extern parse_pgm_inst_t cmd_test;
+
+
+/* in progmem */
+parse_pgm_ctx_t main_ctx[] = {
+
+ /* commands_gen.c */
+ (parse_pgm_inst_t *)&cmd_reset,
+ (parse_pgm_inst_t *)&cmd_bootloader,
+ (parse_pgm_inst_t *)&cmd_encoders,
+ (parse_pgm_inst_t *)&cmd_pwm,
+ (parse_pgm_inst_t *)&cmd_adc,
+ (parse_pgm_inst_t *)&cmd_sensor,
+ (parse_pgm_inst_t *)&cmd_log,
+ (parse_pgm_inst_t *)&cmd_log_show,
+ (parse_pgm_inst_t *)&cmd_log_type,
+ (parse_pgm_inst_t *)&cmd_stack_space,
+ (parse_pgm_inst_t *)&cmd_scheduler,
+
+ /* commands_ax12.c */
+ (parse_pgm_inst_t *)&cmd_baudrate,
+ (parse_pgm_inst_t *)&cmd_uint16_read,
+ (parse_pgm_inst_t *)&cmd_uint16_write,
+ (parse_pgm_inst_t *)&cmd_uint8_read,
+ (parse_pgm_inst_t *)&cmd_uint8_write,
+ (parse_pgm_inst_t *)&cmd_ax12_stress,
+ (parse_pgm_inst_t *)&cmd_ax12_dump_stats,
+
+ /* commands_cs.c */
+ (parse_pgm_inst_t *)&cmd_gain,
+ (parse_pgm_inst_t *)&cmd_gain_show,
+ (parse_pgm_inst_t *)&cmd_speed,
+ (parse_pgm_inst_t *)&cmd_speed_show,
+ (parse_pgm_inst_t *)&cmd_consign,
+ (parse_pgm_inst_t *)&cmd_derivate_filter,
+ (parse_pgm_inst_t *)&cmd_derivate_filter_show,
+ (parse_pgm_inst_t *)&cmd_maximum,
+ (parse_pgm_inst_t *)&cmd_maximum_show,
+ (parse_pgm_inst_t *)&cmd_quadramp,
+ (parse_pgm_inst_t *)&cmd_quadramp_show,
+ (parse_pgm_inst_t *)&cmd_cs_status,
+ (parse_pgm_inst_t *)&cmd_blocking_i,
+ (parse_pgm_inst_t *)&cmd_blocking_i_show,
+
+ /* commands_cobboard.c */
+ (parse_pgm_inst_t *)&cmd_event,
+ (parse_pgm_inst_t *)&cmd_color,
+ (parse_pgm_inst_t *)&cmd_state1,
+ (parse_pgm_inst_t *)&cmd_state2,
+ (parse_pgm_inst_t *)&cmd_state3,
+ (parse_pgm_inst_t *)&cmd_state_debug,
+ (parse_pgm_inst_t *)&cmd_state_machine,
+ (parse_pgm_inst_t *)&cmd_servo_door,
+ (parse_pgm_inst_t *)&cmd_servo_carry,
+ (parse_pgm_inst_t *)&cmd_test,
+
+ NULL,
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_ax12.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "ax12_user.h"
+
+uint8_t addr_from_string(const char *s)
+{
+ /* 16 bits */
+ if (!strcmp_P(s, PSTR("cw_angle_limit")))
+ return AA_CW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("ccw_angle_limit")))
+ return AA_CCW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("max_torque")))
+ return AA_MAX_TORQUE_L;
+ if (!strcmp_P(s, PSTR("down_calibration")))
+ return AA_DOWN_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("up_calibration")))
+ return AA_UP_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("torque_limit")))
+ return AA_TORQUE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("position")))
+ return AA_PRESENT_POSITION_L;
+ if (!strcmp_P(s, PSTR("speed")))
+ return AA_PRESENT_SPEED_L;
+ if (!strcmp_P(s, PSTR("load")))
+ return AA_PRESENT_LOAD_L;
+ if (!strcmp_P(s, PSTR("moving_speed")))
+ return AA_MOVING_SPEED_L;
+ if (!strcmp_P(s, PSTR("model")))
+ return AA_MODEL_NUMBER_L;
+ if (!strcmp_P(s, PSTR("goal_pos")))
+ return AA_GOAL_POSITION_L;
+ if (!strcmp_P(s, PSTR("punch")))
+ return AA_PUNCH_L;
+
+ /* 8 bits */
+ if (!strcmp_P(s, PSTR("firmware")))
+ return AA_FIRMWARE;
+ if (!strcmp_P(s, PSTR("id")))
+ return AA_ID;
+ if (!strcmp_P(s, PSTR("baudrate")))
+ return AA_BAUD_RATE;
+ if (!strcmp_P(s, PSTR("delay")))
+ return AA_DELAY_TIME;
+ if (!strcmp_P(s, PSTR("high_lim_temp")))
+ return AA_HIGHEST_LIMIT_TEMP;
+ if (!strcmp_P(s, PSTR("low_lim_volt")))
+ return AA_LOWEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("high_lim_volt")))
+ return AA_HIGHEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("status_return")))
+ return AA_STATUS_RETURN_LEVEL;
+ if (!strcmp_P(s, PSTR("alarm_led")))
+ return AA_ALARM_LED;
+ if (!strcmp_P(s, PSTR("alarm_shutdown")))
+ return AA_ALARM_SHUTDOWN;
+ if (!strcmp_P(s, PSTR("torque_enable")))
+ return AA_TORQUE_ENABLE;
+ if (!strcmp_P(s, PSTR("led")))
+ return AA_LED;
+ if (!strcmp_P(s, PSTR("cw_comp_margin")))
+ return AA_CW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("ccw_comp_margin")))
+ return AA_CCW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("cw_comp_slope")))
+ return AA_CW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("ccw_comp_slope")))
+ return AA_CCW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("voltage")))
+ return AA_PRESENT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("temp")))
+ return AA_PRESENT_TEMP;
+ if (!strcmp_P(s, PSTR("reginst")))
+ return AA_PRESENT_REGINST;
+ if (!strcmp_P(s, PSTR("moving")))
+ return AA_MOVING;
+ if (!strcmp_P(s, PSTR("lock")))
+ return AA_LOCK;
+
+ return 0;
+}
+
+/**********************************************************/
+/* Ax12_Stress */
+
+/* this structure is filled when cmd_ax12_stress is parsed successfully */
+struct cmd_ax12_stress_result {
+ fixed_string_t arg0;
+ uint8_t id;
+};
+
+/* function called when cmd_ax12_stress is parsed successfully */
+static void cmd_ax12_stress_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_ax12_stress_result *res = parsed_result;
+ int i, nb_errs, id;
+ uint8_t val;
+ microseconds t;
+
+ t = time_get_us2();
+ nb_errs = 0;
+ for (i=0; i<1000; i++) {
+ if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
+ nb_errs ++;
+ }
+
+ printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
+ t = (time_get_us2() - t) / 1000;
+ printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
+
+ t = time_get_us2();
+ nb_errs = 0;
+ for (i=0; i<1000; i++) {
+ if (AX12_write_int(&gen.ax12, res->id, AA_GOAL_POSITION_L, 500))
+ nb_errs ++;
+ }
+
+ printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
+ t = (time_get_us2() - t) / 1000;
+ printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
+
+ /* test 5 servos */
+ t = time_get_us2();
+ nb_errs = 0;
+ id = 1;
+ for (i=0; i<100; i++) {
+ if (AX12_write_int(&gen.ax12, id, AA_GOAL_POSITION_L, 500))
+ nb_errs ++;
+ id ++;
+ if (id > 5)
+ id = 1;
+ }
+
+ printf_P(PSTR("%d errors / 100\r\n"), nb_errs);
+ t = (time_get_us2() - t) / 1000;
+ printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
+
+
+}
+
+prog_char str_ax12_stress_arg0[] = "ax12_stress";
+parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
+parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
+
+prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands (id)";
+parse_pgm_inst_t cmd_ax12_stress = {
+ .f = cmd_ax12_stress_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ax12_stress,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ax12_stress_arg0,
+ (prog_void *)&cmd_ax12_stress_id,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Ax12_Dump_Stats */
+
+/* this structure is filled when cmd_ax12_dump_stats is parsed successfully */
+struct cmd_ax12_dump_stats_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_ax12_dump_stats is parsed successfully */
+static void cmd_ax12_dump_stats_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ ax12_dump_stats();
+}
+
+prog_char str_ax12_dump_stats_arg0[] = "ax12_dump_stats";
+parse_pgm_token_string_t cmd_ax12_dump_stats_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_dump_stats_result, arg0, str_ax12_dump_stats_arg0);
+
+prog_char help_ax12_dump_stats[] = "Dump AX12 stats";
+parse_pgm_inst_t cmd_ax12_dump_stats = {
+ .f = cmd_ax12_dump_stats_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ax12_dump_stats,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ax12_dump_stats_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_baudrate is parsed successfully */
+struct cmd_baudrate_result {
+ fixed_string_t arg0;
+ uint32_t arg1;
+};
+
+/* function called when cmd_baudrate is parsed successfully */
+static void cmd_baudrate_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_baudrate_result *res = parsed_result;
+ struct uart_config c;
+
+ uart_getconf(UART_AX12_NUM, &c);
+ c.baudrate = res->arg1;
+ uart_setconf(UART_AX12_NUM, &c);
+}
+
+prog_char str_baudrate_arg0[] = "baudrate";
+parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
+parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
+
+prog_char help_baudrate[] = "Change ax12 baudrate";
+parse_pgm_inst_t cmd_baudrate = {
+ .f = cmd_baudrate_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_baudrate,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_baudrate_arg0,
+ (prog_void *)&cmd_baudrate_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint16 */
+
+
+/* this structure is filled when cmd_uint16_read is parsed successfully */
+struct cmd_uint16_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint16_t val;
+};
+
+/* function called when cmd_uint16_read is parsed successfully */
+static void cmd_uint16_read_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint16_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+ ret = ax12_user_read_int(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint16_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
+prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#down_calibration#up_calibration#torque_limit#"
+ "position#speed#load#punch";
+parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
+parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
+
+prog_char help_uint16_read[] = "Read uint16 value (type, num)";
+parse_pgm_inst_t cmd_uint16_read = {
+ .f = cmd_uint16_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0,
+ (prog_void *)&cmd_uint16_arg1,
+ (prog_void *)&cmd_uint16_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint16_write is parsed successfully */
+static void cmd_uint16_write_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t addr = addr_from_string(res->arg1);
+ printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = ax12_user_write_int(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint16_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
+prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#torque_limit#punch";
+parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
+parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
+
+prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
+parse_pgm_inst_t cmd_uint16_write = {
+ .f = cmd_uint16_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0_w,
+ (prog_void *)&cmd_uint16_arg1_w,
+ (prog_void *)&cmd_uint16_num,
+ (prog_void *)&cmd_uint16_val,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint8 */
+
+
+/* this structure is filled when cmd_uint8_read is parsed successfully */
+struct cmd_uint8_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint8_t val;
+};
+
+/* function called when cmd_uint8_read is parsed successfully */
+static void cmd_uint8_read_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+
+ ret = ax12_user_read_byte(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint8_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
+prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "voltage#temp#reginst#moving#lock";
+parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
+parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
+
+prog_char help_uint8_read[] = "Read uint8 value (type, num)";
+parse_pgm_inst_t cmd_uint8_read = {
+ .f = cmd_uint8_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0,
+ (prog_void *)&cmd_uint8_arg1,
+ (prog_void *)&cmd_uint8_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint8_write is parsed successfully */
+static void cmd_uint8_write_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t addr = addr_from_string(res->arg1);
+ uint8_t ret;
+ printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = ax12_user_write_byte(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint8_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
+prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "reginst#lock";
+parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
+parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
+
+prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
+parse_pgm_inst_t cmd_uint8_write = {
+ .f = cmd_uint8_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0_w,
+ (prog_void *)&cmd_uint8_arg1_w,
+ (prog_void *)&cmd_uint8_num,
+ (prog_void *)&cmd_uint8_val,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "sensor.h"
+#include "cmdline.h"
+#include "state.h"
+#include "i2c_protocol.h"
+#include "actuator.h"
+#include "actuator.h"
+
+extern uint16_t state_debug;
+
+struct cmd_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+
+/* function called when cmd_event is parsed successfully */
+static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ u08 bit=0;
+
+ struct cmd_event_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("all"))) {
+ bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ cobboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ cobboard.flags &= bit;
+ else { /* show */
+ printf_P(PSTR("encoders is %s\r\n"),
+ (DO_ENCODERS & cobboard.flags) ? "on":"off");
+ printf_P(PSTR("cs is %s\r\n"),
+ (DO_CS & cobboard.flags) ? "on":"off");
+ printf_P(PSTR("bd is %s\r\n"),
+ (DO_BD & cobboard.flags) ? "on":"off");
+ printf_P(PSTR("power is %s\r\n"),
+ (DO_POWER & cobboard.flags) ? "on":"off");
+ }
+ return;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("encoders")))
+ bit = DO_ENCODERS;
+ else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ bit = DO_CS;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("bd")))
+ bit = DO_BD;
+ else if (!strcmp_P(res->arg1, PSTR("power")))
+ bit = DO_POWER;
+
+
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ cobboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off"))) {
+ if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ pwm_ng_set(LEFT_SPICKLE_PWM, 0);
+ pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
+ pwm_ng_set(SHOVEL_PWM, 0);
+ }
+ cobboard.flags &= (~bit);
+ }
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ (bit & cobboard.flags) ? "on":"off");
+}
+
+prog_char str_event_arg0[] = "event";
+parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
+prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
+parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
+prog_char str_event_arg2[] = "on#off#show";
+parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
+
+prog_char help_event[] = "Enable/disable events";
+parse_pgm_inst_t cmd_event = {
+ .f = cmd_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_event_arg0,
+ (prog_void *)&cmd_event_arg1,
+ (prog_void *)&cmd_event_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Color */
+
+/* this structure is filled when cmd_color is parsed successfully */
+struct cmd_color_result {
+ fixed_string_t arg0;
+ fixed_string_t color;
+};
+
+/* function called when cmd_color is parsed successfully */
+static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
+ if (!strcmp_P(res->color, PSTR("red"))) {
+ cobboard.our_color = I2C_COLOR_RED;
+ }
+ else if (!strcmp_P(res->color, PSTR("green"))) {
+ cobboard.our_color = I2C_COLOR_GREEN;
+ }
+ printf_P(PSTR("Done\r\n"));
+}
+
+prog_char str_color_arg0[] = "color";
+parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
+prog_char str_color_color[] = "green#red";
+parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
+
+prog_char help_color[] = "Set our color";
+parse_pgm_inst_t cmd_color = {
+ .f = cmd_color_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_color,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_color_arg0,
+ (prog_void *)&cmd_color_color,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State1 */
+
+/* this structure is filled when cmd_state1 is parsed successfully */
+struct cmd_state1_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_state1 is parsed successfully */
+static void cmd_state1_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state1_result *res = parsed_result;
+ struct i2c_cmd_cobboard_set_mode command;
+
+ if (!strcmp_P(res->arg1, PSTR("init"))) {
+ state_init();
+ return;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("manual")))
+ command.mode = I2C_COBBOARD_MODE_MANUAL;
+ else if (!strcmp_P(res->arg1, PSTR("harvest")))
+ command.mode = I2C_COBBOARD_MODE_HARVEST;
+ state_set_mode(&command);
+}
+
+prog_char str_state1_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
+prog_char str_state1_arg1[] = "init#manual";
+parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
+
+prog_char help_state1[] = "set cobboard mode";
+parse_pgm_inst_t cmd_state1 = {
+ .f = cmd_state1_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state1,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state1_arg0,
+ (prog_void *)&cmd_state1_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State2 */
+
+/* this structure is filled when cmd_state2 is parsed successfully */
+struct cmd_state2_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_state2 is parsed successfully */
+static void cmd_state2_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state2_result *res = parsed_result;
+ struct i2c_cmd_cobboard_set_mode command;
+ uint8_t side;
+
+ if (!strcmp_P(res->arg2, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("right")))
+ side = I2C_RIGHT_SIDE;
+
+ if (!strcmp_P(res->arg1, PSTR("yyy"))) {
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xxx"))) {
+ }
+
+
+ state_set_mode(&command);
+}
+
+prog_char str_state2_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
+prog_char str_state2_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
+prog_char str_state2_arg2[] = "left#right";
+parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
+
+prog_char help_state2[] = "set cobboard mode";
+parse_pgm_inst_t cmd_state2 = {
+ .f = cmd_state2_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state2_arg0,
+ (prog_void *)&cmd_state2_arg1,
+ (prog_void *)&cmd_state2_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State3 */
+
+/* this structure is filled when cmd_state3 is parsed successfully */
+struct cmd_state3_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+};
+
+/* function called when cmd_state3 is parsed successfully */
+static void cmd_state3_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state3_result *res = parsed_result;
+ struct i2c_cmd_cobboard_set_mode command;
+
+ if (!strcmp_P(res->arg1, PSTR("xxx"))) {
+ /* xxx = res->arg2 */
+ }
+ else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
+ }
+ state_set_mode(&command);
+}
+
+prog_char str_state3_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
+prog_char str_state3_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
+parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8);
+
+prog_char help_state3[] = "set cobboard mode";
+parse_pgm_inst_t cmd_state3 = {
+ .f = cmd_state3_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state3,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state3_arg0,
+ (prog_void *)&cmd_state3_arg1,
+ (prog_void *)&cmd_state3_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State_Machine */
+
+/* this structure is filled when cmd_state_machine is parsed successfully */
+struct cmd_state_machine_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_state_machine is parsed successfully */
+static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ state_machine();
+}
+
+prog_char str_state_machine_arg0[] = "state_machine";
+parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
+
+prog_char help_state_machine[] = "launch state machine";
+parse_pgm_inst_t cmd_state_machine = {
+ .f = cmd_state_machine_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state_machine,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state_machine_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State_Debug */
+
+/* this structure is filled when cmd_state_debug is parsed successfully */
+struct cmd_state_debug_result {
+ fixed_string_t arg0;
+ uint8_t on;
+};
+
+/* function called when cmd_state_debug is parsed successfully */
+static void cmd_state_debug_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state_debug_result *res = parsed_result;
+ state_debug = res->on;
+}
+
+prog_char str_state_debug_arg0[] = "state_debug";
+parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
+parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
+
+prog_char help_state_debug[] = "Set debug timer for state machine";
+parse_pgm_inst_t cmd_state_debug = {
+ .f = cmd_state_debug_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state_debug,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state_debug_arg0,
+ (prog_void *)&cmd_state_debug_on,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Servo_Door */
+
+/* this structure is filled when cmd_servo_door is parsed successfully */
+struct cmd_servo_door_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_door is parsed successfully */
+static void cmd_servo_door_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_servo_door_result *res = parsed_result;
+ if (!strcmp_P(res->arg1, PSTR("open")))
+ servo_door_open();
+ else if (!strcmp_P(res->arg1, PSTR("closed")))
+ servo_door_close();
+}
+
+prog_char str_servo_door_arg0[] = "door";
+parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0);
+prog_char str_servo_door_arg1[] = "open#closed";
+parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1);
+
+prog_char help_servo_door[] = "Servo door function";
+parse_pgm_inst_t cmd_servo_door = {
+ .f = cmd_servo_door_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo_door,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_servo_door_arg0,
+ (prog_void *)&cmd_servo_door_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Servo_Carry */
+
+/* this structure is filled when cmd_servo_carry is parsed successfully */
+struct cmd_servo_carry_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_carry is parsed successfully */
+static void cmd_servo_carry_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_servo_carry_result *res = parsed_result;
+ if (!strcmp_P(res->arg1, PSTR("open")))
+ servo_carry_open();
+ else if (!strcmp_P(res->arg1, PSTR("closed")))
+ servo_carry_close();
+}
+
+prog_char str_servo_carry_arg0[] = "carry";
+parse_pgm_token_string_t cmd_servo_carry_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg0, str_servo_carry_arg0);
+prog_char str_servo_carry_arg1[] = "open#closed";
+parse_pgm_token_string_t cmd_servo_carry_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg1, str_servo_carry_arg1);
+
+prog_char help_servo_carry[] = "Servo carry function";
+parse_pgm_inst_t cmd_servo_carry = {
+ .f = cmd_servo_carry_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo_carry,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_servo_carry_arg0,
+ (prog_void *)&cmd_servo_carry_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+}
+
+prog_char str_test_arg0[] = "test";
+parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
+
+prog_char help_test[] = "Test function";
+parse_pgm_inst_t cmd_test = {
+ .f = cmd_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_test_arg0,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_cs.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cs.h"
+#include "cmdline.h"
+
+struct csb_list {
+ const prog_char *name;
+ struct cs_block *csb;
+};
+
+prog_char csb_left_spickle_str[] = "left_spickle";
+prog_char csb_right_spickle_str[] = "right_spickle";
+prog_char csb_shovel_str[] = "shovel";
+struct csb_list csb_list[] = {
+ { .name = csb_left_spickle_str, .csb = &cobboard.left_spickle },
+ { .name = csb_right_spickle_str, .csb = &cobboard.right_spickle },
+ { .name = csb_shovel_str, .csb = &cobboard.shovel },
+};
+
+struct cmd_cs_result {
+ fixed_string_t cmdname;
+ fixed_string_t csname;
+};
+
+/* token to be used for all cs-related commands */
+prog_char str_csb_name[] = "left_spickle#right_spickle#shovel";
+parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
+
+struct cs_block *cs_from_name(const char *name)
+{
+ unsigned int i;
+
+ for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
+ if (!strcmp_P(name, csb_list[i].name))
+ return csb_list[i].csb;
+ }
+ return NULL;
+}
+
+/**********************************************************/
+/* Gains for control system */
+
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_result {
+ struct cmd_cs_result cs;
+ int16_t p;
+ int16_t i;
+ int16_t d;
+};
+
+/* function called when cmd_gain is parsed successfully */
+static void cmd_gain_parsed(void * parsed_result, void *show)
+{
+ struct cmd_gain_result *res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_gains(&csb->pid, res->p, res->i, res->d);
+
+ printf_P(PSTR("%s %s %d %d %d\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ pid_get_gain_P(&csb->pid),
+ pid_get_gain_I(&csb->pid),
+ pid_get_gain_D(&csb->pid));
+}
+
+prog_char str_gain_arg0[] = "gain";
+parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
+parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
+parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
+parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
+
+prog_char help_gain[] = "Set gain values for PID";
+parse_pgm_inst_t cmd_gain = {
+ .f = cmd_gain_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_gain,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_p,
+ (prog_void *)&cmd_gain_i,
+ (prog_void *)&cmd_gain_d,
+ NULL,
+ },
+};
+
+/* show */
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_gain_show_arg[] = "show";
+parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
+
+prog_char help_gain_show[] = "Show gain values for PID";
+parse_pgm_inst_t cmd_gain_show = {
+ .f = cmd_gain_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_gain_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Speeds for control system */
+
+/* this structure is filled when cmd_speed is parsed successfully */
+struct cmd_speed_result {
+ struct cmd_cs_result cs;
+ uint16_t s;
+};
+
+/* function called when cmd_speed is parsed successfully */
+static void cmd_speed_parsed(void *parsed_result, __attribute__((unused)) void *show)
+{
+ struct cmd_speed_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+#if notyet
+ if (!show)
+ ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
+
+ printf_P(PSTR("%s %lu\r\n"),
+ res->cs.csname,
+ ext.r_b.var_pos);
+#else
+ printf_P(PSTR("TODO\r\n"));
+#endif
+}
+
+prog_char str_speed_arg0[] = "speed";
+parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
+parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
+
+prog_char help_speed[] = "Set speed values for ramp filter";
+parse_pgm_inst_t cmd_speed = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_speed,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_s,
+ NULL,
+ },
+};
+
+/* show */
+struct cmd_speed_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
+
+prog_char help_speed_show[] = "Show speed values for ramp filter";
+parse_pgm_inst_t cmd_speed_show = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_speed_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Derivate_Filters for control system */
+
+/* this structure is filled when cmd_derivate_filter is parsed successfully */
+struct cmd_derivate_filter_result {
+ struct cmd_cs_result cs;
+ uint8_t size;
+};
+
+/* function called when cmd_derivate_filter is parsed successfully */
+static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
+{
+ struct cmd_derivate_filter_result * res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_derivate_filter(&csb->pid, res->size);
+
+ printf_P(PSTR("%s %s %u\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ pid_get_derivate_filter(&csb->pid));
+}
+
+prog_char str_derivate_filter_arg0[] = "derivate_filter";
+parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
+parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
+
+prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
+parse_pgm_inst_t cmd_derivate_filter = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_derivate_filter,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_size,
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_derivate_filter_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_derivate_filter_show_arg[] = "show";
+parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
+
+prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
+parse_pgm_inst_t cmd_derivate_filter_show = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_derivate_filter_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_show_arg,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Consign for control system */
+
+/* this structure is filled when cmd_consign is parsed successfully */
+struct cmd_consign_result {
+ struct cmd_cs_result cs;
+ int32_t p;
+};
+
+/* function called when cmd_consign is parsed successfully */
+static void cmd_consign_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_consign_result * res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ cs_set_consign(&csb->cs, res->p);
+}
+
+prog_char str_consign_arg0[] = "consign";
+parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
+parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
+
+prog_char help_consign[] = "Set consign value";
+parse_pgm_inst_t cmd_consign = {
+ .f = cmd_consign_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_consign,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_consign_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_consign_p,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Maximums for control system */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_result {
+ struct cmd_cs_result cs;
+ uint32_t in;
+ uint32_t i;
+ uint32_t out;
+};
+
+/* function called when cmd_maximum is parsed successfully */
+static void cmd_maximum_parsed(void *parsed_result, void *show)
+{
+ struct cmd_maximum_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_maximums(&csb->pid, res->in, res->i, res->out);
+
+ printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
+ res->cs.csname,
+ pid_get_max_in(&csb->pid),
+ pid_get_max_I(&csb->pid),
+ pid_get_max_out(&csb->pid));
+}
+
+prog_char str_maximum_arg0[] = "maximum";
+parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
+parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
+parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
+parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
+
+prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
+parse_pgm_inst_t cmd_maximum = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_maximum,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_in,
+ (prog_void *)&cmd_maximum_i,
+ (prog_void *)&cmd_maximum_out,
+ NULL,
+ },
+};
+
+/* show */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+prog_char str_maximum_show_arg[] = "show";
+parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
+
+prog_char help_maximum_show[] = "Show maximum values for PID";
+parse_pgm_inst_t cmd_maximum_show = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_maximum_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Quadramp for control system */
+
+/* this structure is filled when cmd_quadramp is parsed successfully */
+struct cmd_quadramp_result {
+ struct cmd_cs_result cs;
+ uint32_t ap;
+ uint32_t an;
+ uint32_t sp;
+ uint32_t sn;
+};
+
+/* function called when cmd_quadramp is parsed successfully */
+static void cmd_quadramp_parsed(void *parsed_result, void *show)
+{
+ struct cmd_quadramp_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show) {
+ quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
+ quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
+ }
+
+ printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
+ res->cs.csname,
+ csb->qr.var_2nd_ord_pos,
+ csb->qr.var_2nd_ord_neg,
+ csb->qr.var_1st_ord_pos,
+ csb->qr.var_1st_ord_neg);
+}
+
+prog_char str_quadramp_arg0[] = "quadramp";
+parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+
+prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
+parse_pgm_inst_t cmd_quadramp = {
+ .f = cmd_quadramp_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_quadramp,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_ap,
+ (prog_void *)&cmd_quadramp_an,
+ (prog_void *)&cmd_quadramp_sp,
+ (prog_void *)&cmd_quadramp_sn,
+
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_quadramp_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_quadramp_show_arg[] = "show";
+parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
+
+prog_char help_quadramp_show[] = "Get quadramp values for control system";
+parse_pgm_inst_t cmd_quadramp_show = {
+ .f = cmd_quadramp_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_quadramp_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_show_arg,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* cs_status show for control system */
+
+/* this structure is filled when cmd_cs_status is parsed successfully */
+struct cmd_cs_status_result {
+ struct cmd_cs_result cs;
+ fixed_string_t arg;
+};
+
+/* function called when cmd_cs_status is parsed successfully */
+static void cmd_cs_status_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_cs_status_result *res = parsed_result;
+ struct cs_block *csb;
+ uint8_t loop = 0;
+ uint8_t print_pid = 0, print_cs = 0;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+ if (strcmp_P(res->arg, PSTR("on")) == 0) {
+ csb->on = 1;
+ printf_P(PSTR("%s is on\r\n"), res->cs.csname);
+ return;
+ }
+ else if (strcmp_P(res->arg, PSTR("off")) == 0) {
+ csb->on = 0;
+ printf_P(PSTR("%s is off\r\n"), res->cs.csname);
+ return;
+ }
+ else if (strcmp_P(res->arg, PSTR("show")) == 0) {
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
+ loop = 1;
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
+ print_pid = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
+ print_pid = 1;
+ loop = 1;
+ }
+
+ printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
+ do {
+ if (print_cs)
+ dump_cs(res->cs.csname, &csb->cs);
+ if (print_pid)
+ dump_pid(res->cs.csname, &csb->pid);
+ wait_ms(100);
+ } while(loop && !cmdline_keypressed());
+}
+
+prog_char str_cs_status_arg0[] = "cs_status";
+parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
+prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
+parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
+
+prog_char help_cs_status[] = "Show cs status";
+parse_pgm_inst_t cmd_cs_status = {
+ .f = cmd_cs_status_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_cs_status,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_cs_status_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_cs_status_arg,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Blocking_I for control system */
+
+/* this structure is filled when cmd_blocking_i is parsed successfully */
+struct cmd_blocking_i_result {
+ struct cmd_cs_result cs;
+ int32_t k1;
+ int32_t k2;
+ uint32_t i;
+ uint16_t cpt;
+};
+
+/* function called when cmd_blocking_i is parsed successfully */
+static void cmd_blocking_i_parsed(void *parsed_result, void *show)
+{
+ struct cmd_blocking_i_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
+ res->i, res->cpt);
+
+ printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ csb->bd.k1,
+ csb->bd.k2,
+ csb->bd.i_thres,
+ csb->bd.cpt_thres);
+}
+
+prog_char str_blocking_i_arg0[] = "blocking";
+parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
+parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
+parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
+parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
+parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
+
+prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
+parse_pgm_inst_t cmd_blocking_i = {
+ .f = cmd_blocking_i_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_blocking_i,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_k1,
+ (prog_void *)&cmd_blocking_i_k2,
+ (prog_void *)&cmd_blocking_i_i,
+ (prog_void *)&cmd_blocking_i_cpt,
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_blocking_i_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_blocking_i_show_arg[] = "show";
+parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg);
+
+prog_char help_blocking_i_show[] = "Show blocking detection values";
+parse_pgm_inst_t cmd_blocking_i_show = {
+ .f = cmd_blocking_i_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_blocking_i_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_show_arg,
+ NULL,
+ },
+};
+
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_gen.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <encoders_spi.h>
+#include <adc.h>
+
+#include <scheduler.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include <diagnostic.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "sensor.h"
+
+/**********************************************************/
+/* Reset */
+
+/* this structure is filled when cmd_reset is parsed successfully */
+struct cmd_reset_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_reset is parsed successfully */
+static void cmd_reset_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ reset();
+}
+
+prog_char str_reset_arg0[] = "reset";
+parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
+
+prog_char help_reset[] = "Reset the board";
+parse_pgm_inst_t cmd_reset = {
+ .f = cmd_reset_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_reset,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_reset_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Bootloader */
+
+/* this structure is filled when cmd_bootloader is parsed successfully */
+struct cmd_bootloader_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_bootloader is parsed successfully */
+static void cmd_bootloader_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ bootloader();
+}
+
+prog_char str_bootloader_arg0[] = "bootloader";
+parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
+
+prog_char help_bootloader[] = "Launch the bootloader";
+parse_pgm_inst_t cmd_bootloader = {
+ .f = cmd_bootloader_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_bootloader,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_bootloader_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Encoders tests */
+
+/* this structure is filled when cmd_encoders is parsed successfully */
+struct cmd_encoders_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_encoders is parsed successfully */
+static void cmd_encoders_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_encoders_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ encoders_spi_set_value((void *)0, 0);
+ encoders_spi_set_value((void *)1, 0);
+ encoders_spi_set_value((void *)2, 0);
+ encoders_spi_set_value((void *)3, 0);
+ return;
+ }
+
+ /* show */
+ while(!cmdline_keypressed()) {
+ printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
+ encoders_spi_get_value((void *)0),
+ encoders_spi_get_value((void *)1),
+ encoders_spi_get_value((void *)2),
+ encoders_spi_get_value((void *)3));
+ wait_ms(100);
+ }
+}
+
+prog_char str_encoders_arg0[] = "encoders";
+parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
+prog_char str_encoders_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
+
+prog_char help_encoders[] = "Show encoders values";
+parse_pgm_inst_t cmd_encoders = {
+ .f = cmd_encoders_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_encoders,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_encoders_arg0,
+ (prog_void *)&cmd_encoders_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Scheduler show */
+
+/* this structure is filled when cmd_scheduler is parsed successfully */
+struct cmd_scheduler_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_scheduler is parsed successfully */
+static void cmd_scheduler_parsed( __attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ scheduler_dump_events();
+}
+
+prog_char str_scheduler_arg0[] = "scheduler";
+parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
+prog_char str_scheduler_arg1[] = "show";
+parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
+
+prog_char help_scheduler[] = "Show scheduler events";
+parse_pgm_inst_t cmd_scheduler = {
+ .f = cmd_scheduler_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_scheduler,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_scheduler_arg0,
+ (prog_void *)&cmd_scheduler_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Pwms tests */
+
+/* this structure is filled when cmd_pwm is parsed successfully */
+struct cmd_pwm_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t arg2;
+};
+
+/* function called when cmd_pwm is parsed successfully */
+static void cmd_pwm_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ void * pwm_ptr = NULL;
+ struct cmd_pwm_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("1(4A)")))
+ pwm_ptr = &gen.pwm1_4A;
+ else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
+ pwm_ptr = &gen.pwm2_4B;
+ else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
+ pwm_ptr = &gen.pwm3_1A;
+ else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
+ pwm_ptr = &gen.pwm4_1B;
+
+ else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
+ pwm_ptr = &gen.servo1;
+ else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
+ pwm_ptr = &gen.servo2;
+ else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
+ pwm_ptr = &gen.servo3;
+ else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
+ pwm_ptr = &gen.servo4;
+
+ if (pwm_ptr)
+ pwm_ng_set(pwm_ptr, res->arg2);
+
+ printf_P(PSTR("done\r\n"));
+}
+
+prog_char str_pwm_arg0[] = "pwm";
+parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
+prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)";
+parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
+parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
+
+prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
+parse_pgm_inst_t cmd_pwm = {
+ .f = cmd_pwm_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pwm,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pwm_arg0,
+ (prog_void *)&cmd_pwm_arg1,
+ (prog_void *)&cmd_pwm_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Adcs tests */
+
+/* this structure is filled when cmd_adc is parsed successfully */
+struct cmd_adc_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_adc is parsed successfully */
+static void cmd_adc_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_adc_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("ADC values: "));
+ for (i=0; i<ADC_MAX; i++) {
+ printf_P(PSTR("%.4d "), sensor_get_adc(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_adc_arg0[] = "adc";
+parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
+prog_char str_adc_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
+
+prog_char help_adc[] = "Show adc values";
+parse_pgm_inst_t cmd_adc = {
+ .f = cmd_adc_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_adc,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_adc_arg0,
+ (prog_void *)&cmd_adc_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Sensors tests */
+
+/* this structure is filled when cmd_sensor is parsed successfully */
+struct cmd_sensor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_sensor is parsed successfully */
+static void cmd_sensor_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_sensor_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("SENSOR values: "));
+ for (i=0; i<SENSOR_MAX; i++) {
+ printf_P(PSTR("%d "), !!sensor_get(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_sensor_arg0[] = "sensor";
+parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
+prog_char str_sensor_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
+
+prog_char help_sensor[] = "Show sensor values";
+parse_pgm_inst_t cmd_sensor = {
+ .f = cmd_sensor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sensor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_sensor_arg0,
+ (prog_void *)&cmd_sensor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+ fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const prog_char uart_log[] = "uart";
+static const prog_char i2c_log[] = "i2c";
+static const prog_char i2cproto_log[] = "i2cproto";
+static const prog_char sensor_log[] = "sensor";
+static const prog_char block_log[] = "bd";
+static const prog_char state_log[] = "state";
+static const prog_char ax12_log[] = "ax12";
+
+struct log_name_and_num {
+ const prog_char * name;
+ uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+ { uart_log, E_UART },
+ { i2c_log, E_I2C },
+ { i2cproto_log, E_USER_I2C_PROTO },
+ { sensor_log, E_USER_SENSOR },
+ { block_log, E_BLOCKING_DETECTION_MANAGER },
+ { state_log, E_USER_ST_MACH },
+ { ax12_log, E_USER_AX12 },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (!strcmp_P(s, log_name_and_num[i].name)) {
+ return log_name_and_num[i].num;
+ }
+ }
+ return 0;
+}
+
+const prog_char *
+log_num2name(uint8_t num)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (num == log_name_and_num[i].num) {
+ return log_name_and_num[i].name;
+ }
+ }
+ return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+ uint8_t i, empty=1;
+ const prog_char * name;
+
+ printf_P(PSTR("log level is %d\r\n"), gen.log_level);
+ for (i=0; i<NB_LOGS; i++) {
+ name = log_num2name(gen.logs[i]);
+ if (name) {
+ printf_P(PSTR("log type %S is on\r\n"), name);
+ empty = 0;
+ }
+ }
+ if (empty)
+ printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("level"))) {
+ gen.log_level = res->arg2;
+ }
+
+ /* else it is a show */
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg0[] = "log";
+parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
+prog_char str_log_arg1[] = "level";
+parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
+parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
+
+prog_char help_log[] = "Set log options: level (0 -> 5)";
+parse_pgm_inst_t cmd_log = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1,
+ (prog_void *)&cmd_log_arg2,
+ NULL,
+ },
+};
+
+prog_char str_log_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
+
+prog_char help_log_show[] = "Show configured logs";
+parse_pgm_inst_t cmd_log_show = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_show,
+ NULL,
+ },
+};
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_type_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_type_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
+ uint8_t lognum;
+ uint8_t i;
+
+ lognum = log_name2num(res->arg2);
+ if (lognum == 0) {
+ printf_P(PSTR("Cannot find log num\r\n"));
+ return;
+ }
+
+ if (!strcmp_P(res->arg3, PSTR("on"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ printf_P(PSTR("Already on\r\n"));
+ return;
+ }
+ }
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == 0) {
+ gen.logs[i] = lognum;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("no more room\r\n"));
+ }
+ }
+ else if (!strcmp_P(res->arg3, PSTR("off"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ gen.logs[i] = 0;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("already off\r\n"));
+ }
+ }
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg1_type[] = "type";
+parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd##state#ax12";
+parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+prog_char str_log_arg3[] = "on#off";
+parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+prog_char help_log_type[] = "Set log type";
+parse_pgm_inst_t cmd_log_type = {
+ .f = cmd_log_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_type,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_type,
+ (prog_void *)&cmd_log_arg2_type,
+ (prog_void *)&cmd_log_arg3,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ printf("res stack: %d\r\n", min_stack_space_available());
+
+}
+
+prog_char str_stack_space_arg0[] = "stack_space";
+parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+prog_char help_stack_space[] = "Display remaining stack space";
+parse_pgm_inst_t cmd_stack_space = {
+ .f = cmd_stack_space_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_stack_space,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_stack_space_arg0,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cs.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "actuator.h"
+
+/* called every 5 ms */
+static void do_cs(__attribute__((unused)) void *dummy)
+{
+ /* read encoders */
+ if (cobboard.flags & DO_ENCODERS) {
+ encoders_spi_manage(NULL);
+ }
+ /* control system */
+ if (cobboard.flags & DO_CS) {
+ if (cobboard.left_spickle.on)
+ cs_manage(&cobboard.left_spickle.cs);
+ if (cobboard.right_spickle.on)
+ cs_manage(&cobboard.right_spickle.cs);
+ if (cobboard.shovel.on)
+ cs_manage(&cobboard.shovel.cs);
+ }
+ if (cobboard.flags & DO_BD) {
+ bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs);
+ bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs);
+ bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs);
+ }
+ if (cobboard.flags & DO_POWER)
+ BRAKE_OFF();
+ else
+ BRAKE_ON();
+}
+
+void dump_cs_debug(const char *name, struct cs *cs)
+{
+ DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "
+ "in=% .5ld out=% .5ld",
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_cs(const char *name, struct cs *cs)
+{
+ printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
+ "in=% .5ld out=% .5ld\r\n"),
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_pid(const char *name, struct pid_filter *pid)
+{
+ printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ name,
+ pid_get_value_in(pid) * pid_get_gain_P(pid),
+ pid_get_value_I(pid) * pid_get_gain_I(pid),
+ pid_get_value_D(pid) * pid_get_gain_D(pid),
+ pid_get_value_out(pid));
+}
+
+void microb_cs_init(void)
+{
+ /* ---- CS left_spickle */
+ /* PID */
+ pid_init(&cobboard.left_spickle.pid);
+ pid_set_gains(&cobboard.left_spickle.pid, 500, 40, 5000);
+ pid_set_maximums(&cobboard.left_spickle.pid, 0, 5000, 2400); /* max is 12 V */
+ pid_set_out_shift(&cobboard.left_spickle.pid, 10);
+ pid_set_derivate_filter(&cobboard.left_spickle.pid, 4);
+
+ /* QUADRAMP */
+ quadramp_init(&cobboard.left_spickle.qr);
+ quadramp_set_1st_order_vars(&cobboard.left_spickle.qr, 2000, 2000); /* set speed */
+ quadramp_set_2nd_order_vars(&cobboard.left_spickle.qr, 20, 20); /* set accel */
+
+ /* CS */
+ cs_init(&cobboard.left_spickle.cs);
+ cs_set_consign_filter(&cobboard.left_spickle.cs, quadramp_do_filter, &cobboard.left_spickle.qr);
+ cs_set_correct_filter(&cobboard.left_spickle.cs, pid_do_filter, &cobboard.left_spickle.pid);
+ cs_set_process_in(&cobboard.left_spickle.cs, pwm_ng_set, LEFT_SPICKLE_PWM);
+ cs_set_process_out(&cobboard.left_spickle.cs, encoders_spi_get_value, LEFT_SPICKLE_ENCODER);
+ cs_set_consign(&cobboard.left_spickle.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&cobboard.left_spickle.bd);
+ bd_set_speed_threshold(&cobboard.left_spickle.bd, 150);
+ bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40);
+
+ /* ---- CS right_spickle */
+ /* PID */
+ pid_init(&cobboard.right_spickle.pid);
+ pid_set_gains(&cobboard.right_spickle.pid, 500, 40, 5000);
+ pid_set_maximums(&cobboard.right_spickle.pid, 0, 5000, 2400); /* max is 12 V */
+ pid_set_out_shift(&cobboard.right_spickle.pid, 10);
+ pid_set_derivate_filter(&cobboard.right_spickle.pid, 6);
+
+ /* QUADRAMP */
+ quadramp_init(&cobboard.right_spickle.qr);
+ quadramp_set_1st_order_vars(&cobboard.right_spickle.qr, 1000, 1000); /* set speed */
+ quadramp_set_2nd_order_vars(&cobboard.right_spickle.qr, 20, 20); /* set accel */
+
+ /* CS */
+ cs_init(&cobboard.right_spickle.cs);
+ cs_set_consign_filter(&cobboard.right_spickle.cs, quadramp_do_filter, &cobboard.right_spickle.qr);
+ cs_set_correct_filter(&cobboard.right_spickle.cs, pid_do_filter, &cobboard.right_spickle.pid);
+ cs_set_process_in(&cobboard.right_spickle.cs, pwm_ng_set, RIGHT_SPICKLE_PWM);
+ cs_set_process_out(&cobboard.right_spickle.cs, encoders_spi_get_value, RIGHT_SPICKLE_ENCODER);
+ cs_set_consign(&cobboard.right_spickle.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&cobboard.right_spickle.bd);
+ bd_set_speed_threshold(&cobboard.right_spickle.bd, 150);
+ bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40);
+
+ /* ---- CS shovel */
+ /* PID */
+ pid_init(&cobboard.shovel.pid);
+ pid_set_gains(&cobboard.shovel.pid, 500, 40, 5000);
+ pid_set_maximums(&cobboard.shovel.pid, 0, 5000, 2400); /* max is 12 V */
+ pid_set_out_shift(&cobboard.shovel.pid, 10);
+ pid_set_derivate_filter(&cobboard.shovel.pid, 6);
+
+ /* QUADRAMP */
+ quadramp_init(&cobboard.shovel.qr);
+ quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 1000); /* set speed */
+ quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 20, 20); /* set accel */
+
+ /* CS */
+ cs_init(&cobboard.shovel.cs);
+ cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);
+ cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid);
+ cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM);
+ cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER);
+ cs_set_consign(&cobboard.shovel.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&cobboard.shovel.bd);
+ bd_set_speed_threshold(&cobboard.shovel.bd, 150);
+ bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40);
+
+ /* set them on !! */
+ cobboard.left_spickle.on = 1;
+ cobboard.right_spickle.on = 1;
+ cobboard.shovel.on = 1;
+
+ scheduler_add_periodical_event_priority(do_cs, NULL,
+ CS_PERIOD / SCHEDULER_UNIT,
+ CS_PRIO);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cs.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $
+ *
+ */
+
+void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
--- /dev/null
+/*
+ * 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_
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_protocol.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/error.h>
+
+#include <i2c.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "sensor.h"
+#include "state.h"
+
+void i2c_protocol_init(void)
+{
+}
+
+/*** LED CONTROL ***/
+void i2c_led_control(uint8_t l, uint8_t state)
+{
+ switch(l) {
+ case 1:
+ state? LED1_ON():LED1_OFF();
+ break;
+ case 2:
+ state? LED2_ON():LED2_OFF();
+ break;
+ case 3:
+ state? LED3_ON():LED3_OFF();
+ break;
+ case 4:
+ state? LED4_ON():LED4_OFF();
+ break;
+ default:
+ break;
+ }
+}
+
+#ifdef notyet
+static void i2c_test(uint16_t val)
+{
+ static uint16_t prev=0;
+
+ if ( (val-prev) != 1 ) {
+ WARNING(E_USER_I2C_PROTO, "Dupplicated msg %d", val);
+ }
+ prev = val;
+ ext.nb_test_cmd ++;
+}
+#endif
+
+static void i2c_send_status(void)
+{
+ struct i2c_ans_cobboard_status ans;
+ i2c_flush();
+ ans.hdr.cmd = I2C_ANS_COBBOARD_STATUS;
+
+ /* status */
+ ans.mode = state_get_mode();
+ ans.status = 0x55;
+
+ i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
+ sizeof(ans), I2C_CTRL_GENERIC);
+}
+
+static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+{
+ state_set_mode(cmd);
+ return 0;
+}
+
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+ void *void_cmd = buf;
+
+ static uint8_t a = 0;
+
+ a++;
+ if (a & 0x10)
+ LED2_TOGGLE();
+
+ if (size <= 0) {
+ goto error;
+ }
+
+ switch (buf[0]) {
+
+ /* Commands (no answer needed) */
+ case I2C_CMD_GENERIC_LED_CONTROL:
+ {
+ struct i2c_cmd_led_control *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+ i2c_led_control(cmd->led_num, cmd->state);
+ break;
+ }
+
+ case I2C_CMD_COBBOARD_SET_MODE:
+ {
+ struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
+ if (size != sizeof(struct i2c_cmd_cobboard_set_mode))
+ goto error;
+ i2c_set_mode(cmd);
+ break;
+ }
+
+ case I2C_CMD_GENERIC_SET_COLOR:
+ {
+ struct i2c_cmd_generic_color *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+ cobboard.our_color = cmd->color;
+ break;
+ }
+
+#ifdef notyet
+ case I2C_CMD_EXTENSION_TEST:
+ {
+ struct i2c_cmd_extension_test *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+ i2c_test(cmd->val);
+ break;
+ }
+#endif
+
+ /* Add other commands here ...*/
+
+
+ case I2C_REQ_COBBOARD_STATUS:
+ {
+ //struct i2c_req_cobboard_status *cmd = void_cmd;
+ if (size != sizeof (struct i2c_req_cobboard_status))
+ goto error;
+
+ i2c_send_status();
+ break;
+ }
+
+ default:
+ goto error;
+ }
+
+ error:
+ /* log error on a led ? */
+ return;
+}
+
+void i2c_recvbyteevent(__attribute__((unused)) uint8_t hwstatus,
+ __attribute__((unused)) uint8_t i,
+ __attribute__((unused)) int8_t c)
+{
+}
+
+void i2c_sendevent(__attribute__((unused)) int8_t size)
+{
+}
+
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2007)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_protocol.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+
+void i2c_protocol_init(void);
+
+void i2c_recvevent(uint8_t * buf, int8_t size);
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
+void i2c_sendevent(int8_t size);
+
+int debug_send(char c, FILE* f);
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: main.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <avr/eeprom.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <i2c.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "../common/eeprom_mapping.h"
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+#include "ax12_user.h"
+#include "cmdline.h"
+#include "sensor.h"
+#include "state.h"
+#include "actuator.h"
+#include "arm_xy.h"
+#include "cs.h"
+#include "i2c_protocol.h"
+
+/* 0 means "programmed"
+ * ---- with 16 Mhz quartz
+ * CKSEL 3-0 : 0111
+ * SUT 1-0 : 10
+ * CKDIV8 : 1
+ * ---- bootloader
+ * BOOTZ 1-0 : 01 (4K bootloader)
+ * BOOTRST : 0 (reset on bootloader)
+ * ---- jtag
+ * jtagen : 0
+ */
+
+struct genboard gen;
+struct cobboard cobboard;
+
+/***********************/
+
+void bootloader(void)
+{
+#define BOOTLOADER_ADDR 0x3f000
+ if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
+ printf_P(PSTR("Bootloader is not present\r\n"));
+ return;
+ }
+ cli();
+ BRAKE_ON();
+ /* ... very specific :( */
+ TIMSK0 = 0;
+ TIMSK1 = 0;
+ TIMSK2 = 0;
+ TIMSK3 = 0;
+ TIMSK4 = 0;
+ TIMSK5 = 0;
+ EIMSK = 0;
+ UCSR0B = 0;
+ UCSR1B = 0;
+ UCSR2B = 0;
+ UCSR3B = 0;
+ SPCR = 0;
+ TWCR = 0;
+ ACSR = 0;
+ ADCSRA = 0;
+
+ EIND = 1;
+ __asm__ __volatile__ ("ldi r31,0xf8\n");
+ __asm__ __volatile__ ("ldi r30,0x00\n");
+ __asm__ __volatile__ ("eijmp\n");
+
+ /* never returns */
+}
+
+void do_led_blink(__attribute__((unused)) void *dummy)
+{
+#if 1 /* simple blink */
+ static uint8_t a=0;
+
+ if(a)
+ LED1_ON();
+ else
+ LED1_OFF();
+
+ a = !a;
+#endif
+}
+
+static void main_timer_interrupt(void)
+{
+ static uint8_t cpt = 0;
+ cpt++;
+ sei();
+ if ((cpt & 0x3) == 0)
+ scheduler_interrupt();
+}
+
+int main(void)
+{
+ /* brake */
+ BRAKE_ON();
+ BRAKE_DDR();
+
+ /* CPLD reset on PG3 */
+ DDRG |= 1<<3;
+ PORTG &= ~(1<<3); /* implicit */
+
+ /* LEDS */
+ DDRJ |= 0x0c;
+ DDRL = 0xc0;
+ LED1_OFF();
+ memset(&gen, 0, sizeof(gen));
+ memset(&cobboard, 0, sizeof(cobboard));
+ /* cs is enabled after arm_calibrate() */
+ cobboard.flags = DO_ENCODERS | DO_POWER; // DO_BD
+
+ /* UART */
+ uart_init();
+#if CMDLINE_UART == 3
+ fdevopen(uart3_dev_send, uart3_dev_recv);
+ uart_register_rx_event(3, emergency);
+#elif CMDLINE_UART == 1
+ fdevopen(uart1_dev_send, uart1_dev_recv);
+ uart_register_rx_event(1, emergency);
+#else
+# error not supported
+#endif
+
+ //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
+ /* check eeprom to avoid to run the bad program */
+ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
+ EEPROM_MAGIC_COBBOARD) {
+ sei();
+ printf_P(PSTR("Bad eeprom value\r\n"));
+ while(1);
+ }
+
+ /* LOGS */
+ error_register_emerg(mylog);
+ error_register_error(mylog);
+ error_register_warning(mylog);
+ error_register_notice(mylog);
+ error_register_debug(mylog);
+
+ /* SPI + ENCODERS */
+ encoders_spi_init(); /* this will also init spi hardware */
+
+ /* I2C */
+ i2c_protocol_init();
+ i2c_init(I2C_MODE_SLAVE, I2C_COBBOARD_ADDR);
+ i2c_register_recv_event(i2c_recvevent);
+
+ /* TIMER */
+ timer_init();
+ timer0_register_OV_intr(main_timer_interrupt);
+
+ /* PWM */
+ PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
+ TIMER1_PRESCALER_DIV_1);
+ PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10,
+ TIMER4_PRESCALER_DIV_1);
+
+ PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
+ &PORTD, 4);
+ PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
+ &PORTD, 5);
+ PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED |
+ PWM_NG_MODE_SIGN_INVERTED,
+ &PORTD, 6);
+ PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED |
+ PWM_NG_MODE_SIGN_INVERTED,
+ &PORTD, 7);
+
+
+ /* servos */
+ PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
+ TIMER1_PRESCALER_DIV_256);
+ PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+ PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10,
+ TIMER1_PRESCALER_DIV_256);
+ PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+ PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+ PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
+ NULL, 0);
+
+ /* SCHEDULER */
+ scheduler_init();
+
+ scheduler_add_periodical_event_priority(do_led_blink, NULL,
+ 100000L / SCHEDULER_UNIT,
+ LED_PRIO);
+ /* all cs management */
+ microb_cs_init();
+
+ /* sensors, will also init hardware adc */
+ sensor_init();
+
+ /* TIME */
+ time_init(TIME_PRIO);
+
+ /* ax12 */
+ ax12_user_init();
+
+ sei();
+
+ /* finger + other actuators */
+ actuator_init();
+
+ state_init();
+
+ printf_P(PSTR("\r\n"));
+ printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
+
+ /* arm management */
+ gen.logs[0] = E_USER_ST_MACH;
+ gen.log_level = 5;
+ cobboard.flags |= DO_CS;
+
+ state_machine();
+ cmdline_interact();
+
+ return 0;
+}
--- /dev/null
+/*
+ * 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; \
+})
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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_
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ * Olivier MATZ <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: sensor.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ */
+
+#include <stdlib.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <adc.h>
+#include <scheduler.h>
+#include <ax12.h>
+#include <pwm_ng.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "sensor.h"
+
+/************ ADC */
+
+struct adc_infos {
+ uint16_t config;
+ int16_t value;
+ int16_t prev_val;
+ int16_t (*filter)(struct adc_infos *, int16_t);
+};
+
+/* reach 90% of the value in 4 samples */
+int16_t rii_light(struct adc_infos *adc, int16_t val)
+{
+ adc->prev_val = val + (int32_t)adc->prev_val / 2;
+ return adc->prev_val / 2;
+}
+
+/* reach 90% of the value in 8 samples */
+int16_t rii_medium(struct adc_infos *adc, int16_t val)
+{
+ adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4;
+ return adc->prev_val / 4;
+}
+
+/* reach 90% of the value in 16 samples */
+int16_t rii_strong(struct adc_infos *adc, int16_t val)
+{
+ adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8;
+ return adc->prev_val / 8;
+}
+
+
+#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
+
+/* define which ADC to poll, see in sensor.h */
+static struct adc_infos adc_infos[ADC_MAX] = {
+ [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
+ [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
+ [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
+ [ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium },
+
+ /* add adc on "cap" pins if needed */
+/* [ADC_CAP1] = { .config = ADC_CONF(10) }, */
+/* [ADC_CAP2] = { .config = ADC_CONF(11) }, */
+/* [ADC_CAP3] = { .config = ADC_CONF(12) }, */
+/* [ADC_CAP4] = { .config = ADC_CONF(13) }, */
+};
+
+static void adc_event(int16_t result);
+
+/* called every 10 ms, see init below */
+static void do_adc(__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_MAX; i++) {
+ if ((1 << i) & sensor) {
+ if (sensor_filter[i].cpt < sensor_filter[i].filter)
+ sensor_filter[i].cpt++;
+ if (sensor_filter[i].cpt >= sensor_filter[i].thres_on)
+ sensor_filter[i].prev = 1;
+ }
+ else {
+ if (sensor_filter[i].cpt > 0)
+ sensor_filter[i].cpt--;
+ if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
+ sensor_filter[i].prev = 0;
+ }
+
+ if (sensor_filter[i].prev && !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);
+}
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ * Olivier MATZ <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: sensor.h,v 1.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);
--- /dev/null
+/*
+ * 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
+
--- /dev/null
+/*
+ * 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 <math.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <vt100.h>
+
+#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;
+}
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/* \r
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
+ * \r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation; either version 2 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ *\r
+ * Revision : $Id: uart_config.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $\r
+ *\r
+ */\r
+\r
+/* Droids-corp 2004 - Zer0\r
+ * config for uart module\r
+ */\r
+\r
+#ifndef UART_CONFIG_H\r
+#define UART_CONFIG_H\r
+\r
+/* compile uart0 fonctions, undefine it to pass compilation */\r
+#define UART0_COMPILE \r
+#define UART0_ENABLED 1\r
+#define UART0_INTERRUPT_ENABLED 1\r
+#define UART0_BAUDRATE 57600\r
+//#define UART0_BAUDRATE 1000000\r
+#define UART0_USE_DOUBLE_SPEED 1\r
+#define UART0_RX_FIFO_SIZE 32\r
+#define UART0_TX_FIFO_SIZE 32\r
+#define UART0_NBITS 8\r
+#define UART0_PARITY UART_PARTITY_NONE\r
+#define UART0_STOP_BIT UART_STOP_BITS_2\r
+\r
+#define UART1_COMPILE \r
+#define UART1_ENABLED 1\r
+#define UART1_INTERRUPT_ENABLED 1\r
+#define UART1_BAUDRATE 57600\r
+#define UART1_USE_DOUBLE_SPEED 1\r
+#define UART1_RX_FIFO_SIZE 128\r
+#define UART1_TX_FIFO_SIZE 128\r
+#define UART1_NBITS 8\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+\r
+#define UART3_COMPILE \r
+#define UART3_ENABLED 1\r
+#define UART3_INTERRUPT_ENABLED 1\r
+#define UART3_BAUDRATE 57600\r
+#define UART3_USE_DOUBLE_SPEED 1\r
+#define UART3_RX_FIFO_SIZE 128\r
+#define UART3_TX_FIFO_SIZE 128\r
+#define UART3_NBITS 8\r
+#define UART3_PARITY UART_PARTITY_NONE\r
+#define UART3_STOP_BIT UART_STOP_BITS_1\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+#endif\r
+\r
#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)
#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;
};
/****/
-#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;
};
};
/****/
-#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_ */
+++ /dev/null
-#
-# 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
+++ /dev/null
-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) ;\
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
- *
- */
-
-#include <aversive.h>
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <spi.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-
-#include "../common/i2c_commands.h"
-#include "actuator.h"
-#include "ax12_user.h"
-#include "main.h"
-
-#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();
-}
+++ /dev/null
-/*
- * 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);
-
+++ /dev/null
-/*
- * 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 <aversive.h>
-#include <aversive/wait.h>
-#include <aversive/pgmspace.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <spi.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-
-#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);
-}
+++ /dev/null
-/*
- * 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);
+++ /dev/null
-/*
- * 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 <serpilliere@droids-corp.org>
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <math.h>
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/wait.h>
-#include <aversive/pgmspace.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <spi.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-
-#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");
-}
+++ /dev/null
-/*
- * 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 <serpilliere@droids-corp.org>
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-/* 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);
+++ /dev/null
-#ifndef _AX12_CONFIG_H_
-#define _AX12_CONFIG_H_
-
-#define AX12_MAX_PARAMS 32
-
-
-#endif/*_AX12_CONFIG_H_*/
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: ax12_user.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $
- *
- */
-
-#include <aversive.h>
-#include <aversive/list.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <i2c.h>
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "main.h"
-#include "ax12_user.h"
-
-#define AX12_ERROR(args...) ERROR(E_USER_AX12, args)
-#define AX12_NOTICE(args...) NOTICE(E_USER_AX12, args)
-#define AX12_MAX_TRIES 3
-
-/*
- * Cmdline interface for AX12. Use the PC to command a daisy-chain of
- * AX12 actuators with a nice command line interface.
- *
- * The circuit should be as following:
- *
- * |----------|
- * | uart3|------->--- PC (baudrate=57600)
- * | |-------<---
- * | atmega128|
- * | |
- * | uart0|---->---+-- AX12 (baudrate 115200)
- * | |----<---|
- * |----------|
- *
- * Note that RX and TX pins of UART1 are connected together to provide
- * a half-duplex UART emulation.
- *
- */
-
-#define UCSRxB UCSR0B
-#define AX12_TIMEOUT 15000L /* in us */
-
-static uint32_t ax12_stats_ops = 0; /* total ops */
-static uint32_t ax12_stats_fails = 0; /* number of fails */
-static uint32_t ax12_stats_drops = 0; /* number of drops (3 fails) */
-static uint32_t ax12_dropped_logs = 0; /* error messages that were not displayed */
-static microseconds t_prev_msg = 0;
-
-/********************************* AX12 commands */
-
-/*
- * We use synchronous access (not interrupt driven) to the hardware
- * UART, because we have to be sure that the transmission/reception is
- * really finished when we return from the functions.
- *
- * We don't use the CM-5 circuit as described in the AX12
- * documentation, we simply connect TX and RX and use TXEN + RXEN +
- * DDR to manage the port directions.
- */
-
-static volatile uint8_t ax12_state = AX12_STATE_READ;
-extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
-static volatile uint8_t ax12_nsent = 0;
-
-/* Called by ax12 module to send a character on serial line. Count the
- * number of transmitted bytes. It will be used in ax12_recv_char() to
- * drop the bytes that we transmitted. */
-static int8_t ax12_send_char(uint8_t c)
-{
- uart_send(UART_AX12_NUM, c);
- ax12_nsent++;
- return 0;
-}
-
-/* for atmega256 */
-#ifndef TXEN
-#define TXEN TXEN0
-#endif
-
-/* called by uart module when the character has been written in
- * UDR. It does not mean that the byte is physically transmitted. */
-static void ax12_send_callback(__attribute__((unused)) char c)
-{
- if (ax12_state == AX12_STATE_READ) {
- /* disable TX when last byte is pushed. */
- if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
- UCSRxB &= ~(1<<TXEN);
- }
-}
-
-/* Called by ax12 module when we want to receive a char. Note that we
- * also receive the bytes we sent ! So we need to drop them. */
-static int16_t ax12_recv_char(void)
-{
- microseconds t = time_get_us2();
- int c;
- while (1) {
- c = uart_recv_nowait(UART_AX12_NUM);
- if (c != -1) {
- if (ax12_nsent == 0)
- return c;
- ax12_nsent --;
- }
-
- /* 5 ms timeout */
- if ((time_get_us2() - t) > AX12_TIMEOUT)
- return -1;
- }
- return c;
-}
-
-/* called by ax12 module when we want to switch serial line. As we
- * work in interruption mode, this function can be called to switch
- * back in read mode even if the bytes are not really transmitted on
- * the line. That's why in this case we do nothing, we will fall back
- * in read mode in any case when xmit is finished -- see in
- * ax12_send_callback() -- */
-static void ax12_switch_uart(uint8_t state)
-{
- uint8_t flags;
-
- if (state == AX12_STATE_WRITE) {
- IRQ_LOCK(flags);
- ax12_nsent=0;
- while (uart_recv_nowait(UART_AX12_NUM) != -1);
- UCSRxB |= (1<<TXEN);
- ax12_state = AX12_STATE_WRITE;
- IRQ_UNLOCK(flags);
- }
- else {
- IRQ_LOCK(flags);
- if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
- UCSRxB &= ~(1<<TXEN);
- ax12_state = AX12_STATE_READ;
- IRQ_UNLOCK(flags);
- }
-}
-
-/* ----- */
-
-/* log rate limit */
-static void ax12_print_error(uint8_t err, uint16_t line)
-{
- microseconds t2;
-
- /* no more than 1 log per sec */
- t2 = time_get_us2();
-
- if (t2 - t_prev_msg < 1000000L) {
- ax12_dropped_logs++;
- return;
- }
- AX12_ERROR("AX12 error %x at line %d (%ld messages dropped)",
- err, line, ax12_dropped_logs);
- ax12_dropped_logs = 0;
- t_prev_msg = t2;
-}
-
-uint8_t __ax12_user_write_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint8_t data, uint16_t line)
-{
- uint8_t err, i;
-
- ax12_stats_ops++;
-
- for (i=0; i<AX12_MAX_TRIES ; i++) {
- err = AX12_write_byte(ax12, id, address, data);
- if (err == 0)
- break;
- wait_ms(2); /* BAD HACK XXX */
- ax12_stats_fails++;
- }
- if (err == 0)
- return 0;
-
- ax12_print_error(err, line);
- ax12_stats_drops++;
- return err;
-}
-
-uint8_t __ax12_user_write_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint16_t data, uint16_t line)
-{
- uint8_t err, i;
-
- ax12_stats_ops++;
-
- for (i=0; i<AX12_MAX_TRIES ; i++) {
- err = AX12_write_int(ax12, id, address, data);
- if (err == 0)
- break;
- wait_ms(2); /* BAD HACK XXX */
- ax12_stats_fails++;
- }
- if (err == 0)
- return 0;
-
- ax12_print_error(err, line);
- ax12_stats_drops++;
- return err;
-}
-
-uint8_t __ax12_user_read_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint8_t *val, uint16_t line)
-{
- uint8_t err, i;
-
- ax12_stats_ops++;
-
- for (i=0; i<AX12_MAX_TRIES ; i++) {
- err = AX12_read_byte(ax12, id, address, val);
- if (err == 0)
- break;
- wait_ms(2); /* BAD HACK XXX */
- ax12_stats_fails++;
- }
- if (err == 0) {
- /* XXX hack for broadcast */
- if (id == AX12_BROADCAST_ID)
- wait_ms(1);
- return 0;
- }
-
- ax12_print_error(err, line);
- ax12_stats_drops++;
- return err;
-}
-
-uint8_t __ax12_user_read_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint16_t *val, uint16_t line)
-{
- uint8_t err, i;
-
- ax12_stats_ops++;
-
- for (i=0; i<AX12_MAX_TRIES ; i++) {
- err = AX12_read_int(ax12, id, address, val);
- if (err == 0)
- break;
- wait_ms(2); /* BAD HACK XXX */
- ax12_stats_fails++;
- }
- if (err == 0) {
- /* XXX hack for broadcast */
- if (id == AX12_BROADCAST_ID)
- wait_ms(1);
- return 0;
- }
-
- ax12_print_error(err, line);
- ax12_stats_drops++;
- return err;
-}
-
-void ax12_dump_stats(void)
-{
- printf_P(PSTR("AX12 stats:\r\n"));
- printf_P(PSTR(" total ops: %ld\r\n"), ax12_stats_ops);
- printf_P(PSTR(" total fails: %ld\r\n"), ax12_stats_fails);
- printf_P(PSTR(" total drops: %ld\r\n"), ax12_stats_drops);
- printf_P(PSTR(" logs dropped since last message: %ld\r\n"), ax12_dropped_logs);
-}
-
-void ax12_user_init(void)
-{
- /* AX12 */
- AX12_init(&gen.ax12);
- AX12_set_hardware_send(&gen.ax12, ax12_send_char);
- AX12_set_hardware_recv(&gen.ax12, ax12_recv_char);
- AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart);
- uart_register_tx_event(UART_AX12_NUM, ax12_send_callback);
- t_prev_msg = time_get_us2();
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: ax12_user.h,v 1.3 2009-04-07 20:03:48 zer0 Exp $
- *
- */
-
-/* This is the ax12 user interface. It initializes the aversive AX12
- * module so that it works in background, using interrupt driver uart.
- *
- * Be carreful, a call to AX12 module is synchronous and uses
- * interruptions, so interrupts must be enabled. On the other side, a
- * call _must not_ interrupt another one. That's why all calls to the
- * module are done either in init() functions or in a scheduler event
- * with prio=ARM_PRIO.
- */
-
-/* XXX do a safe_ax12() function that will retry once or twice if we
- * see some problems. */
-
-#define UART_AX12_NUM 0
-
-void ax12_user_init(void);
-
-void ax12_dump_stats(void);
-
-#define ax12_user_write_byte(ax12, id, addr, data) \
- __ax12_user_write_byte(ax12, id, addr, data, __LINE__)
-
-#define ax12_user_write_int(ax12, id, addr, data) \
- __ax12_user_write_int(ax12, id, addr, data, __LINE__)
-
-#define ax12_user_read_byte(ax12, id, addr, data) \
- __ax12_user_read_byte(ax12, id, addr, data, __LINE__)
-
-#define ax12_user_read_int(ax12, id, addr, data) \
- __ax12_user_read_int(ax12, id, addr, data, __LINE__)
-
-/** @brief Write byte in AX-12 memory
- * @return Error code from AX-12 (0 means okay) */
-uint8_t __ax12_user_write_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint8_t data, uint16_t line);
-
-/** @brief Write integer (2 bytes) in AX-12 memory
- * @return Error code from AX-12 (0 means okay)
- *
- * address : data low
- * address+1 : data high
- */
-uint8_t __ax12_user_write_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint16_t data, uint16_t line);
-
-/** @brief Read byte from AX-12 memory
- * @return Error code from AX-12 (0 means okay) */
-uint8_t __ax12_user_read_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint8_t *val, uint16_t line);
-
-/** @brief Write integer (2 bytes) from AX-12 memory
- * @return Error code from AX-12 (0 means okay) */
-uint8_t __ax12_user_read_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
- uint16_t *val, uint16_t line);
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cmdline.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/error.h>
-
-#include <parse.h>
-#include <rdline.h>
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include "main.h"
-#include "cmdline.h"
-
-
-/******** See in commands.c for the list of commands. */
-extern parse_pgm_ctx_t main_ctx[];
-
-static void write_char(char c)
-{
- uart_send(CMDLINE_UART, c);
-}
-
-static void
-valid_buffer(const char *buf, __attribute__((unused)) uint8_t size)
-{
- int8_t ret;
- ret = parse(main_ctx, buf);
- if (ret == PARSE_AMBIGUOUS)
- printf_P(PSTR("Ambiguous command\r\n"));
- else if (ret == PARSE_NOMATCH)
- printf_P(PSTR("Command not found\r\n"));
- else if (ret == PARSE_BAD_ARGS)
- printf_P(PSTR("Bad arguments\r\n"));
-}
-
-static int8_t
-complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
- int16_t *state)
-{
- return complete(main_ctx, buf, state, dstbuf, dstsize);
-}
-
-
-/* sending "pop" on cmdline uart resets the robot */
-void emergency(char c)
-{
- static uint8_t i = 0;
-
- if ((i == 0 && c == 'p') ||
- (i == 1 && c == 'o') ||
- (i == 2 && c == 'p'))
- i++;
- else if ( !(i == 1 && c == 'p') )
- i = 0;
- if (i == 3)
- reset();
-}
-
-/* log function, add a command to configure
- * it dynamically */
-void mylog(struct error * e, ...)
-{
- va_list ap;
- u16 stream_flags = stdout->flags;
- uint8_t i;
- time_h tv;
-
- if (e->severity > ERROR_SEVERITY_ERROR) {
- if (gen.log_level < e->severity)
- return;
-
- for (i=0; i<NB_LOGS+1; i++)
- if (gen.logs[i] == e->err_num)
- break;
- if (i == NB_LOGS+1)
- return;
- }
-
- va_start(ap, e);
- tv = time_get_time();
- printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
- vfprintf_P(stdout, e->text, ap);
- printf_P(PSTR("\r\n"));
- va_end(ap);
- stdout->flags = stream_flags;
-}
-
-int cmdline_interact(void)
-{
- const char *history, *buffer;
- int8_t ret, same = 0;
- int16_t c;
-
- rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
- snprintf(gen.prompt, sizeof(gen.prompt), "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;
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cmdline.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $
- *
- */
-
-#define CMDLINE_UART 1
-
-/* uart rx callback for reset() */
-void emergency(char c);
-
-/* log function */
-void mylog(struct error * e, ...);
-
-/* launch cmdline */
-int cmdline_interact(void);
-
-static inline uint8_t cmdline_keypressed(void) {
- return (uart_recv_nowait(CMDLINE_UART) != -1);
-}
-
-static inline int16_t cmdline_getchar(void) {
- return uart_recv_nowait(CMDLINE_UART);
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdlib.h>
-#include <aversive/pgmspace.h>
-#include <parse.h>
-
-/* commands_gen.c */
-extern parse_pgm_inst_t cmd_reset;
-extern parse_pgm_inst_t cmd_bootloader;
-extern parse_pgm_inst_t cmd_encoders;
-extern parse_pgm_inst_t cmd_pwm;
-extern parse_pgm_inst_t cmd_adc;
-extern parse_pgm_inst_t cmd_sensor;
-extern parse_pgm_inst_t cmd_log;
-extern parse_pgm_inst_t cmd_log_show;
-extern parse_pgm_inst_t cmd_log_type;
-extern parse_pgm_inst_t cmd_stack_space;
-extern parse_pgm_inst_t cmd_scheduler;
-
-/* commands_ax12.c */
-extern parse_pgm_inst_t cmd_baudrate;
-extern parse_pgm_inst_t cmd_uint16_read;
-extern parse_pgm_inst_t cmd_uint16_write;
-extern parse_pgm_inst_t cmd_uint8_read;
-extern parse_pgm_inst_t cmd_uint8_write;
-extern parse_pgm_inst_t cmd_ax12_stress;
-extern parse_pgm_inst_t cmd_ax12_dump_stats;
-
-/* commands_cs.c */
-extern parse_pgm_inst_t cmd_gain;
-extern parse_pgm_inst_t cmd_gain_show;
-extern parse_pgm_inst_t cmd_speed;
-extern parse_pgm_inst_t cmd_speed_show;
-extern parse_pgm_inst_t cmd_derivate_filter;
-extern parse_pgm_inst_t cmd_derivate_filter_show;
-extern parse_pgm_inst_t cmd_consign;
-extern parse_pgm_inst_t cmd_maximum;
-extern parse_pgm_inst_t cmd_maximum_show;
-extern parse_pgm_inst_t cmd_quadramp;
-extern parse_pgm_inst_t cmd_quadramp_show;
-extern parse_pgm_inst_t cmd_cs_status;
-extern parse_pgm_inst_t cmd_blocking_i;
-extern parse_pgm_inst_t cmd_blocking_i_show;
-
-/* commands_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,
-};
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands_ax12.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "main.h"
-#include "ax12_user.h"
-
-uint8_t addr_from_string(const char *s)
-{
- /* 16 bits */
- if (!strcmp_P(s, PSTR("cw_angle_limit")))
- return AA_CW_ANGLE_LIMIT_L;
- if (!strcmp_P(s, PSTR("ccw_angle_limit")))
- return AA_CCW_ANGLE_LIMIT_L;
- if (!strcmp_P(s, PSTR("max_torque")))
- return AA_MAX_TORQUE_L;
- if (!strcmp_P(s, PSTR("down_calibration")))
- return AA_DOWN_CALIBRATION_L;
- if (!strcmp_P(s, PSTR("up_calibration")))
- return AA_UP_CALIBRATION_L;
- if (!strcmp_P(s, PSTR("torque_limit")))
- return AA_TORQUE_LIMIT_L;
- if (!strcmp_P(s, PSTR("position")))
- return AA_PRESENT_POSITION_L;
- if (!strcmp_P(s, PSTR("speed")))
- return AA_PRESENT_SPEED_L;
- if (!strcmp_P(s, PSTR("load")))
- return AA_PRESENT_LOAD_L;
- if (!strcmp_P(s, PSTR("moving_speed")))
- return AA_MOVING_SPEED_L;
- if (!strcmp_P(s, PSTR("model")))
- return AA_MODEL_NUMBER_L;
- if (!strcmp_P(s, PSTR("goal_pos")))
- return AA_GOAL_POSITION_L;
- if (!strcmp_P(s, PSTR("punch")))
- return AA_PUNCH_L;
-
- /* 8 bits */
- if (!strcmp_P(s, PSTR("firmware")))
- return AA_FIRMWARE;
- if (!strcmp_P(s, PSTR("id")))
- return AA_ID;
- if (!strcmp_P(s, PSTR("baudrate")))
- return AA_BAUD_RATE;
- if (!strcmp_P(s, PSTR("delay")))
- return AA_DELAY_TIME;
- if (!strcmp_P(s, PSTR("high_lim_temp")))
- return AA_HIGHEST_LIMIT_TEMP;
- if (!strcmp_P(s, PSTR("low_lim_volt")))
- return AA_LOWEST_LIMIT_VOLTAGE;
- if (!strcmp_P(s, PSTR("high_lim_volt")))
- return AA_HIGHEST_LIMIT_VOLTAGE;
- if (!strcmp_P(s, PSTR("status_return")))
- return AA_STATUS_RETURN_LEVEL;
- if (!strcmp_P(s, PSTR("alarm_led")))
- return AA_ALARM_LED;
- if (!strcmp_P(s, PSTR("alarm_shutdown")))
- return AA_ALARM_SHUTDOWN;
- if (!strcmp_P(s, PSTR("torque_enable")))
- return AA_TORQUE_ENABLE;
- if (!strcmp_P(s, PSTR("led")))
- return AA_LED;
- if (!strcmp_P(s, PSTR("cw_comp_margin")))
- return AA_CW_COMPLIANCE_MARGIN;
- if (!strcmp_P(s, PSTR("ccw_comp_margin")))
- return AA_CCW_COMPLIANCE_MARGIN;
- if (!strcmp_P(s, PSTR("cw_comp_slope")))
- return AA_CW_COMPLIANCE_SLOPE;
- if (!strcmp_P(s, PSTR("ccw_comp_slope")))
- return AA_CCW_COMPLIANCE_SLOPE;
- if (!strcmp_P(s, PSTR("voltage")))
- return AA_PRESENT_VOLTAGE;
- if (!strcmp_P(s, PSTR("temp")))
- return AA_PRESENT_TEMP;
- if (!strcmp_P(s, PSTR("reginst")))
- return AA_PRESENT_REGINST;
- if (!strcmp_P(s, PSTR("moving")))
- return AA_MOVING;
- if (!strcmp_P(s, PSTR("lock")))
- return AA_LOCK;
-
- return 0;
-}
-
-/**********************************************************/
-/* Ax12_Stress */
-
-/* this structure is filled when cmd_ax12_stress is parsed successfully */
-struct cmd_ax12_stress_result {
- fixed_string_t arg0;
- uint8_t id;
-};
-
-/* function called when cmd_ax12_stress is parsed successfully */
-static void cmd_ax12_stress_parsed(void *parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_ax12_stress_result *res = parsed_result;
- int i, nb_errs, id;
- uint8_t val;
- microseconds t;
-
- t = time_get_us2();
- nb_errs = 0;
- for (i=0; i<1000; i++) {
- if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
- nb_errs ++;
- }
-
- printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
- t = (time_get_us2() - t) / 1000;
- printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
-
- t = time_get_us2();
- nb_errs = 0;
- for (i=0; i<1000; i++) {
- if (AX12_write_int(&gen.ax12, res->id, AA_GOAL_POSITION_L, 500))
- nb_errs ++;
- }
-
- printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
- t = (time_get_us2() - t) / 1000;
- printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
-
- /* test 5 servos */
- t = time_get_us2();
- nb_errs = 0;
- id = 1;
- for (i=0; i<100; i++) {
- if (AX12_write_int(&gen.ax12, id, AA_GOAL_POSITION_L, 500))
- nb_errs ++;
- id ++;
- if (id > 5)
- id = 1;
- }
-
- printf_P(PSTR("%d errors / 100\r\n"), nb_errs);
- t = (time_get_us2() - t) / 1000;
- printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
-
-
-}
-
-prog_char str_ax12_stress_arg0[] = "ax12_stress";
-parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
-parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
-
-prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands (id)";
-parse_pgm_inst_t cmd_ax12_stress = {
- .f = cmd_ax12_stress_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_ax12_stress,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_ax12_stress_arg0,
- (prog_void *)&cmd_ax12_stress_id,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Ax12_Dump_Stats */
-
-/* this structure is filled when cmd_ax12_dump_stats is parsed successfully */
-struct cmd_ax12_dump_stats_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_ax12_dump_stats is parsed successfully */
-static void cmd_ax12_dump_stats_parsed(__attribute__((unused)) void *parsed_result,
- __attribute__((unused)) void *data)
-{
- ax12_dump_stats();
-}
-
-prog_char str_ax12_dump_stats_arg0[] = "ax12_dump_stats";
-parse_pgm_token_string_t cmd_ax12_dump_stats_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_dump_stats_result, arg0, str_ax12_dump_stats_arg0);
-
-prog_char help_ax12_dump_stats[] = "Dump AX12 stats";
-parse_pgm_inst_t cmd_ax12_dump_stats = {
- .f = cmd_ax12_dump_stats_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_ax12_dump_stats,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_ax12_dump_stats_arg0,
- NULL,
- },
-};
-
-/**********************************************************/
-
-/* this structure is filled when cmd_baudrate is parsed successfully */
-struct cmd_baudrate_result {
- fixed_string_t arg0;
- uint32_t arg1;
-};
-
-/* function called when cmd_baudrate is parsed successfully */
-static void cmd_baudrate_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_baudrate_result *res = parsed_result;
- struct uart_config c;
-
- uart_getconf(UART_AX12_NUM, &c);
- c.baudrate = res->arg1;
- uart_setconf(UART_AX12_NUM, &c);
-}
-
-prog_char str_baudrate_arg0[] = "baudrate";
-parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
-parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
-
-prog_char help_baudrate[] = "Change ax12 baudrate";
-parse_pgm_inst_t cmd_baudrate = {
- .f = cmd_baudrate_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_baudrate,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_baudrate_arg0,
- (prog_void *)&cmd_baudrate_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Uint16 */
-
-
-/* this structure is filled when cmd_uint16_read is parsed successfully */
-struct cmd_uint16_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t num;
- uint16_t val;
-};
-
-/* function called when cmd_uint16_read is parsed successfully */
-static void cmd_uint16_read_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_uint16_result *res = parsed_result;
- uint8_t ret;
- uint16_t val;
- uint8_t addr = addr_from_string(res->arg1);
- ret = ax12_user_read_int(&gen.ax12, res->num, addr, &val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
- printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
-}
-
-prog_char str_uint16_arg0[] = "read";
-parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
-prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
- "max_torque#down_calibration#up_calibration#torque_limit#"
- "position#speed#load#punch";
-parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
-parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
-
-prog_char help_uint16_read[] = "Read uint16 value (type, num)";
-parse_pgm_inst_t cmd_uint16_read = {
- .f = cmd_uint16_read_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint16_read,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint16_arg0,
- (prog_void *)&cmd_uint16_arg1,
- (prog_void *)&cmd_uint16_num,
- NULL,
- },
-};
-
-/* function called when cmd_uint16_write is parsed successfully */
-static void cmd_uint16_write_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_uint16_result *res = parsed_result;
- uint8_t ret;
- uint8_t addr = addr_from_string(res->arg1);
- printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
- res->val, res->val);
- ret = ax12_user_write_int(&gen.ax12, res->num, addr, res->val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
-}
-
-prog_char str_uint16_arg0_w[] = "write";
-parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
-prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
- "max_torque#torque_limit#punch";
-parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
-parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
-
-prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
-parse_pgm_inst_t cmd_uint16_write = {
- .f = cmd_uint16_write_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint16_write,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint16_arg0_w,
- (prog_void *)&cmd_uint16_arg1_w,
- (prog_void *)&cmd_uint16_num,
- (prog_void *)&cmd_uint16_val,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Uint8 */
-
-
-/* this structure is filled when cmd_uint8_read is parsed successfully */
-struct cmd_uint8_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t num;
- uint8_t val;
-};
-
-/* function called when cmd_uint8_read is parsed successfully */
-static void cmd_uint8_read_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_uint8_result *res = parsed_result;
- uint8_t ret;
- uint8_t val;
- uint8_t addr = addr_from_string(res->arg1);
-
- ret = ax12_user_read_byte(&gen.ax12, res->num, addr, &val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
- printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
-}
-
-prog_char str_uint8_arg0[] = "read";
-parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
-prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
- "low_lim_volt#high_lim_volt#status_return#alarm_led#"
- "alarm_shutdown#torque_enable#led#cw_comp_margin#"
- "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
- "voltage#temp#reginst#moving#lock";
-parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
-parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
-
-prog_char help_uint8_read[] = "Read uint8 value (type, num)";
-parse_pgm_inst_t cmd_uint8_read = {
- .f = cmd_uint8_read_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint8_read,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint8_arg0,
- (prog_void *)&cmd_uint8_arg1,
- (prog_void *)&cmd_uint8_num,
- NULL,
- },
-};
-
-/* function called when cmd_uint8_write is parsed successfully */
-static void cmd_uint8_write_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_uint8_result *res = parsed_result;
- uint8_t addr = addr_from_string(res->arg1);
- uint8_t ret;
- printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
- res->val, res->val);
- ret = ax12_user_write_byte(&gen.ax12, res->num, addr, res->val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
-}
-
-prog_char str_uint8_arg0_w[] = "write";
-parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
-prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
- "low_lim_volt#high_lim_volt#status_return#alarm_led#"
- "alarm_shutdown#torque_enable#led#cw_comp_margin#"
- "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
- "reginst#lock";
-parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
-parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
-
-prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
-parse_pgm_inst_t cmd_uint8_write = {
- .f = cmd_uint8_write_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint8_write,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint8_arg0_w,
- (prog_void *)&cmd_uint8_arg1_w,
- (prog_void *)&cmd_uint8_num,
- (prog_void *)&cmd_uint8_val,
- NULL,
- },
-};
+++ /dev/null
-/*
- * Copyright Droids Corporation (2008)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands_cs.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "main.h"
-#include "cs.h"
-#include "cmdline.h"
-
-struct csb_list {
- const prog_char *name;
- struct cs_block *csb;
-};
-
-prog_char csb_left_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,
- },
-};
-
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2008)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands_gen.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <encoders_spi.h>
-#include <adc.h>
-
-#include <scheduler.h>
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include <diagnostic.h>
-
-#include "main.h"
-#include "cmdline.h"
-#include "sensor.h"
-
-/**********************************************************/
-/* Reset */
-
-/* this structure is filled when cmd_reset is parsed successfully */
-struct cmd_reset_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_reset is parsed successfully */
-static void cmd_reset_parsed(__attribute__((unused)) void *parsed_result,
- __attribute__((unused)) void *data)
-{
- reset();
-}
-
-prog_char str_reset_arg0[] = "reset";
-parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
-
-prog_char help_reset[] = "Reset the board";
-parse_pgm_inst_t cmd_reset = {
- .f = cmd_reset_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_reset,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_reset_arg0,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Bootloader */
-
-/* this structure is filled when cmd_bootloader is parsed successfully */
-struct cmd_bootloader_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_bootloader is parsed successfully */
-static void cmd_bootloader_parsed(__attribute__((unused)) void *parsed_result,
- __attribute__((unused)) void *data)
-{
- bootloader();
-}
-
-prog_char str_bootloader_arg0[] = "bootloader";
-parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
-
-prog_char help_bootloader[] = "Launch the bootloader";
-parse_pgm_inst_t cmd_bootloader = {
- .f = cmd_bootloader_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_bootloader,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_bootloader_arg0,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Encoders tests */
-
-/* this structure is filled when cmd_encoders is parsed successfully */
-struct cmd_encoders_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_encoders is parsed successfully */
-static void cmd_encoders_parsed(void *parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_encoders_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("reset"))) {
- encoders_spi_set_value((void *)0, 0);
- encoders_spi_set_value((void *)1, 0);
- encoders_spi_set_value((void *)2, 0);
- encoders_spi_set_value((void *)3, 0);
- return;
- }
-
- /* show */
- while(!cmdline_keypressed()) {
- printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
- encoders_spi_get_value((void *)0),
- encoders_spi_get_value((void *)1),
- encoders_spi_get_value((void *)2),
- encoders_spi_get_value((void *)3));
- wait_ms(100);
- }
-}
-
-prog_char str_encoders_arg0[] = "encoders";
-parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
-prog_char str_encoders_arg1[] = "show#reset";
-parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
-
-prog_char help_encoders[] = "Show encoders values";
-parse_pgm_inst_t cmd_encoders = {
- .f = cmd_encoders_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_encoders,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_encoders_arg0,
- (prog_void *)&cmd_encoders_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Scheduler show */
-
-/* this structure is filled when cmd_scheduler is parsed successfully */
-struct cmd_scheduler_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_scheduler is parsed successfully */
-static void cmd_scheduler_parsed( __attribute__((unused)) void *parsed_result,
- __attribute__((unused)) void *data)
-{
- scheduler_dump_events();
-}
-
-prog_char str_scheduler_arg0[] = "scheduler";
-parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
-prog_char str_scheduler_arg1[] = "show";
-parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
-
-prog_char help_scheduler[] = "Show scheduler events";
-parse_pgm_inst_t cmd_scheduler = {
- .f = cmd_scheduler_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_scheduler,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_scheduler_arg0,
- (prog_void *)&cmd_scheduler_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Pwms tests */
-
-/* this structure is filled when cmd_pwm is parsed successfully */
-struct cmd_pwm_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- int16_t arg2;
-};
-
-/* function called when cmd_pwm is parsed successfully */
-static void cmd_pwm_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- void * pwm_ptr = NULL;
- struct cmd_pwm_result * res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("1(4A)")))
- pwm_ptr = &gen.pwm1_4A;
- else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
- pwm_ptr = &gen.pwm2_4B;
- else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
- pwm_ptr = &gen.pwm3_1A;
- else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
- pwm_ptr = &gen.pwm4_1B;
-
- else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
- pwm_ptr = &gen.servo1;
- else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
- pwm_ptr = &gen.servo2;
- else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
- pwm_ptr = &gen.servo3;
- else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
- pwm_ptr = &gen.servo4;
-
- if (pwm_ptr)
- pwm_ng_set(pwm_ptr, res->arg2);
-
- printf_P(PSTR("done\r\n"));
-}
-
-prog_char str_pwm_arg0[] = "pwm";
-parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
-prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)";
-parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
-parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
-
-prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
-parse_pgm_inst_t cmd_pwm = {
- .f = cmd_pwm_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_pwm,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pwm_arg0,
- (prog_void *)&cmd_pwm_arg1,
- (prog_void *)&cmd_pwm_arg2,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Adcs tests */
-
-/* this structure is filled when cmd_adc is parsed successfully */
-struct cmd_adc_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_adc is parsed successfully */
-static void cmd_adc_parsed(void *parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_adc_result *res = parsed_result;
- uint8_t i, loop = 0;
-
- if (!strcmp_P(res->arg1, PSTR("loop_show")))
- loop = 1;
-
- do {
- printf_P(PSTR("ADC values: "));
- for (i=0; i<ADC_MAX; i++) {
- printf_P(PSTR("%.4d "), sensor_get_adc(i));
- }
- printf_P(PSTR("\r\n"));
- wait_ms(100);
- } while (loop && !cmdline_keypressed());
-}
-
-prog_char str_adc_arg0[] = "adc";
-parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
-prog_char str_adc_arg1[] = "show#loop_show";
-parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
-
-prog_char help_adc[] = "Show adc values";
-parse_pgm_inst_t cmd_adc = {
- .f = cmd_adc_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_adc,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_adc_arg0,
- (prog_void *)&cmd_adc_arg1,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Sensors tests */
-
-/* this structure is filled when cmd_sensor is parsed successfully */
-struct cmd_sensor_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_sensor is parsed successfully */
-static void cmd_sensor_parsed(void *parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_sensor_result *res = parsed_result;
- uint8_t i, loop = 0;
-
- if (!strcmp_P(res->arg1, PSTR("loop_show")))
- loop = 1;
-
- do {
- printf_P(PSTR("SENSOR values: "));
- for (i=0; i<SENSOR_MAX; i++) {
- printf_P(PSTR("%d "), !!sensor_get(i));
- }
- printf_P(PSTR("\r\n"));
- wait_ms(100);
- } while (loop && !cmdline_keypressed());
-}
-
-prog_char str_sensor_arg0[] = "sensor";
-parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
-prog_char str_sensor_arg1[] = "show#loop_show";
-parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
-
-prog_char help_sensor[] = "Show sensor values";
-parse_pgm_inst_t cmd_sensor = {
- .f = cmd_sensor_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_sensor,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_sensor_arg0,
- (prog_void *)&cmd_sensor_arg1,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Log */
-
-/* this structure is filled when cmd_log is parsed successfully */
-struct cmd_log_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t arg2;
- fixed_string_t arg3;
-};
-
-/* keep it sync with string choice */
-static const prog_char uart_log[] = "uart";
-static const prog_char i2c_log[] = "i2c";
-static const prog_char i2cproto_log[] = "i2cproto";
-static const prog_char sensor_log[] = "sensor";
-static const prog_char block_log[] = "bd";
-static const prog_char arm_log[] = "arm";
-static const prog_char finger_log[] = "finger";
-static const prog_char state_log[] = "state";
-static const prog_char ax12_log[] = "ax12";
-
-struct log_name_and_num {
- const prog_char * name;
- uint8_t num;
-};
-
-static const struct log_name_and_num log_name_and_num[] = {
- { uart_log, E_UART },
- { i2c_log, E_I2C },
- { i2cproto_log, E_USER_I2C_PROTO },
- { sensor_log, E_USER_SENSOR },
- { block_log, E_BLOCKING_DETECTION_MANAGER },
- { arm_log, E_USER_ARM },
- { finger_log, E_USER_FINGER },
- { state_log, E_USER_ST_MACH },
- { ax12_log, E_USER_AX12 },
-};
-
-static uint8_t
-log_name2num(const char * s)
-{
- uint8_t i;
-
- for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
- if (!strcmp_P(s, log_name_and_num[i].name)) {
- return log_name_and_num[i].num;
- }
- }
- return 0;
-}
-
-const prog_char *
-log_num2name(uint8_t num)
-{
- uint8_t i;
-
- for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
- if (num == log_name_and_num[i].num) {
- return log_name_and_num[i].name;
- }
- }
- return NULL;
-}
-
-/* function called when cmd_log is parsed successfully */
-static void cmd_log_do_show(void)
-{
- uint8_t i, empty=1;
- const prog_char * name;
-
- printf_P(PSTR("log level is %d\r\n"), gen.log_level);
- for (i=0; i<NB_LOGS; i++) {
- name = log_num2name(gen.logs[i]);
- if (name) {
- printf_P(PSTR("log type %S is on\r\n"), name);
- empty = 0;
- }
- }
- if (empty)
- printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
-}
-
-/* function called when cmd_log is parsed successfully */
-static void cmd_log_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("level"))) {
- gen.log_level = res->arg2;
- }
-
- /* else it is a show */
- cmd_log_do_show();
-}
-
-prog_char str_log_arg0[] = "log";
-parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
-prog_char str_log_arg1[] = "level";
-parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
-parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
-
-prog_char help_log[] = "Set log options: level (0 -> 5)";
-parse_pgm_inst_t cmd_log = {
- .f = cmd_log_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_log,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_log_arg0,
- (prog_void *)&cmd_log_arg1,
- (prog_void *)&cmd_log_arg2,
- NULL,
- },
-};
-
-prog_char str_log_arg1_show[] = "show";
-parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
-
-prog_char help_log_show[] = "Show configured logs";
-parse_pgm_inst_t cmd_log_show = {
- .f = cmd_log_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_log_show,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_log_arg0,
- (prog_void *)&cmd_log_arg1_show,
- NULL,
- },
-};
-
-/* this structure is filled when cmd_log is parsed successfully */
-struct cmd_log_type_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- fixed_string_t arg2;
- fixed_string_t arg3;
-};
-
-/* function called when cmd_log is parsed successfully */
-static void cmd_log_type_parsed(void * parsed_result, __attribute__((unused)) void *data)
-{
- struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
- uint8_t lognum;
- uint8_t i;
-
- lognum = log_name2num(res->arg2);
- if (lognum == 0) {
- printf_P(PSTR("Cannot find log num\r\n"));
- return;
- }
-
- if (!strcmp_P(res->arg3, PSTR("on"))) {
- for (i=0; i<NB_LOGS; i++) {
- if (gen.logs[i] == lognum) {
- printf_P(PSTR("Already on\r\n"));
- return;
- }
- }
- for (i=0; i<NB_LOGS; i++) {
- if (gen.logs[i] == 0) {
- gen.logs[i] = lognum;
- break;
- }
- }
- if (i==NB_LOGS) {
- printf_P(PSTR("no more room\r\n"));
- }
- }
- else if (!strcmp_P(res->arg3, PSTR("off"))) {
- for (i=0; i<NB_LOGS; i++) {
- if (gen.logs[i] == lognum) {
- gen.logs[i] = 0;
- break;
- }
- }
- if (i==NB_LOGS) {
- printf_P(PSTR("already off\r\n"));
- }
- }
- cmd_log_do_show();
-}
-
-prog_char str_log_arg1_type[] = "type";
-parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
-/* keep it sync with log_name_and_num above */
-prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#arm#finger#state#ax12";
-parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
-prog_char str_log_arg3[] = "on#off";
-parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
-
-prog_char help_log_type[] = "Set log type";
-parse_pgm_inst_t cmd_log_type = {
- .f = cmd_log_type_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_log_type,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_log_arg0,
- (prog_void *)&cmd_log_arg1_type,
- (prog_void *)&cmd_log_arg2_type,
- (prog_void *)&cmd_log_arg3,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Stack_Space */
-
-/* this structure is filled when cmd_stack_space is parsed successfully */
-struct cmd_stack_space_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_stack_space is parsed successfully */
-static void cmd_stack_space_parsed(__attribute__((unused)) void *parsed_result,
- __attribute__((unused)) void *data)
-{
- printf("res stack: %d\r\n", min_stack_space_available());
-
-}
-
-prog_char str_stack_space_arg0[] = "stack_space";
-parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
-
-prog_char help_stack_space[] = "Display remaining stack space";
-parse_pgm_inst_t cmd_stack_space = {
- .f = cmd_stack_space_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_stack_space,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_stack_space_arg0,
- NULL,
- },
-};
+++ /dev/null
-/*
- * 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_mechboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "../common/i2c_commands.h"
-#include "main.h"
-#include "sensor.h"
-#include "cmdline.h"
-#include "state.h"
-#include "i2c_protocol.h"
-#include "actuator.h"
-#include "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,
- },
-};
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cs.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <spi.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-#include <adc.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <parse.h>
-#include <rdline.h>
-
-#include "main.h"
-#include "actuator.h"
-
-/* called every 5 ms */
-static void do_cs(__attribute__((unused)) void *dummy)
-{
- /* read encoders */
- if (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);
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cs.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $
- *
- */
-
-void microb_cs_init(void);
-void dump_cs(const char *name, struct cs *cs);
-void dump_pid(const char *name, struct pid_filter *pid);
+++ /dev/null
-/*
- * 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_
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: i2c_protocol.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
- *
- */
-
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/list.h>
-#include <aversive/error.h>
-
-#include <i2c.h>
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "../common/i2c_commands.h"
-#include "main.h"
-#include "sensor.h"
-#include "state.h"
-
-void i2c_protocol_init(void)
-{
-}
-
-/*** LED CONTROL ***/
-void i2c_led_control(uint8_t l, uint8_t state)
-{
- switch(l) {
- case 1:
- state? LED1_ON():LED1_OFF();
- break;
- case 2:
- state? LED2_ON():LED2_OFF();
- break;
- case 3:
- state? LED3_ON():LED3_OFF();
- break;
- case 4:
- state? LED4_ON():LED4_OFF();
- break;
- default:
- break;
- }
-}
-
-#ifdef notyet
-static void i2c_test(uint16_t val)
-{
- static uint16_t prev=0;
-
- if ( (val-prev) != 1 ) {
- WARNING(E_USER_I2C_PROTO, "Dupplicated msg %d", val);
- }
- prev = val;
- ext.nb_test_cmd ++;
-}
-#endif
-
-static void i2c_send_status(void)
-{
- struct i2c_ans_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)
-{
-}
-
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2007)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: i2c_protocol.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $
- *
- */
-
-#include <aversive.h>
-
-void i2c_protocol_init(void);
-
-void i2c_recvevent(uint8_t * buf, int8_t size);
-void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
-void i2c_sendevent(int8_t size);
-
-int debug_send(char c, FILE* f);
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: main.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-#include <avr/eeprom.h>
-
-#include <aversive.h>
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <spi.h>
-#include <i2c.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-#include <adc.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <parse.h>
-#include <rdline.h>
-
-#include "../common/eeprom_mapping.h"
-#include "../common/i2c_commands.h"
-
-#include "main.h"
-#include "ax12_user.h"
-#include "cmdline.h"
-#include "sensor.h"
-#include "state.h"
-#include "actuator.h"
-#include "arm_xy.h"
-#include "cs.h"
-#include "i2c_protocol.h"
-
-/* 0 means "programmed"
- * ---- with 16 Mhz quartz
- * CKSEL 3-0 : 0111
- * SUT 1-0 : 10
- * CKDIV8 : 1
- * ---- bootloader
- * BOOTZ 1-0 : 01 (4K bootloader)
- * BOOTRST : 0 (reset on bootloader)
- * ---- jtag
- * jtagen : 0
- */
-
-struct genboard gen;
-struct 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;
-}
+++ /dev/null
-/*
- * 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; \
-})
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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_
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- * Olivier MATZ <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: sensor.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
- *
- */
-
-#include <stdlib.h>
-
-#include <aversive.h>
-#include <aversive/error.h>
-
-#include <adc.h>
-#include <scheduler.h>
-#include <ax12.h>
-#include <pwm_ng.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <parse.h>
-#include <rdline.h>
-
-#include "main.h"
-#include "sensor.h"
-
-/************ ADC */
-
-struct adc_infos {
- uint16_t config;
- int16_t value;
- int16_t prev_val;
- int16_t (*filter)(struct adc_infos *, int16_t);
-};
-
-/* reach 90% of the value in 4 samples */
-int16_t rii_light(struct adc_infos *adc, int16_t val)
-{
- adc->prev_val = val + (int32_t)adc->prev_val / 2;
- return adc->prev_val / 2;
-}
-
-/* reach 90% of the value in 8 samples */
-int16_t rii_medium(struct adc_infos *adc, int16_t val)
-{
- adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4;
- return adc->prev_val / 4;
-}
-
-/* reach 90% of the value in 16 samples */
-int16_t rii_strong(struct adc_infos *adc, int16_t val)
-{
- adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8;
- return adc->prev_val / 8;
-}
-
-
-#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
-
-/* define which ADC to poll, see in sensor.h */
-static struct adc_infos adc_infos[ADC_MAX] = {
- [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
- [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
- [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
- [ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium },
-
- /* add adc on "cap" pins if needed */
-/* [ADC_CAP1] = { .config = ADC_CONF(10) }, */
-/* [ADC_CAP2] = { .config = ADC_CONF(11) }, */
-/* [ADC_CAP3] = { .config = ADC_CONF(12) }, */
-/* [ADC_CAP4] = { .config = ADC_CONF(13) }, */
-};
-
-static void adc_event(int16_t result);
-
-/* called every 10 ms, see init below */
-static void do_adc(__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_MAX; i++) {
- if ((1 << i) & sensor) {
- if (sensor_filter[i].cpt < sensor_filter[i].filter)
- sensor_filter[i].cpt++;
- if (sensor_filter[i].cpt >= sensor_filter[i].thres_on)
- sensor_filter[i].prev = 1;
- }
- else {
- if (sensor_filter[i].cpt > 0)
- sensor_filter[i].cpt--;
- if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
- sensor_filter[i].prev = 0;
- }
-
- if (sensor_filter[i].prev && !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);
-}
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- * Olivier MATZ <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: sensor.h,v 1.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);
+++ /dev/null
-/*
- * 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
-
+++ /dev/null
-/*
- * 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 <math.h>
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <spi.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <vt100.h>
-
-#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);
-}
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/* \r
- * Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
- * \r
- * This program is free software; you can redistribute it and/or modify\r
- * it under the terms of the GNU General Public License as published by\r
- * the Free Software Foundation; either version 2 of the License, or\r
- * (at your option) any later version.\r
- *\r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- *\r
- * You should have received a copy of the GNU General Public License\r
- * along with this program; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- *\r
- * Revision : $Id: uart_config.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $\r
- *\r
- */\r
-\r
-/* Droids-corp 2004 - Zer0\r
- * config for uart module\r
- */\r
-\r
-#ifndef UART_CONFIG_H\r
-#define UART_CONFIG_H\r
-\r
-/* compile uart0 fonctions, undefine it to pass compilation */\r
-#define UART0_COMPILE \r
-#define UART0_ENABLED 1\r
-#define UART0_INTERRUPT_ENABLED 1\r
-#define UART0_BAUDRATE 57600\r
-//#define UART0_BAUDRATE 1000000\r
-#define UART0_USE_DOUBLE_SPEED 1\r
-#define UART0_RX_FIFO_SIZE 32\r
-#define UART0_TX_FIFO_SIZE 32\r
-#define UART0_NBITS 8\r
-#define UART0_PARITY UART_PARTITY_NONE\r
-#define UART0_STOP_BIT UART_STOP_BITS_2\r
-\r
-#define UART1_COMPILE \r
-#define UART1_ENABLED 1\r
-#define UART1_INTERRUPT_ENABLED 1\r
-#define UART1_BAUDRATE 57600\r
-#define UART1_USE_DOUBLE_SPEED 1\r
-#define UART1_RX_FIFO_SIZE 128\r
-#define UART1_TX_FIFO_SIZE 128\r
-#define UART1_NBITS 8\r
-#define UART1_PARITY UART_PARTITY_NONE\r
-#define UART1_STOP_BIT UART_STOP_BITS_1\r
-\r
-#define UART3_COMPILE \r
-#define UART3_ENABLED 1\r
-#define UART3_INTERRUPT_ENABLED 1\r
-#define UART3_BAUDRATE 57600\r
-#define UART3_USE_DOUBLE_SPEED 1\r
-#define UART3_RX_FIFO_SIZE 128\r
-#define UART3_TX_FIFO_SIZE 128\r
-#define UART3_NBITS 8\r
-#define UART3_PARITY UART_PARTITY_NONE\r
-#define UART3_STOP_BIT UART_STOP_BITS_1\r
-\r
-/* .... same for uart 1, 2, 3 ... */\r
-\r
-#endif\r
-\r
+++ /dev/null
-#
-# 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
+++ /dev/null
-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) ;\
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: actuator.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $
- *
- */
-
-#include <aversive.h>
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <spi.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-
-#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);
-}
-
+++ /dev/null
-/*
- * 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);
-
-
+++ /dev/null
-#ifndef _AX12_CONFIG_H_
-#define _AX12_CONFIG_H_
-
-#define AX12_MAX_PARAMS 32
-
-
-#endif/*_AX12_CONFIG_H_*/
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: ax12_user.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $
- *
- */
-
-#include <aversive.h>
-#include <aversive/list.h>
-#include <aversive/error.h>
-
-#include <i2c.h>
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "main.h"
-
-/*
- * Cmdline interface for AX12. Use the PC to command a daisy-chain of
- * AX12 actuators with a nice command line interface.
- *
- * The circuit should be as following:
- *
- * |----------|
- * | uart3|------->--- PC (baudrate=57600)
- * | |-------<---
- * | atmega128|
- * | |
- * | uart0|---->---+-- AX12 (baudrate 115200)
- * | |----<---|
- * |----------|
- *
- * Note that RX and TX pins of UART1 are connected together to provide
- * a half-duplex UART emulation.
- *
- */
-
-#define UART_AX12_NUM 0
-#define UCSRxB UCSR0B
-#define AX12_TIMEOUT 5000UL /* in us */
-
-/********************************* AX12 commands */
-
-/*
- * We use synchronous access (not interrupt driven) to the hardware
- * UART, because we have to be sure that the transmission/reception is
- * really finished when we return from the functions.
- *
- * We don't use the CM-5 circuit as described in the AX12
- * documentation, we simply connect TX and RX and use TXEN + RXEN +
- * DDR to manage the port directions.
- */
-
-static volatile uint8_t ax12_state = AX12_STATE_READ;
-extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
-static volatile uint8_t ax12_nsent = 0;
-
-/* Called by ax12 module to send a character on serial line. Count the
- * number of transmitted bytes. It will be used in ax12_recv_char() to
- * drop the bytes that we transmitted. */
-static int8_t ax12_send_char(uint8_t c)
-{
- uart_send(UART_AX12_NUM, c);
- ax12_nsent++;
- return 0;
-}
-
-/* for atmega256 */
-#ifndef TXEN
-#define TXEN TXEN0
-#endif
-
-/* called by uart module when the character has been written in
- * UDR. It does not mean that the byte is physically transmitted. */
-static void ax12_send_callback(char c)
-{
- if (ax12_state == AX12_STATE_READ) {
- /* disable TX when last byte is pushed. */
- if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
- UCSRxB &= ~(1<<TXEN);
- }
-}
-
-/* Called by ax12 module when we want to receive a char. Note that we
- * also receive the bytes we sent ! So we need to drop them. */
-static int16_t ax12_recv_char(void)
-{
- microseconds t = time_get_us2();
- int c;
- while (1) {
- c = uart_recv_nowait(UART_AX12_NUM);
- if (c != -1) {
- if (ax12_nsent == 0)
- return c;
- ax12_nsent --;
- }
-
- /* 5 ms timeout */
- if ((time_get_us2() - t) > AX12_TIMEOUT)
- return -1;
- }
- return c;
-}
-
-/* called by ax12 module when we want to switch serial line. As we
- * work in interruption mode, this function can be called to switch
- * back in read mode even if the bytes are not really transmitted on
- * the line. That's why in this case we do nothing, we will fall back
- * in read mode in any case when xmit is finished -- see in
- * ax12_send_callback() -- */
-static void ax12_switch_uart(uint8_t state)
-{
- uint8_t flags;
-
- if (state == AX12_STATE_WRITE) {
- IRQ_LOCK(flags);
- ax12_nsent=0;
- while (uart_recv_nowait(UART_AX12_NUM) != -1);
- UCSRxB |= (1<<TXEN);
- ax12_state = AX12_STATE_WRITE;
- IRQ_UNLOCK(flags);
- }
- else {
- IRQ_LOCK(flags);
- if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
- UCSRxB &= ~(1<<TXEN);
- ax12_state = AX12_STATE_READ;
- IRQ_UNLOCK(flags);
- }
-}
-
-
-void ax12_user_init(void)
-{
- /* AX12 */
- AX12_init(&gen.ax12);
- AX12_set_hardware_send(&gen.ax12, ax12_send_char);
- AX12_set_hardware_recv(&gen.ax12, ax12_recv_char);
- AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart);
- uart_register_tx_event(UART_AX12_NUM, ax12_send_callback);
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: ax12_user.h,v 1.2 2009-04-07 20:03:48 zer0 Exp $
- *
- */
-
-/* This is the ax12 user interface. It initializes the aversive AX12
- * module so that it works in background, using interrupt driver uart.
- *
- * Be carreful, a call to AX12 module is synchronous and uses
- * interruptions, so interrupts must be enabled. On the other side, a
- * call _must not_ interrupt another one. That's why all calls to the
- * module are done either in init() functions or in a scheduler event
- * with prio=ARM_PRIO.
- */
-
-/* XXX do a safe_ax12() function that will retry once or twice if we
- * see some problems. */
-
-void ax12_user_init(void);
+++ /dev/null
-
-#include <stdio.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive.h>
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <uart.h>
-#include <i2c.h>
-#include <ax12.h>
-#include <parse.h>
-#include <rdline.h>
-#include <pwm_ng.h>
-#include <encoders_spi.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <pid.h>
-#include <time.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <adc.h>
-#include <spi.h>
-
-#include <blocking_detection_manager.h>
-
-#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<BEACON_MAX_SAMPLE;i++)
- beacon_sample_size[i] = 0;*/
-
- /* set external interrupt (any edge) */
- PCMSK2 = (1<<PCINT18);
- PCICR = (1<<PCIE2);
-
-
-}
-
-void beacon_calibre_pos(void)
-{
- sensorboard.flags &= ~DO_CS;
-
- /* init beacon pos */
- pwm_ng_set(BEACON_PWM, 100);
-
- /* find rising edge of the mirror*/
- wait_ms(100);
- while (sensor_get(BEACON_POS_SENSOR));
- wait_ms(100);
- while (!sensor_get(BEACON_POS_SENSOR));
-
- pwm_ng_set(BEACON_PWM, 0);
-
-
- beacon_reset_pos();
- pid_reset(&sensorboard.beacon.pid);
- encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
-
- cs_set_consign(&sensorboard.beacon.cs, 0);
-
- sensorboard.flags |= DO_CS;
-}
-
-void beacon_start(void)
-{
- beacon_reset_pos();
- sensorboard.beacon.on = 1;
- cs_set_consign(&sensorboard.beacon.cs, 600);
-}
-
-void beacon_stop(void)
-{
- sensorboard.beacon.on = 0;
- pwm_ng_set(BEACON_PWM, 0);
-}
-
-void beacon_reset_pos(void)
-{
- pwm_ng_set(BEACON_PWM, 0);
- encoders_spi_set_value(BEACON_ENCODER, 0);
-}
-
-
-
-int32_t encoders_spi_get_beacon_speed(void * dummy)
-{
- return beacon_speed;
-}
-
-
-//port K bit 2
-/* motor speed (top tour) */
-SIGNAL(SIG_PIN_CHANGE2)
-{
- uint8_t flags;
-
- /* rising edge */
- if ( IR_SENSOR()) {
- IRQ_LOCK(flags);
- count = TCNT3;
- rising = beacon_pos;
- count_diff_rising = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
- valid_beacon = 0;
- IRQ_UNLOCK(flags);
-
- }
- /* falling edge */
- else {
- IRQ_LOCK(flags);
- count = TCNT3;
- falling = beacon_pos;
- count_diff_falling = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
- valid_beacon = 1;
- IRQ_UNLOCK(flags);
- }
-}
-
-void beacon_calc(void *dummy)
-{
- static uint8_t a=0;
- static int32_t local_rising, local_falling;
- static int32_t middle;
- static float size = 0;
- int32_t local_angle;
- int32_t local_dist;
-
- int32_t local_count_diff_rising ;
- int32_t local_count_diff_falling ;
- int32_t local_beacon_coeff;
-
- int32_t result_x=0;
- int32_t result_y=0;
- int32_t temp=0;
- int32_t edge=0;
- //int32_t total_size=0;
-
- uint8_t flags;
- //uint8_t i;
- int8_t local_valid;
-
- if(a)
- LED4_ON();
- else
- LED4_OFF();
-
- a = !a;
-
- if (falling == -1){
- /* 0.5 second timeout */
- if (invalid_count < 25)
- invalid_count++;
- else {
- IRQ_LOCK(flags);
- beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
- IRQ_UNLOCK(flags);
- }
- return;
- }
-
- invalid_count = 0;
- IRQ_LOCK(flags);
- local_valid = valid_beacon;
- local_count_diff_rising = count_diff_rising;
- local_count_diff_falling = count_diff_falling ;
- local_rising = rising;
- local_falling = falling;
- local_beacon_coeff = beacon_coeff;
- IRQ_UNLOCK(flags);
-
- if (local_valid){
- invalid_count = 0;
- //BEACON_DEBUG("rising= %ld\t",local_rising);
- //BEACON_DEBUG("falling= %ld\r\n",local_falling);
-
- /* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */
- local_rising = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
- local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
-
- //BEACON_DEBUG("rising1= %ld\t",local_rising);
- //BEACON_DEBUG("falling1= %ld\r\n",local_falling);
-
- //BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising);
- //BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling);
-
- /* if around 360 deg, rising > 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;
-
-}
+++ /dev/null
-/*
- * 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);
-
-
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cmdline.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/error.h>
-
-#include <parse.h>
-#include <rdline.h>
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include "main.h"
-#include "cmdline.h"
-
-
-/******** See in commands.c for the list of commands. */
-extern parse_pgm_ctx_t main_ctx[];
-
-static void write_char(char c)
-{
- uart_send(CMDLINE_UART, c);
-}
-
-static void
-valid_buffer(const char *buf, uint8_t size)
-{
- int8_t ret;
- ret = parse(main_ctx, buf);
- if (ret == PARSE_AMBIGUOUS)
- printf_P(PSTR("Ambiguous command\r\n"));
- else if (ret == PARSE_NOMATCH)
- printf_P(PSTR("Command not found\r\n"));
- else if (ret == PARSE_BAD_ARGS)
- printf_P(PSTR("Bad arguments\r\n"));
-}
-
-static int8_t
-complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
- int16_t *state)
-{
- return complete(main_ctx, buf, state, dstbuf, dstsize);
-}
-
-
-/* sending "pop" on cmdline uart resets the robot */
-void emergency(char c)
-{
- static uint8_t i = 0;
-
- if ((i == 0 && c == 'p') ||
- (i == 1 && c == 'o') ||
- (i == 2 && c == 'p'))
- i++;
- else if ( !(i == 1 && c == 'p') )
- i = 0;
- if (i == 3)
- reset();
-}
-
-/* log function, add a command to configure
- * it dynamically */
-void mylog(struct error * e, ...)
-{
- va_list ap;
- u16 stream_flags = stdout->flags;
- uint8_t i;
- time_h tv;
-
- if (e->severity > ERROR_SEVERITY_ERROR) {
- if (gen.log_level < e->severity)
- return;
-
- for (i=0; i<NB_LOGS+1; i++)
- if (gen.logs[i] == e->err_num)
- break;
- if (i == NB_LOGS+1)
- return;
- }
-
- va_start(ap, e);
- tv = time_get_time();
- printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
- vfprintf_P(stdout, e->text, ap);
- printf_P(PSTR("\r\n"));
- va_end(ap);
- stdout->flags = stream_flags;
-}
-
-int cmdline_interact(void)
-{
- const char *history, *buffer;
- int8_t ret, same = 0;
- int16_t c;
-
- rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
- snprintf(gen.prompt, sizeof(gen.prompt), "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;
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cmdline.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $
- *
- */
-
-#define CMDLINE_UART 1
-
-/* uart rx callback for reset() */
-void emergency(char c);
-
-/* log function */
-void mylog(struct error * e, ...);
-
-/* launch cmdline */
-int cmdline_interact(void);
-
-static inline uint8_t cmdline_keypressed(void) {
- return (uart_recv_nowait(CMDLINE_UART) != -1);
-}
-
-static inline uint8_t cmdline_getchar(void) {
- return uart_recv_nowait(CMDLINE_UART);
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands.c,v 1.2 2009-05-27 20:04:07 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdlib.h>
-#include <aversive/pgmspace.h>
-#include <parse.h>
-
-/* commands_gen.c */
-extern parse_pgm_inst_t cmd_reset;
-extern parse_pgm_inst_t cmd_bootloader;
-extern parse_pgm_inst_t cmd_encoders;
-extern parse_pgm_inst_t cmd_pwm;
-extern parse_pgm_inst_t cmd_adc;
-extern parse_pgm_inst_t cmd_sensor;
-extern parse_pgm_inst_t cmd_log;
-extern parse_pgm_inst_t cmd_log_show;
-extern parse_pgm_inst_t cmd_log_type;
-extern parse_pgm_inst_t cmd_stack_space;
-extern parse_pgm_inst_t cmd_scheduler;
-
-/* commands_ax12.c */
-extern parse_pgm_inst_t cmd_baudrate;
-extern parse_pgm_inst_t cmd_uint16_read;
-extern parse_pgm_inst_t cmd_uint16_write;
-extern parse_pgm_inst_t cmd_uint8_read;
-extern parse_pgm_inst_t cmd_uint8_write;
-
-/* commands_cs.c */
-extern parse_pgm_inst_t cmd_gain;
-extern parse_pgm_inst_t cmd_gain_show;
-extern parse_pgm_inst_t cmd_speed;
-extern parse_pgm_inst_t cmd_speed_show;
-extern parse_pgm_inst_t cmd_derivate_filter;
-extern parse_pgm_inst_t cmd_derivate_filter_show;
-extern parse_pgm_inst_t cmd_consign;
-extern parse_pgm_inst_t cmd_maximum;
-extern parse_pgm_inst_t cmd_maximum_show;
-extern parse_pgm_inst_t cmd_quadramp;
-extern parse_pgm_inst_t cmd_quadramp_show;
-extern parse_pgm_inst_t cmd_cs_status;
-extern parse_pgm_inst_t cmd_blocking_i;
-extern parse_pgm_inst_t cmd_blocking_i_show;
-
-/* commands_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,
-};
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands_ax12.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "main.h"
-
-uint8_t addr_from_string(const char *s)
-{
- /* 16 bits */
- if (!strcmp_P(s, PSTR("cw_angle_limit")))
- return AA_CW_ANGLE_LIMIT_L;
- if (!strcmp_P(s, PSTR("ccw_angle_limit")))
- return AA_CCW_ANGLE_LIMIT_L;
- if (!strcmp_P(s, PSTR("max_torque")))
- return AA_MAX_TORQUE_L;
- if (!strcmp_P(s, PSTR("down_calibration")))
- return AA_DOWN_CALIBRATION_L;
- if (!strcmp_P(s, PSTR("up_calibration")))
- return AA_UP_CALIBRATION_L;
- if (!strcmp_P(s, PSTR("torque_limit")))
- return AA_TORQUE_LIMIT_L;
- if (!strcmp_P(s, PSTR("position")))
- return AA_PRESENT_POSITION_L;
- if (!strcmp_P(s, PSTR("speed")))
- return AA_PRESENT_SPEED_L;
- if (!strcmp_P(s, PSTR("load")))
- return AA_PRESENT_LOAD_L;
- if (!strcmp_P(s, PSTR("moving_speed")))
- return AA_MOVING_SPEED_L;
- if (!strcmp_P(s, PSTR("model")))
- return AA_MODEL_NUMBER_L;
- if (!strcmp_P(s, PSTR("goal_pos")))
- return AA_GOAL_POSITION_L;
- if (!strcmp_P(s, PSTR("punch")))
- return AA_PUNCH_L;
-
- /* 8 bits */
- if (!strcmp_P(s, PSTR("firmware")))
- return AA_FIRMWARE;
- if (!strcmp_P(s, PSTR("id")))
- return AA_ID;
- if (!strcmp_P(s, PSTR("baudrate")))
- return AA_BAUD_RATE;
- if (!strcmp_P(s, PSTR("delay")))
- return AA_DELAY_TIME;
- if (!strcmp_P(s, PSTR("high_lim_temp")))
- return AA_HIGHEST_LIMIT_TEMP;
- if (!strcmp_P(s, PSTR("low_lim_volt")))
- return AA_LOWEST_LIMIT_VOLTAGE;
- if (!strcmp_P(s, PSTR("high_lim_volt")))
- return AA_HIGHEST_LIMIT_VOLTAGE;
- if (!strcmp_P(s, PSTR("status_return")))
- return AA_STATUS_RETURN_LEVEL;
- if (!strcmp_P(s, PSTR("alarm_led")))
- return AA_ALARM_LED;
- if (!strcmp_P(s, PSTR("alarm_shutdown")))
- return AA_ALARM_SHUTDOWN;
- if (!strcmp_P(s, PSTR("torque_enable")))
- return AA_TORQUE_ENABLE;
- if (!strcmp_P(s, PSTR("led")))
- return AA_LED;
- if (!strcmp_P(s, PSTR("cw_comp_margin")))
- return AA_CW_COMPLIANCE_MARGIN;
- if (!strcmp_P(s, PSTR("ccw_comp_margin")))
- return AA_CCW_COMPLIANCE_MARGIN;
- if (!strcmp_P(s, PSTR("cw_comp_slope")))
- return AA_CW_COMPLIANCE_SLOPE;
- if (!strcmp_P(s, PSTR("ccw_comp_slope")))
- return AA_CCW_COMPLIANCE_SLOPE;
- if (!strcmp_P(s, PSTR("voltage")))
- return AA_PRESENT_VOLTAGE;
- if (!strcmp_P(s, PSTR("temp")))
- return AA_PRESENT_TEMP;
- if (!strcmp_P(s, PSTR("reginst")))
- return AA_PRESENT_REGINST;
- if (!strcmp_P(s, PSTR("moving")))
- return AA_MOVING;
- if (!strcmp_P(s, PSTR("lock")))
- return AA_LOCK;
-
- return 0;
-}
-
-/**********************************************************/
-/* Ax12_Stress */
-
-/* this structure is filled when cmd_ax12_stress is parsed successfully */
-struct cmd_ax12_stress_result {
- fixed_string_t arg0;
- uint8_t id;
-};
-
-/* function called when cmd_ax12_stress is parsed successfully */
-static void cmd_ax12_stress_parsed(void *parsed_result, void *data)
-{
- struct cmd_ax12_stress_result *res = parsed_result;
- int i, nb_errs = 0;
- uint8_t val;
- microseconds t = time_get_us2();
-
- for (i=0; i<1000; i++) {
- if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
- nb_errs ++;
- }
-
- printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
- t = (time_get_us2() - t) / 1000;
- printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
-}
-
-prog_char str_ax12_stress_arg0[] = "ax12_stress";
-parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
-parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
-
-prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands";
-parse_pgm_inst_t cmd_ax12_stress = {
- .f = cmd_ax12_stress_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_ax12_stress,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_ax12_stress_arg0,
- (prog_void *)&cmd_ax12_stress_id,
- NULL,
- },
-};
-
-/**********************************************************/
-
-/* this structure is filled when cmd_baudrate is parsed successfully */
-struct cmd_baudrate_result {
- fixed_string_t arg0;
- uint32_t arg1;
-};
-
-/* function called when cmd_baudrate is parsed successfully */
-static void cmd_baudrate_parsed(void * parsed_result, void * data)
-{
- struct cmd_baudrate_result *res = parsed_result;
- struct uart_config c;
-
- printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
- uart_getconf(1, &c);
- c.baudrate = res->arg1;
- uart_setconf(1, &c);
- printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
-}
-
-prog_char str_baudrate_arg0[] = "baudrate";
-parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
-parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
-
-prog_char help_baudrate[] = "Change ax12 baudrate";
-parse_pgm_inst_t cmd_baudrate = {
- .f = cmd_baudrate_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_baudrate,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_baudrate_arg0,
- (prog_void *)&cmd_baudrate_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Uint16 */
-
-
-/* this structure is filled when cmd_uint16_read is parsed successfully */
-struct cmd_uint16_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t num;
- uint16_t val;
-};
-
-/* function called when cmd_uint16_read is parsed successfully */
-static void cmd_uint16_read_parsed(void * parsed_result, void * data)
-{
- struct cmd_uint16_result *res = parsed_result;
- uint8_t ret;
- uint16_t val;
- uint8_t addr = addr_from_string(res->arg1);
- ret = AX12_read_int(&gen.ax12, res->num, addr, &val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
- printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
-}
-
-prog_char str_uint16_arg0[] = "read";
-parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
-prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
- "max_torque#down_calibration#up_calibration#torque_limit#"
- "position#speed#load#punch";
-parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
-parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
-
-prog_char help_uint16_read[] = "Read uint16 value (type, num)";
-parse_pgm_inst_t cmd_uint16_read = {
- .f = cmd_uint16_read_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint16_read,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint16_arg0,
- (prog_void *)&cmd_uint16_arg1,
- (prog_void *)&cmd_uint16_num,
- NULL,
- },
-};
-
-/* function called when cmd_uint16_write is parsed successfully */
-static void cmd_uint16_write_parsed(void * parsed_result, void * data)
-{
- struct cmd_uint16_result *res = parsed_result;
- uint8_t ret;
- uint8_t addr = addr_from_string(res->arg1);
- printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
- res->val, res->val);
- ret = AX12_write_int(&gen.ax12, res->num, addr, res->val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
-}
-
-prog_char str_uint16_arg0_w[] = "write";
-parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
-prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
- "max_torque#torque_limit#punch";
-parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
-parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
-
-prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
-parse_pgm_inst_t cmd_uint16_write = {
- .f = cmd_uint16_write_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint16_write,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint16_arg0_w,
- (prog_void *)&cmd_uint16_arg1_w,
- (prog_void *)&cmd_uint16_num,
- (prog_void *)&cmd_uint16_val,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Uint8 */
-
-
-/* this structure is filled when cmd_uint8_read is parsed successfully */
-struct cmd_uint8_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t num;
- uint8_t val;
-};
-
-/* function called when cmd_uint8_read is parsed successfully */
-static void cmd_uint8_read_parsed(void * parsed_result, void * data)
-{
- struct cmd_uint8_result *res = parsed_result;
- uint8_t ret;
- uint8_t val;
- uint8_t addr = addr_from_string(res->arg1);
-
- ret = AX12_read_byte(&gen.ax12, res->num, addr, &val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
- printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
-}
-
-prog_char str_uint8_arg0[] = "read";
-parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
-prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
- "low_lim_volt#high_lim_volt#status_return#alarm_led#"
- "alarm_shutdown#torque_enable#led#cw_comp_margin#"
- "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
- "voltage#temp#reginst#moving#lock";
-parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
-parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
-
-prog_char help_uint8_read[] = "Read uint8 value (type, num)";
-parse_pgm_inst_t cmd_uint8_read = {
- .f = cmd_uint8_read_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint8_read,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint8_arg0,
- (prog_void *)&cmd_uint8_arg1,
- (prog_void *)&cmd_uint8_num,
- NULL,
- },
-};
-
-/* function called when cmd_uint8_write is parsed successfully */
-static void cmd_uint8_write_parsed(void * parsed_result, void * data)
-{
- struct cmd_uint8_result *res = parsed_result;
- uint8_t addr = addr_from_string(res->arg1);
- uint8_t ret;
- printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
- res->val, res->val);
- ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val);
- if (ret)
- printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
-}
-
-prog_char str_uint8_arg0_w[] = "write";
-parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
-prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
- "low_lim_volt#high_lim_volt#status_return#alarm_led#"
- "alarm_shutdown#torque_enable#led#cw_comp_margin#"
- "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
- "reginst#lock";
-parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
-parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
-
-prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
-parse_pgm_inst_t cmd_uint8_write = {
- .f = cmd_uint8_write_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_uint8_write,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_uint8_arg0_w,
- (prog_void *)&cmd_uint8_arg1_w,
- (prog_void *)&cmd_uint8_num,
- (prog_void *)&cmd_uint8_val,
- NULL,
- },
-};
+++ /dev/null
-/*
- * Copyright Droids Corporation (2008)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands_cs.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "main.h"
-#include "cs.h"
-#include "cmdline.h"
-
-struct csb_list {
- const prog_char *name;
- struct cs_block *csb;
-};
-
-prog_char csb_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,
- },
-};
-
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2008)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: commands_gen.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <encoders_spi.h>
-#include <adc.h>
-
-#include <scheduler.h>
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include <diagnostic.h>
-
-#include "main.h"
-#include "cmdline.h"
-#include "sensor.h"
-
-/**********************************************************/
-/* Reset */
-
-/* this structure is filled when cmd_reset is parsed successfully */
-struct cmd_reset_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_reset is parsed successfully */
-static void cmd_reset_parsed(void * parsed_result, void * data)
-{
- reset();
-}
-
-prog_char str_reset_arg0[] = "reset";
-parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
-
-prog_char help_reset[] = "Reset the board";
-parse_pgm_inst_t cmd_reset = {
- .f = cmd_reset_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_reset,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_reset_arg0,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Bootloader */
-
-/* this structure is filled when cmd_bootloader is parsed successfully */
-struct cmd_bootloader_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_bootloader is parsed successfully */
-static void cmd_bootloader_parsed(void *parsed_result, void *data)
-{
- bootloader();
-}
-
-prog_char str_bootloader_arg0[] = "bootloader";
-parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
-
-prog_char help_bootloader[] = "Launch the bootloader";
-parse_pgm_inst_t cmd_bootloader = {
- .f = cmd_bootloader_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_bootloader,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_bootloader_arg0,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Encoders tests */
-
-/* this structure is filled when cmd_encoders is parsed successfully */
-struct cmd_encoders_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_encoders is parsed successfully */
-static void cmd_encoders_parsed(void *parsed_result, void *data)
-{
- struct cmd_encoders_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("reset"))) {
- encoders_spi_set_value((void *)0, 0);
- encoders_spi_set_value((void *)1, 0);
- encoders_spi_set_value((void *)2, 0);
- encoders_spi_set_value((void *)3, 0);
- return;
- }
-
- /* show */
- while(!cmdline_keypressed()) {
- printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
- encoders_spi_get_value((void *)0),
- encoders_spi_get_value((void *)1),
- encoders_spi_get_value((void *)2),
- encoders_spi_get_value((void *)3));
- wait_ms(100);
- }
-}
-
-prog_char str_encoders_arg0[] = "encoders";
-parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
-prog_char str_encoders_arg1[] = "show#reset";
-parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
-
-prog_char help_encoders[] = "Show encoders values";
-parse_pgm_inst_t cmd_encoders = {
- .f = cmd_encoders_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_encoders,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_encoders_arg0,
- (prog_void *)&cmd_encoders_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Scheduler show */
-
-/* this structure is filled when cmd_scheduler is parsed successfully */
-struct cmd_scheduler_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_scheduler is parsed successfully */
-static void cmd_scheduler_parsed(void *parsed_result, void *data)
-{
- scheduler_dump_events();
-}
-
-prog_char str_scheduler_arg0[] = "scheduler";
-parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
-prog_char str_scheduler_arg1[] = "show";
-parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
-
-prog_char help_scheduler[] = "Show scheduler events";
-parse_pgm_inst_t cmd_scheduler = {
- .f = cmd_scheduler_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_scheduler,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_scheduler_arg0,
- (prog_void *)&cmd_scheduler_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Pwms tests */
-
-/* this structure is filled when cmd_pwm is parsed successfully */
-struct cmd_pwm_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- int16_t arg2;
-};
-
-/* function called when cmd_pwm is parsed successfully */
-static void cmd_pwm_parsed(void * parsed_result, void * data)
-{
- void * pwm_ptr = NULL;
- struct cmd_pwm_result * res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("1(4A)")))
- pwm_ptr = &gen.pwm1_4A;
- else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
- pwm_ptr = &gen.pwm2_4B;
- else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
- pwm_ptr = &gen.pwm3_1A;
- else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
- pwm_ptr = &gen.pwm4_1B;
-
- else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
- pwm_ptr = &gen.servo1;
- else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
- pwm_ptr = &gen.servo2;
- else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
- pwm_ptr = &gen.servo3;
- else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
- pwm_ptr = &gen.servo4;
-
- if (pwm_ptr)
- pwm_ng_set(pwm_ptr, res->arg2);
-
- printf_P(PSTR("done\r\n"));
-}
-
-prog_char str_pwm_arg0[] = "pwm";
-parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
-prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)";
-parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
-parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
-
-prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
-parse_pgm_inst_t cmd_pwm = {
- .f = cmd_pwm_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_pwm,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pwm_arg0,
- (prog_void *)&cmd_pwm_arg1,
- (prog_void *)&cmd_pwm_arg2,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Adcs tests */
-
-/* this structure is filled when cmd_adc is parsed successfully */
-struct cmd_adc_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_adc is parsed successfully */
-static void cmd_adc_parsed(void *parsed_result, void *data)
-{
- struct cmd_adc_result *res = parsed_result;
- uint8_t i, loop = 0;
-
- if (!strcmp_P(res->arg1, PSTR("loop_show")))
- loop = 1;
-
- do {
- printf_P(PSTR("ADC values: "));
- for (i=0; i<ADC_MAX; i++) {
- printf_P(PSTR("%.4d "), sensor_get_adc(i));
- }
- printf_P(PSTR("\r\n"));
- wait_ms(100);
- } while (loop && !cmdline_keypressed());
-}
-
-prog_char str_adc_arg0[] = "adc";
-parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
-prog_char str_adc_arg1[] = "show#loop_show";
-parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
-
-prog_char help_adc[] = "Show adc values";
-parse_pgm_inst_t cmd_adc = {
- .f = cmd_adc_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_adc,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_adc_arg0,
- (prog_void *)&cmd_adc_arg1,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Sensors tests */
-
-/* this structure is filled when cmd_sensor is parsed successfully */
-struct cmd_sensor_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_sensor is parsed successfully */
-static void cmd_sensor_parsed(void *parsed_result, void *data)
-{
- struct cmd_sensor_result *res = parsed_result;
- uint8_t i, loop = 0;
-
- if (!strcmp_P(res->arg1, PSTR("loop_show")))
- loop = 1;
-
- do {
- printf_P(PSTR("SENSOR values: "));
- for (i=0; i<SENSOR_MAX; i++) {
- printf_P(PSTR("%d "), !!sensor_get(i));
- }
- printf_P(PSTR("\r\n"));
- wait_ms(100);
- } while (loop && !cmdline_keypressed());
-}
-
-prog_char str_sensor_arg0[] = "sensor";
-parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
-prog_char str_sensor_arg1[] = "show#loop_show";
-parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
-
-prog_char help_sensor[] = "Show sensor values";
-parse_pgm_inst_t cmd_sensor = {
- .f = cmd_sensor_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_sensor,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_sensor_arg0,
- (prog_void *)&cmd_sensor_arg1,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Log */
-
-/* this structure is filled when cmd_log is parsed successfully */
-struct cmd_log_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t arg2;
- fixed_string_t arg3;
-};
-
-/* keep it sync with string choice */
-static const prog_char uart_log[] = "uart";
-static const prog_char i2c_log[] = "i2c";
-static const prog_char i2cproto_log[] = "i2cproto";
-static const prog_char sensor_log[] = "sensor";
-static const prog_char block_log[] = "bd";
-static const prog_char beacon_log[] = "beacon";
-static const prog_char scanner_log[] = "scanner";
-static const prog_char imgprocess_log[] = "imgprocess";
-
-struct log_name_and_num {
- const prog_char * name;
- uint8_t num;
-};
-
-static const struct log_name_and_num log_name_and_num[] = {
- { uart_log, E_UART },
- { i2c_log, E_I2C },
- { i2cproto_log, E_USER_I2C_PROTO },
- { sensor_log, E_USER_SENSOR },
- { block_log, E_BLOCKING_DETECTION_MANAGER },
- { beacon_log, E_USER_BEACON },
- { scanner_log, E_USER_SCANNER },
- { imgprocess_log, E_USER_IMGPROCESS },
-};
-
-static uint8_t
-log_name2num(const char * s)
-{
- uint8_t i;
-
- for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
- if (!strcmp_P(s, log_name_and_num[i].name)) {
- return log_name_and_num[i].num;
- }
- }
- return 0;
-}
-
-const prog_char *
-log_num2name(uint8_t num)
-{
- uint8_t i;
-
- for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
- if (num == log_name_and_num[i].num) {
- return log_name_and_num[i].name;
- }
- }
- return NULL;
-}
-
-/* function called when cmd_log is parsed successfully */
-static void cmd_log_do_show(void)
-{
- uint8_t i, empty=1;
- const prog_char * name;
-
- printf_P(PSTR("log level is %d\r\n"), gen.log_level);
- for (i=0; i<NB_LOGS; i++) {
- name = log_num2name(gen.logs[i]);
- if (name) {
- printf_P(PSTR("log type %S is on\r\n"), name);
- empty = 0;
- }
- }
- if (empty)
- printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
-}
-
-/* function called when cmd_log is parsed successfully */
-static void cmd_log_parsed(void * parsed_result, void * data)
-{
- struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("level"))) {
- gen.log_level = res->arg2;
- }
-
- /* else it is a show */
- cmd_log_do_show();
-}
-
-prog_char str_log_arg0[] = "log";
-parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
-prog_char str_log_arg1[] = "level";
-parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
-parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
-
-prog_char help_log[] = "Set log options: level (0 -> 5)";
-parse_pgm_inst_t cmd_log = {
- .f = cmd_log_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_log,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_log_arg0,
- (prog_void *)&cmd_log_arg1,
- (prog_void *)&cmd_log_arg2,
- NULL,
- },
-};
-
-prog_char str_log_arg1_show[] = "show";
-parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
-
-prog_char help_log_show[] = "Show configured logs";
-parse_pgm_inst_t cmd_log_show = {
- .f = cmd_log_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_log_show,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_log_arg0,
- (prog_void *)&cmd_log_arg1_show,
- NULL,
- },
-};
-
-/* this structure is filled when cmd_log is parsed successfully */
-struct cmd_log_type_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- fixed_string_t arg2;
- fixed_string_t arg3;
-};
-
-/* function called when cmd_log is parsed successfully */
-static void cmd_log_type_parsed(void * parsed_result, void * data)
-{
- struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
- uint8_t lognum;
- uint8_t i;
-
- lognum = log_name2num(res->arg2);
- if (lognum == 0) {
- printf_P(PSTR("Cannot find log num\r\n"));
- return;
- }
-
- if (!strcmp_P(res->arg3, PSTR("on"))) {
- for (i=0; i<NB_LOGS; i++) {
- if (gen.logs[i] == lognum) {
- printf_P(PSTR("Already on\r\n"));
- return;
- }
- }
- for (i=0; i<NB_LOGS; i++) {
- if (gen.logs[i] == 0) {
- gen.logs[i] = lognum;
- break;
- }
- }
- if (i==NB_LOGS) {
- printf_P(PSTR("no more room\r\n"));
- }
- }
- else if (!strcmp_P(res->arg3, PSTR("off"))) {
- for (i=0; i<NB_LOGS; i++) {
- if (gen.logs[i] == lognum) {
- gen.logs[i] = 0;
- break;
- }
- }
- if (i==NB_LOGS) {
- printf_P(PSTR("already off\r\n"));
- }
- }
- cmd_log_do_show();
-}
-
-prog_char str_log_arg1_type[] = "type";
-parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
-/* keep it sync with log_name_and_num above */
-prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#beacon#scanner#imgprocess";
-parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
-prog_char str_log_arg3[] = "on#off";
-parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
-
-prog_char help_log_type[] = "Set log type";
-parse_pgm_inst_t cmd_log_type = {
- .f = cmd_log_type_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_log_type,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_log_arg0,
- (prog_void *)&cmd_log_arg1_type,
- (prog_void *)&cmd_log_arg2_type,
- (prog_void *)&cmd_log_arg3,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Stack_Space */
-
-/* this structure is filled when cmd_stack_space is parsed successfully */
-struct cmd_stack_space_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_stack_space is parsed successfully */
-static void cmd_stack_space_parsed(void *parsed_result, void *data)
-{
- printf("res stack: %d\r\n", min_stack_space_available());
-
-}
-
-prog_char str_stack_space_arg0[] = "stack_space";
-parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
-
-prog_char help_stack_space[] = "Display remaining stack space";
-parse_pgm_inst_t cmd_stack_space = {
- .f = cmd_stack_space_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_stack_space,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_stack_space_arg0,
- NULL,
- },
-};
+++ /dev/null
-/*
- * 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_scan.c,v 1.1 2009-05-27 20:04:07 zer0 Exp $
- *
- * Olivier MATZ <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-#include <time.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include <adc.h>
-
-#include <math.h>
-
-#include "main.h"
-#include "cmdline.h"
-#include "../common/i2c_commands.h"
-#include "i2c_protocol.h"
-#include "actuator.h"
-
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-
-#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;i<MAX_OBJECTS;i++){
- sac_obj[i].x_min = 0;
- sac_obj[i].x_max = 0;
- sac_obj[i].y_min = 0;
- sac_obj[i].y_max = 0;
- }
-
- //parcour_img(sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN, sac_obj, MAX_OBJECTS);
- /*
- process_img(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
- 4, 1,
- 0, 15, 15,
- 1);
- */
-
- for (i=0;i<MAX_OBJECTS;i++){
- printf_P(PSTR("obj: %d %d %d %d\r\n"),
- sac_obj[i].x_min,
- sac_obj[i].y_min,
- sac_obj[i].x_max,
- sac_obj[i].y_max);
- }
-
- printf_P(PSTR("start dumping %ld\r\n"), PIX_PER_SCAN);
-
- for (i=0;i<MAX_SAMPLE;i++)
- printf_P(PSTR("%d\r\n"),scan_params.sample_tab[i]);
-
- printf_P(PSTR("end dumping (pos: %ld)\r\n"),
- encoders_spi_get_value_scanner((void *)SCANNER_ENC));
- }
-
-}
-
-prog_char str_sample_arg0[] = "sample";
-parse_pgm_token_string_t cmd_sample_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sample_result, arg0, str_sample_arg0);
-prog_char str_sample_arg1[] = "start#dump";
-parse_pgm_token_string_t cmd_sample_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sample_result, arg1, str_sample_arg1);
-parse_pgm_token_num_t cmd_sample_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, offset_b, UINT16);
-parse_pgm_token_num_t cmd_sample_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, offset_a, FLOAT);
-parse_pgm_token_num_t cmd_sample_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, dump_speed, UINT16);
-parse_pgm_token_num_t cmd_sample_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, filter, UINT8);
-
-prog_char help_sample[] = "Sample func off_a_mot, off_a_angl, dump_speed";
-parse_pgm_inst_t cmd_sample = {
- .f = cmd_sample_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_sample,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_sample_arg0,
- (prog_void *)&cmd_sample_arg1,
- (prog_void *)&cmd_sample_arg2,
- (prog_void *)&cmd_sample_arg3,
- (prog_void *)&cmd_sample_arg4,
- (prog_void *)&cmd_sample_arg5,
- NULL,
- },
-};
-
-
-
-
-/**********************************************************/
-/* sadc tests */
-
-/* this structure is filled when cmd_sadc is parsed successfully */
-struct cmd_sadc_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_sadc is parsed successfully */
-static void cmd_sadc_parsed(void *parsed_result, void *data)
-{
-
- printf_P(PSTR("Scan ADC values:\r\n"));
- do {
- printf_P(PSTR("%.4d "), adc_get_value( ADC_REF_AVCC | MUX_ADC13 ));
- printf_P(PSTR("\r\n"));
- wait_ms(100);
- } while (!cmdline_keypressed());
-}
-
-prog_char str_sadc_arg0[] = "sadc";
-parse_pgm_token_string_t cmd_sadc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sadc_result, arg0, str_sadc_arg0);
-
-prog_char help_sadc[] = "Show sadc values";
-parse_pgm_inst_t cmd_sadc = {
- .f = cmd_sadc_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_sadc,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_sadc_arg0,
- NULL,
- },
-};
-
-
-
-
-/**********************************************************/
-/* set scanner params */
-
-/* this structure is filled when cmd_scan_params is parsed successfully */
-struct cmd_scan_params_result {
- fixed_string_t arg0;
- int speed;
- uint8_t debug;
-};
-
-/* function called when cmd_scan_params is parsed successfully */
-static void cmd_scan_params_parsed(void * parsed_result, void * data)
-{
- struct cmd_scan_params_result * res = parsed_result;
-
- scan_params.speed = res->speed;
- 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,
- },
-};
-
-
+++ /dev/null
-/*
- * 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 <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "main.h"
-#include "cmdline.h"
-#include "../common/i2c_commands.h"
-#include "i2c_protocol.h"
-#include "actuator.h"
-
-struct cmd_event_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- fixed_string_t arg2;
-};
-
-
-/* function called when cmd_event is parsed successfully */
-static void cmd_event_parsed(void *parsed_result, void *data)
-{
- u08 bit=0;
-
- struct cmd_event_result * res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("all"))) {
- bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
- if (!strcmp_P(res->arg2, PSTR("on")))
- 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,
- },
-};
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cs.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <spi.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-#include <adc.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <parse.h>
-#include <rdline.h>
-
-#include "main.h"
-#include "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);
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: cs.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
- *
- */
-
-void microb_cs_init(void);
-void dump_cs(const char *name, struct cs *cs);
-void dump_pid(const char *name, struct pid_filter *pid);
+++ /dev/null
-/*
- * 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_
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-#include <inttypes.h>
-
-#include <stdint.h>
-
-
-#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;i<sizeof(h_limit)/sizeof(uint8_t);i++){
- if (H < h_limit[i]+H_MARGIN){
- H_fin = i;
- break;
- }
- }
- }
-
- L_fin = L;
- //printf_P(PSTR("%f %f\r\n"), dist, m_a*180/M_PI);
- //printf_P(PSTR("%f %f\r\n"), m_a*180/M_PI, b*180/M_PI);
-
- //printf_P(PSTR("%d %f\r\n"), a, b*180/M_PI);
- //printf_P(PSTR("%f %f %f\r\n"), H, m_a*180/M_PI, offset_b);
-
- //printf_P(PSTR("a, tpos: %d %d \r\n"), a, tour_pos);
-
-
- //printf_P(PSTR("%f %f\r\n"), H, L);
-
-
- /*
- if (row_n%2){
- //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
- tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
- }
- else{
- tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
- }
- */
- 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;
-
-
-
- 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 <MAX_SAMPLE)// && row_n%2)
- //sample_tab[pos] = a>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 <MAX_SAMPLE)// && row_n%2)
- //sample_tab[pos_tmp] = min_sample;
- nop();
-
- }
-
-
- }
-
- scan_params.last_row_n = row_n;
- scan_params.last_col_n = col_n;
- scan_params.last_sample = H_fin;
-
-
- //printf("pos : %ld\r\n", pos);
- //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
-
- //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
-
- /*
- if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
- sample_tab[pos] = 0xffff;
- */
- scan_params.sample_i--;
-}
-
-
-#define my_offset_a (0*M_PI/180)
-#define my_offset_b (34*M_PI/180)
-
-//#define my_offset_a (40*M_PI/180)
-//#define my_offset_b (-5*M_PI/180)
-
-
-
-/*
- *2 cause we don't known exactly limits
- at computation time
-*/
-lookup_h_l array_h_l[DIM_DIST*2][DIM_ANGLE*2];
-
-
-
-#define pgm_read_word_near(a) (*((uint16_t*)(a)))
-
-
-void do_scan_quick(void * dummy)
-{
-
- int i, j;
- 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;
- uint8_t min_sample;
- //double H, L;
- //int32_t H_fin, L_fin;
-
-
- union{
- uint16_t u16;
- lookup_h_l h_l;
- }val;
- if (scan_params.sample_i==0)
- return;
-
- mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
- 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;
-
- a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
-
-
- tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
-
-
- if (scan_params.offset_a != 0)
- printf_P(PSTR("%ld %d \r\n"), tour_pos, a);
-
- /* lookup in precomputed array */
-
-
- j = (DIM_DIST * (telemetre_to_cm(a)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
- i = (DIM_ANGLE * tour_pos)/STEP_PER_ROUND;
-
-
- if (j<0)
- j=0;
- if (j>=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 <MAX_SAMPLE)// && row_n%2)
- //sample_tab[pos] = a>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 <MAX_SAMPLE)// && row_n%2)
- //sample_tab[pos_tmp] = min_sample;
- nop();
-
- }
-
-
- }
-
- scan_params.last_row_n = row_n;
- scan_params.last_col_n = col_n;
- scan_params.last_sample = H_fin;
-
-
- //printf("pos : %ld\r\n", pos);
- //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
-
- //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
-
- /*
- if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
- sample_tab[pos] = 0xffff;
- */
- scan_params.sample_i--;
-}
-
-
-int main(int argc, char** argv)
-{
- union{
- uint16_t u16;
- lookup_h_l h_l;
- }val;
-
- int i, j;
- FILE* f, *fin, *f_header;
-
- unsigned char line[1000];
- unsigned int a, b;
- unsigned int dist_max, angle_max;
-
- scan_params.filter = 1;
-
-
-
- f = fopen("oo", "w");
-
-
- TELEMETRE_B = 23.50;
-
- scan_params.offset_a = my_offset_a;
- scan_params.offset_b = my_offset_b;
-
-
-
-
-
- /*precompute H & L array */
- for (j = 0, scan_dist = TELEMETRE_MIN; scan_dist< TELEMETRE_MAX; j++, scan_dist+=DIST_STEP){
-
- for (i = 0, motor_angle = 0; motor_angle < STEP_PER_ROUND; i++, motor_angle+=ANGLE_STEP){
- scan_params.sample_i = 100;
- scan_params.pos_start_scan = 0;
- //printf("%d\n", i);
- do_scan(0);
- //printf("mangle, dist, h, l: %d %d, (%d, %2.2f)\n", motor_angle, scan_dist, H_fin, L);
- //fprintf(f, "mangle, dist, h, l: %d %d, (%d, %2.2f)\n", motor_angle, scan_dist, H_fin, L);
- /*
- j = (scan_dist - TELEMETRE_MIN)/DIST_STEP;
- i = motor_angle/ANGLE_STEP;
- //printf("%d %d (%d %d) (%d %d %d = %d)\n", i, j, motor_angle, ANGLE_STEP, scan_dist, TELEMETRE_MIN, DIST_STEP, (scan_dist - TELEMETRE_MIN)/DIST_STEP);
- array_h_l[j][i].h = H_fin;
- array_h_l[j][i].l = L_fin;
- */
- j = (DIM_DIST * (telemetre_to_cm(scan_dist)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
- i = (DIM_ANGLE * motor_angle)/STEP_PER_ROUND;
- array_h_l[j][i].h = H_fin;
- array_h_l[j][i].l = L_fin;
-
- }
- }
-
-
- scan_params.filter = 0;
- if (argc>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<dist_max; j++, scan_dist+=DIST_STEP){
- fprintf(f_header, "{\n");
-
- for (i = 0, motor_angle = 0; i < angle_max; i++, motor_angle+=ANGLE_STEP){
- fprintf(f_header, "{ .h = %d, .l = %d }, ", array_h_l[j][i].h, array_h_l[j][i].l);
-
- }
- fprintf(f_header, "},\n");
- }
- fprintf(f_header, "};\n", DIM_DIST, DIM_ANGLE);
-
- fclose(f_header);
-
-
-
- printf("IIIIIIIIIIII %d %d\n", array_h_l[38][152].h, array_h_l[38][152].l);
- val.u16 = pgm_read_word_near(&array_h_l[38][152]);
- printf("IIIIIIIIIIII %d %d\n", val.h_l.h, val.h_l.l);
-
- motor_angle = 9000;
- scan_dist = 388;
-
- for (motor_angle = 0; motor_angle<14000; motor_angle+=100){
- for (scan_dist = TELEMETRE_MIN; scan_dist<300; scan_dist+=10){
- scan_params.sample_i = 100;
- scan_params.pos_start_scan = 0;
-
- do_scan(0);
-
-
- //printf("m s %d %d\n", motor_angle, scan_dist);
- //printf("R %d %d\n", H_fin, L_fin);
-
- val.u16 = pgm_read_word_near(&array_h_l[(scan_dist-TELEMETRE_MIN)/DIST_STEP][motor_angle/ANGLE_STEP]);
- //printf("Q %d %d\n\n", val.h_l.h, val.h_l.l);
-
- if (val.h_l.h != H_fin || val.h_l.l != L_fin)
- printf("BUG BUG\n");
- }
- }
-
- fin = fopen("out", "r");
- while(fgets(line, sizeof(line), fin)){
- scan_params.sample_i = 100;
- scan_params.pos_start_scan = 0;
-
- //printf("[%s]\n", line);
- sscanf(line, "%d %d\n", &a, &b);
- //printf("%d %d\n", a, b);
- motor_angle = a;
- scan_dist = b;
- do_scan_quick(0);
- /*
- j = (DIM_DIST * (telemetre_to_cm(scan_dist)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
- i = (DIM_ANGLE * motor_angle)/STEP_PER_ROUND;
-
- val.u16 = pgm_read_word_near(&array_h_l[j][i]);
- H_fin = val.h_l.h;
- L_fin = val.h_l.l;
- */
-
- printf("Q %d %d\n", H_fin, L_fin);
-
- do_scan(0);
-
- printf("R %d %d\n", H_fin, L_fin);
-
- fprintf(f, "mangle, dist, h, l: %d %d, (%d, %d)\n", motor_angle, scan_dist, H_fin, L_fin);
-
- }
- fclose(fin);
-
- fclose(f);
-
- return;
- /*
- for (j = 0, scan_dist = 0; j<DIM_DIST; j++, scan_dist+=DIST_STEP){
- for (i = 0, motor_angle = 0; i < DIM_ANGLE; i++, motor_angle+=ANGLE_STEP){
- do_scan(0);
- printf("%d %d, (%d, %2.2f)\n", motor_angle, scan_dist, H_fin, L);
-
- }
-
- }
- */
-
-
-}
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * Copyright Droids Corporation (2007)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: i2c_protocol.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $
- *
- */
-
-#include <string.h>
-
-#include <aversive.h>
-#include <aversive/list.h>
-#include <aversive/error.h>
-
-#include <scheduler.h>
-
-#include <i2c.h>
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "../common/i2c_commands.h"
-#include "main.h"
-#include "actuator.h"
-#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)
-{
-}
-
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2007)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: i2c_protocol.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
- *
- */
-
-#include <aversive.h>
-
-void i2c_protocol_init(void);
-
-void i2c_recvevent(uint8_t * buf, int8_t size);
-void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
-void i2c_sendevent(int8_t size);
-
-int debug_send(char c, FILE* f);
+++ /dev/null
-#include <stdio.h>
-#include <string.h>
-#include <inttypes.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/error.h>
-
-#include <stdint.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-
-#include "img_processing.h"
-
-
-#define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
-
-#ifndef HOST_VERSION
-
-#include <aversive.h>
-#include <pwm_ng.h>
-#include <pid.h>
-#include <time.h>
-#include <quadramp.h>
-#include <blocking_detection_manager.h>
-#include <rdline.h>
-
-#include <control_system_manager.h>
-#include <adc.h>
-#include <spi.h>
-#include <ax12.h>
-
-#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;i<total;i++){
- if (!memcmp(&tab_o[i], o, sizeof(Object_bb)))
- return 0;
-
- if (tab_o[i].x_min==0 && tab_o[i].x_max==0 &&
- tab_o[i].y_min==0 && tab_o[i].y_max==0){
- memcpy(&tab_o[i], o, sizeof(Object_bb));
- return 1;
- }
-
- }
- return 0;
-}
-
-/* step around object of given color and compute its bounding box */
-void object_find_bounding_box(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_bb* 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;
-
- vect_t v, vi;
-
-
- v.x = 1;
- v.y = 0;
-
- vi = v;
-
- o->x_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_x<o->x_min)
- o->x_min = pos_x;
- if (pos_y<o->y_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;i<o->pts_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;i<NUM_CALIPERS;i++){
- /* compute polygon edge vector */
- edges[i].x = o->pts[(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;i<NUM_CALIPERS;i++){
- if (angles[i]<min_angle){
- min_angle = angles[i];
- caliper_result_index = i;
- }
- }
-
- /* rotate calipers */
- calipers[caliper_result_index] = edges[caliper_result_index];
-
- for (i=caliper_result_index; i<caliper_result_index + o->pts_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;i<NUM_CALIPERS;i++){
- if (angles[i]==min_angle)
- calipers_pts_index[i] = (calipers_pts_index[i] + 1) % o->pts_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;i<NUM_CALIPERS;i++){
- pts_index_out[i] = calipers_pts_index[i];
- }
- *v_out = calipers[0];
- *r1 = res1;
- *r2 = res2;
-
- }
- total_rot_angle+=min_angle;
- }
-
- return;
-}
-
-#define COEF_CALIP 1
-/* transform caliper to rectangle coordinates */
-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)
-{
- line_t l1, l2;
- vect_t caliper_tmp;
- int ret, i;
- double mp_x, mp_y;
- point_t p_int1;//, p_int2;
-
- point_t p1, p2;
-
- caliper_tmp = *caliper;
-
- //IMG_DEBUG("cal: %" PRIi32 " %" PRIi32 "", caliper_tmp.x, caliper_tmp.y);
-
- mp_x = 0;
- mp_y = 0;
-
- /* to be precise, calc 4 intersection of 4 calipers */
- for (i=0;i<NUM_CALIPERS;i++){
- p1.x = o->pts[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;i<r1_s;i++){
- for(j=0;j<r2_s;j++){
- zones[index].p.x = ptmp.x + (i*r1->x)/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 <HIGHER_MAX_PIXEL))
- return 1;
-
- return 0;
-}
-
-int zone_filter_min_surface(unsigned char* data, int16_t x_in, int16_t y_in,
- uint8_t color, unsigned int zones_num, zone* p)
-{
- int i;
-
- for (i = 0; i < zones_num ; i++){
- if (zone_has_enought_pixels(data, x_in, y_in, &p[i]))
- continue;
-
- p[i].valid = 0;
- }
-
- IMG_NOTICE("num zone after min surf: %d", zones_num);
-
- return zones_num;
-
-}
-
-/* center is 15 cm radius*/
-#define CENTER_RADIUS 15
-
-/* the complete column must be in the drop zone*/
-#define CENTER_MAX_DIST (15-3)
-
-/* the column must not be too close from center*/
-#define CENTER_MIN_DIST (8)
-
-int zone_filter_center(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y, int tweak_min_margin)
-{
- int i;
- vect_t v;
-
- for (i = 0; i < zones_num; i++){
-
- v.x = p[i].p.x - center_x;
- v.y = p[i].p.y - center_y;
- IMG_DEBUG("square dist to center %"PRIi32" (%d %d)",
- v.x*v.x + v.y*v.y, (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin), CENTER_MAX_DIST * CENTER_MAX_DIST);
-
- if (v.x*v.x + v.y*v.y < CENTER_MAX_DIST * CENTER_MAX_DIST &&
- v.x*v.x + v.y*v.y > (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;i<o->pts_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<x_in;i++){
- data[i] = 0;
- data[(y_in - 1) * x_in + i] = 0;
- }
-
- for (i=0;i<y_in;i++){
- data[i * x_in] = 0;
- data[i * x_in + x_in - 1] = 0;
- }
-
-
-
- etat = 0;
- /*
- 0 look for color (object or edge)
- 1 look for edge end
- */
-
- for (i=1;i<x_in*y_in;i++){
- switch(etat){
- case 0:
- //we are in the dark
- switch(data[i]){
- case 0:
- //look for in dark
- break;
- /*
- case 1:
- case 2:
- case 0x15:
- case 0x24:
- */
- default:
- in_color = data[i];
- etat = 1;
- // we found an object
- object_find_bounding_box(data, x_in, y_in, (i-1)%x_in, (i-1)/x_in, data[i], 255, &o);
-
- ret = store_obj(sac_obj, max_obj, &o);
- /* if new object, process rotating calipers */
- if (ret){
- sac_obj_poly[obj_num].len = o.len;
- object_find_poly(data, x_in, y_in,
- (i-1)%x_in, (i-1)/x_in,
- data[i], 255, &sac_obj_poly[obj_num]);
- IMG_DEBUG("%d",sac_obj_poly[obj_num].pts_num);
- object_poly_to_convex(&sac_obj_poly[obj_num]);
- /*
- for (j=0;j<sac_obj_poly[obj_num].pts_num;j++){
- data[sac_obj_poly[obj_num].pts[j].y*x_in + sac_obj_poly[obj_num].pts[j].x] = 8;
- }*/
- sac_obj_poly[obj_num].color = data[i];
- obj_num++;
- }
-
- break;
-
- case DEFAULT_COLOR:
- //we must out of color
- break;
- }
- break;
-
- case 1:
- if (data[i] != in_color){
- i--;
- etat = 0;
- }
- /*
- we are in a color and want to go out of it
- */
- break;
-
- }
-
-
-
- }
-
-
- return data;
-
-
-}
-/* space between twin tower is 13 pixels*/
-#define SPACE_INTER_TWIN_TOWER (13)
-
-#define SPACE_INTER_TWIN_TOWER_TOLERENCE 3
-
-
-
-/* find best twin tower for each zone */
-void find_twin_tower(uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE],
- int16_t center_x, int16_t center_y)
-{
-
- uint8_t i, j;
- uint8_t z_n;
- int n1, n2;
- unsigned int z1, z2, z3;
- //int32_t scal1, scal2;
- vect_t v, v1, v2;
- line_t l;
- point_t p;
- unsigned int good_zone;
- double dist, dist2;
- unsigned int current_sister;
-
- /* init dummy sisters */
- for (i = 0; i < zones_num; i++)
- for (j = 0; j < MAX_SISTER_PER_ZONE; j++)
- sisters[i][j] = -1;
-
-
-
- for (z_n = 0; z_n < zones_num; z_n++){
- if (!zones[z_n].valid)
- continue;
-
- current_sister = 0;
-
- for (i = 0; i < zones_num; i++){
-
-
- /* we already have max sisters */
- if (current_sister >= 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<n_columns;i++)
- IMG_NOTICE("c %d:(h:%d) (%"PRIi32" %"PRIi32") valid=%d",
- i, drop_c[i].h,
- zones[drop_c[i].z].p.x, zones[drop_c[i].z].p.y,
- drop_c[i].valid);
-
- for (i=0;i<n_lintels;i++)
- IMG_NOTICE("l %d:(h:%d) (%"PRIi32" %"PRIi32") "
- "(%"PRIi32" %"PRIi32") valid=%d",
- i, drop_l[i].h,
- zones[drop_l[i].z1].p.x, zones[drop_l[i].z1].p.y,
- zones[drop_l[i].z2].p.x, zones[drop_l[i].z2].p.y, drop_l[i].valid);
-
-}
-
-#define MY_MAX(a, b) ((a)>(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<MAX_OBJECTS;i++){
-
- if (!sac_obj_poly[i].pts_num)
- continue;
-
- IMG_DEBUG("obj: %d %d %d %d %d",
- i,
- sac_obj[i].x_min,
- sac_obj[i].y_min,
- sac_obj[i].x_max,
- sac_obj[i].y_max);
-
- //IMG_DEBUG("poly pts_num: %d", sac_obj_poly[i].pts_num);
-
- object_poly_get_min_ar(&sac_obj_poly[i], &pts_cal[0], &caliper, &r1, &r2);
-
- ret = object_poly_caliper_to_rectangle(&sac_obj_poly[i], &pts_cal[0], &caliper,
- &r1, &r2, &ptmp);
-
- if (!ret)
- continue;
-
- /*
- IMG_DEBUG("r: (%3"PRIi32" %3"PRIi32") "
- "(%3"PRIi32" %3"PRIi32")",
- r1.x, r1.y, r2.x, r2.y);
- IMG_DEBUG("intersection: %"PRIi32" %"PRIi32"",
- ptmp.x, ptmp.y);
- */
-
- zone_len = split_rectangle(&ptmp, &r1, &r2,
- MAX_ZONES_PER_OBJECT, &zones[0], sac_obj_poly[i].color);
- //IMG_DEBUG("split ok %d", zone_len);
-
- zone_len = zone_filter_min_surface(buffer, x_in, y_in,
- sac_obj_poly[i].color,
- zone_len, &zones[0]);
-
- for (j = 0; j < zone_len && zone_num < max_zones; zone_num++, j++)
- all_zones[zone_num] = zones[j];
-
- }
-
-
- IMG_NOTICE("num zones end: %d", zone_num);
-
- return zone_num;
-}
-
-
-void process_img_to_zone(unsigned char *buffer, int16_t x_in, int16_t y_in)
-{
- g_zone_num = process_img(buffer, x_in, y_in,
- g_all_zones, MAX_ZONES);
-}
-
-uint8_t filter_zones(unsigned char *buffer, int16_t x_in, int16_t y_in,
- zone * all_zones, uint8_t zone_num, uint8_t max_zones,
- uint8_t working_zone, int16_t center_x, int16_t center_y,
- int tweak_min_margin)
-{
- uint8_t i;
-
- /* first valid all zones */
- for (i = 0; i < zone_num; i++){
- all_zones[i].valid = 1;
-
- /* filter zone lower thatn working zone */
- if (all_zones[i].h < working_zone)
- all_zones[i].valid = 0;
- }
-
-
- /*
- generate correct zone at ground level
- (depending on working zone)
- */
- if (working_zone == 2)
- zone_num = generate_center_ground_zones(buffer, x_in, y_in,
- all_zones, zone_num, max_zones, center_x, center_y);
- else
- zone_num = generate_rectangle_ground_zones(buffer, x_in, y_in,
- all_zones, zone_num, max_zones, center_x, center_y,
- working_zone);
-
- /* filter zone position, depending on workingzone */
- if (working_zone == 2)
- zone_num = zone_filter_center(zone_num, all_zones, center_x, center_y, tweak_min_margin);
- else
- zone_num = zone_filter_zone_rect(zone_num, all_zones, center_x, center_y , working_zone);
-
-
-
- /* display zones (debug purpose) */
-
- for (i = 0; i < zone_num; i++){
- //buffer[all_zones[i].p.y*x_in+all_zones[i].p.x] = 0x3;
- IMG_NOTICE("h:%d (v:%d) x:%"PRIi32" y:%"PRIi32"", all_zones[i].h, all_zones[i].valid, all_zones[i].p.x, all_zones[i].p.y);
-
- }
-
- IMG_NOTICE("num zones: %d", zone_num);
-
-
-
-
- return zone_num;
-}
-
-
-/*
- return -1 if not column dropzone is found
- return column hight if found
-*/
-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 zone_num;
- int c_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,
- 0);
-
- c_drop_zone = find_column_dropzone(zone_num, g_all_zones);
-
- if (c_drop_zone<0)
- return -1;
-
- *dropzone_x = g_all_zones[c_drop_zone].p.x;
- *dropzone_y = g_all_zones[c_drop_zone].p.y;
-
- *dropzone_x = (*dropzone_x) * PIXEL2CM;
- *dropzone_y = (*dropzone_y) * PIXEL2CM + 30;
-
- return g_all_zones[c_drop_zone].h;
-}
-
-#define ROBOT_SEMI_INTERCOLUMN_SPACE 75
-
-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)
-{
- zone z;
- int ret;
- int i;
-
-
- IMG_NOTICE("test z (mm) : x:%d y:%d", center_x, center_y);
- IMG_NOTICE("test z:(pix): x:%d y:%d", (int)(center_x/ PIXEL2CM), (int)(center_y/ PIXEL2CM));
-
-
- z.p.x = center_x / PIXEL2CM;
- z.p.y = (center_y - ROBOT_SEMI_INTERCOLUMN_SPACE) / PIXEL2CM ;
- z.h = h;
- z.valid = 1;
- ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
- IMG_NOTICE("test z1: %d", ret);
-
- if (!ret)
- return 0;
-
- z.p.x = center_x / PIXEL2CM;
- z.p.y = (center_y + ROBOT_SEMI_INTERCOLUMN_SPACE)/ PIXEL2CM;
- z.h = h;
- z.valid = 1;
- ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
- IMG_NOTICE("test z2: %d", ret);
- if (!ret)
- return 0;
-
-
- /*
- if middle zone is less or egual to tested temple
- */
- z.p.x = center_x / PIXEL2CM;
- z.p.y = center_y / PIXEL2CM;
-
- for (i = h; 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<MAX_SISTER_PER_ZONE;j++){
- IMG_DEBUG("s: %d", sisters[i][j]);
- }
- }
-
- /* only use first sister of each zone */
- for (i=0; i< zone_num; i++){
-
- /* if zone doesn't have twin tower */
- if (sisters[i][0] == -1)
- continue;
-
- if (!g_all_zones[i].valid)
- continue;
-
- if (z_n == -1){
- z_n = i;
- continue;
- }
-
- /* if we found higher twin tower */
- if (g_all_zones[z_n].h > 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;
-
-}
-
+++ /dev/null
-
-
-
-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];
+++ /dev/null
-/*
- * Copyright Droids Corporation
- * Olivier Matz <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: main.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-#include <avr/eeprom.h>
-
-#include <aversive.h>
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <spi.h>
-#include <i2c.h>
-#include <encoders_spi.h>
-#include <pwm_ng.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <time.h>
-#include <adc.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <parse.h>
-#include <rdline.h>
-
-#include "../common/eeprom_mapping.h"
-#include "../common/i2c_commands.h"
-
-#include "main.h"
-#include "ax12_user.h"
-#include "cmdline.h"
-#include "sensor.h"
-#include "actuator.h"
-#include "cs.h"
-#include "i2c_protocol.h"
-#include "beacon.h"
-#include "scanner.h"
-
-/* 0 means "programmed"
- * ---- with 16 Mhz quartz
- * CKSEL 3-0 : 0111
- * SUT 1-0 : 10
- * CKDIV8 : 1
- * ---- bootloader
- * BOOTZ 1-0 : 01 (4K bootloader)
- * BOOTRST : 0 (reset on bootloader)
- * ---- jtag
- * jtagen : 0
- */
-
-struct genboard gen;
-struct 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;
-}
+++ /dev/null
-/*
- * 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; \
-})
+++ /dev/null
-/*
- * 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
+++ /dev/null
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-#include <aversive/pgmspace.h>
-#include <pwm_ng.h>
-#include <pid.h>
-#include <time.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <adc.h>
-#include <spi.h>
-#include <ax12.h>
-
-#include <time.h>
-#include <blocking_detection_manager.h>
-
-#include <encoders_spi.h>
-
-#include <rdline.h>
-
-#include "sensor.h"
-
-#include <uart.h>
-
-#include "main.h"
-#include "scanner.h"
-
-#include "cmdline.h"
-
-
-#include "scan_h_l.h"
-
-
-#include <vect_base.h>
-#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<SCANNER_MAX_SAMPLE;i++)
- scanner_sample_size[i] = 0;*/
-
-
-
-}
-
-
-#define SCANNER_OFFSET_CALIBRE 1500
-
-#define CALIBRE_LASER_SAMPLE 100
-
-
-
-
-
-
-void scanner_calibre_laser(void)
-{
- unsigned int i;
- int32_t laser_value = 0;
-
- /* arm out */
- pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
-
- /* set mirror to have vertical laser */
- cs_set_consign(&sensorboard.scanner.cs, (35L*SCANNER_STEP_TOUR)/100);
-
- wait_ms(500);
-
- /*
- laser must point on the ground:
- we sample laser in order to determine
- laser cold/warm calibration
- */
-
- for (i = 0; i<CALIBRE_LASER_SAMPLE; i++){
- laser_value += adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
- wait_ms(2);
- }
-
- laser_value/=CALIBRE_LASER_SAMPLE;
-
- SCANNER_NOTICE("laser calibration value %"PRIi32"", laser_value);
-
- quadramp_set_1st_order_vars(&sensorboard.scanner.qr, scan_params.speed, scan_params.speed); /* set speed */
- cs_set_consign(&sensorboard.scanner.cs, 0);
- wait_ms(200);
-
-
- TELEMETRE_B = TELEMETRE_B_INIT + ((double)(424 - laser_value))*6./90.;
-
- SCANNER_NOTICE("TEL B value %2.2f", TELEMETRE_B);
-
-
- /* arm in */
-
- pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
-
-
-}
-
-
-
-
-
-void scanner_calibre_mirror(void)
-{
-
- sensorboard.flags &= ~DO_CS;
-
-
-
- /* set arm in calibre position */
- pwm_ng_set(&gen.servo3, SCANNER_POS_CALIBRE);
- wait_ms(500);
-
-
-
- /* init scanner pos */
- pwm_ng_set(SCANNER_PWM, 100);
-
- /* find rising edge of the mirror*/
- wait_ms(100);
- while (sensor_get(SCANNER_POS_SENSOR));
- wait_ms(100);
- while (!sensor_get(SCANNER_POS_SENSOR));
-
- pwm_ng_set(SCANNER_PWM, 0);
-
-
- scanner_reset_pos();
- pid_reset(&sensorboard.scanner.pid);
- cs_set_consign(&sensorboard.scanner.cs, 0);
-
-
- quadramp_reset(&sensorboard.scanner.qr);
-
-
- sensorboard.flags |= DO_CS;
-
-
- /*
- set mirror to set laser point at maximum
- distance from robot
- */
- encoders_spi_set_value_scanner(SCANNER_ENCODER, -SCANNER_OFFSET_CALIBRE);
- wait_ms(100);
-
- /* arm in */
-
- pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
-
-}
-
-
-/* arm must be in OUT position! */
-void scanner_do_scan(void){
- scan_params.working = 1;
- scan_params.dropzone_h = -1;
-
-
- quadramp_set_1st_order_vars(&sensorboard.scanner.qr, scan_params.speed, scan_params.speed); /* set speed */
-
- scan_params.sample_i = MAX_SAMPLE;
- scan_params.pos_start_scan = encoders_spi_get_value_scanner(SCANNER_ENC);
-
- 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);
-
- scan_params.last_col_n = 0;
- scan_params.last_row_n = 0;
- scan_params.last_sample = 0;
-
-
-}
-
-
-void scanner_end_process(void)
-{
- int16_t dropzone_x, dropzone_y;
- int8_t ret;
- uint32_t t1, t2;
-
- t1 = time_get_us2();
-
-
- SCANNER_NOTICE("process img algo %d", scan_params.algo);
- if (scan_params.algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE) {
- SCANNER_NOTICE("column dropzone h: %d x:%d y:%d", scan_params.drop_zone.working_zone,
- scan_params.drop_zone.center_x, scan_params.drop_zone.center_y);
-
- ret = get_column_dropzone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
- scan_params.drop_zone.working_zone, scan_params.drop_zone.center_x, scan_params.drop_zone.center_y,
- &dropzone_x, &dropzone_y);
-
- scan_params.dropzone_h = ret;
- scan_params.dropzone_x = dropzone_x;
- scan_params.dropzone_y = dropzone_y;
-
- SCANNER_NOTICE("best column h:%d x:%d y:%d",
- scan_params.dropzone_h,
- scan_params.dropzone_x, scan_params.dropzone_y);
-
-
- }
- else if (scan_params.algo == I2C_SCANNER_ALGO_CHECK_TEMPLE){
- SCANNER_NOTICE("checktemple h: %d x:%d y:%d", scan_params.check_temple.level,
- scan_params.check_temple.temple_x, scan_params.check_temple.temple_y);
- ret = is_temple_there(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
- scan_params.check_temple.level, scan_params.check_temple.temple_x, scan_params.check_temple.temple_y);
-
- scan_params.dropzone_h = ret?scan_params.check_temple.level:-1;
- }
- else if (scan_params.algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE){
- SCANNER_NOTICE("temple dropzone h: %d x:%d y:%d", scan_params.drop_zone.working_zone,
- scan_params.drop_zone.center_x, scan_params.drop_zone.center_y);
- ret = find_temple_dropzone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
- scan_params.drop_zone.working_zone, scan_params.drop_zone.center_x, scan_params.drop_zone.center_y,
- &dropzone_x, &dropzone_y);
-
- scan_params.dropzone_h = ret;
- scan_params.dropzone_x = dropzone_x;
- scan_params.dropzone_y = dropzone_y;
-
- SCANNER_NOTICE("best temple h:%d x:%d y:%d",
- scan_params.dropzone_h,
- scan_params.dropzone_x, scan_params.dropzone_y);
-
- }
-
- scan_params.working = 0;
-
- t2 = time_get_us2();
- SCANNER_NOTICE("process total time %"PRIi32"",t2-t1);
-
-
-}
-
-
-void scanner_scan_autonomous(void)
-{
- /* arm out*/
- pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
- time_wait_ms(300);
-
- scanner_do_scan();
-
- while(scan_params.sample_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;i<sizeof(h_limit)/sizeof(uint8_t);i++){
- if (H < h_limit[i]-H_MARGIN){
- H_fin = i;
- break;
- }
- }
- }
-
- //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI);
- //SCANNER_DEBUG("%f %f", m_a*180/M_PI, b*180/M_PI);
-
- //SCANNER_DEBUG("%d %f", a, b*180/M_PI);
- //SCANNER_DEBUG("%f %f %f", H, m_a*180/M_PI, offset_b);
-
- //SCANNER_DEBUG("%d %2.2f ", a, tour_pos);
-
-
- //SCANNER_DEBUG("%f %f %ld %d", H, L, tour_pos, a);
-
-
- /*
- if (row_n%2){
- //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
- tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
- }
- else{
- tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
- }
- */
- 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+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 <MAX_SAMPLE)// && row_n%2)
- //sample_tab[pos] = a>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 <MAX_SAMPLE)// && row_n%2)
- //sample_tab[pos_tmp] = min_sample;
- nop();
-
- }
-
-
- }
-
- scan_params.last_row_n = row_n;
- scan_params.last_col_n = col_n;
- scan_params.last_sample = H_fin;
-
-
- //printf("pos : %ld", pos);
- //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
-
- //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
-
- /*
- if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
- sample_tab[pos] = 0xffff;
- */
- scan_params.sample_i--;
-}
-#endif
-
-
-
-void do_scan(void * dummy)
-{
-
- int i, j;
- 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;
- uint8_t min_sample;
- double H, L;
- int32_t H_fin, L_fin;
-
- uint8_t flags;
-
- union{
- uint16_t u16;
- lookup_h_l h_l;
- }val;
-
-
-
- if (scan_params.sample_i==0)
- return;
-
-
-
- scan_params.max_column_detected = !!sensor_get(SCANNER_MAXCOLUMN_SENSOR);
-
- mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
- if (scan_params.sample_i==1 || scan_params.max_column_detected || scan_params.must_stop ){
-
- /* reset sample num in case of max column detected */
- IRQ_LOCK(flags);
- scan_params.sample_i = 1;
- IRQ_UNLOCK(flags);
-
-
- SCANNER_DEBUG("dump end enc %"PRIi32" %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 %"PRIi32" ", mot_pos);
-
- cs_set_consign(&sensorboard.scanner.cs, mot_pos);
- //pwm_ng_set(SCANNER_MOT_PWM, 0);
-
-
- /* end working */
-
- //scanner_end_process();
- if (!scan_params.must_stop && !scan_params.max_column_detected)
- process_img_to_zone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN);
-
-
- scan_params.working = 0;
-
- }
-
- mot_pos-= scan_params.pos_start_scan;
-
- a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
-
-
- tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
-
-
- if (scan_params.debug != 0)
- SCANNER_DEBUG("%ld %d ", tour_pos, a);
-
- /* lookup in precomputed array */
-
-
- j = (DIM_DIST * (telemetre_to_cm(a)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
- i = (DIM_ANGLE * tour_pos)/STEP_PER_ROUND;
-
-
- if (j<0)
- j=0;
- if (j>=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 <MAX_SAMPLE)// && row_n%2)
- //sample_tab[pos] = a>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 <MAX_SAMPLE){
- //scan_params.sample_tab[pos_tmp] = min_sample;
- nop();
- }
-
- }
-
-
- }
-
- scan_params.last_row_n = row_n;
- scan_params.last_col_n = col_n;
- scan_params.last_sample = H_fin;
-
-
- //printf("pos : %ld", pos);
- //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
-
- //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
-
- /*
- if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
- sample_tab[pos] = 0xffff;
- */
- IRQ_LOCK(flags);
- scan_params.sample_i--;
- IRQ_UNLOCK(flags);
-
-}
+++ /dev/null
-
-#define SCANNER_ENC ((void *)1)
-
-/* relative motor position from mirror axis */
-#define X 4.5
-#define Y -1.25
-
-/* length of motor arm*/
-#define l1 0.9
-
-/* length of bielle */
-#define l2 2.113
-
-/* length of mirror (rotating axe to bielle) */
-//#define l3 4.714
-#define l3 4.1613
-
-/* distance from telemetre to mirror axis */
-#define l_mirror 24.0
-
-/* higth between mirror axis and horizontal laser beam*/
-#define h_mirror -1.0
-
-
-/* transform telemeter unit to cm
- using linear approximation
- d_telemetre = a * cm + b
-*/
-//#define TELEMETRE_A (16.76)
-//#define TELEMETRE_B (-476.)
-
-
-#define TELEMETRE_A_INIT 0.067325
-#define TELEMETRE_B_INIT 23.225
-
-
-extern double TELEMETRE_A;
-extern double TELEMETRE_B;
-
-
-
-
-#define SCAN_L_MIN 15.
-#define SCAN_L_MAX 40.
-
-#define SCAN_H_MAX 40.
-#define SCAN_H_COEF (255./(SCAN_H_MAX+10.))
-
-#define SCAN_MAX_DIST 70.
-
-#define MAX_SAMPLE (1500L)
-
-
-// TRUE encoder value: 3531.75
-#define SCANNER_STEP_TOUR (14127L)
-
-//#define PIX_PER_SCAN 30L
-#define PIX_PER_SCAN (25L)
-
-void scanner_init(void);
-void scanner_set_mode(uint8_t mode);
-
-void scanner_end_process(void);
-
-void scanner_calibre_laser(void);
-void scanner_calibre_mirror(void);
-
-
-void scanner_scan_autonomous(void);
-
-int32_t encoders_spi_update_scanner(void * number);
-int32_t encoders_spi_get_value_scanner(void *number);
-
-void do_scan(void * dummy);
-
-
-
-struct scan_params{
- uint16_t sample_i;
- float offset_a;
- float offset_b;
- int speed;
- uint8_t filter;
- int32_t pos_start_scan;
-
- int32_t last_col_n;
- int32_t last_row_n;
- uint8_t last_sample;
-
- uint8_t debug;
- uint8_t sample_tab[MAX_SAMPLE+10];
- volatile uint8_t working;
- uint8_t must_stop;
-
-
- uint8_t algo;
-
-
- union {
- struct {
- 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;
- };
-
-
-
- /*
- for column drop zone answer
- */
- int8_t dropzone_h;
- int16_t dropzone_x;
- int16_t dropzone_y;
-
- uint8_t max_column_detected;
-};
-
-
-
-#define STEP_PER_ROUND 14127
-
-#define TELEMETRE_MIN 66
-#define TELEMETRE_MAX 466
-
-
-#define TELEMETRE_MIN_CM (25)
-#define TELEMETRE_MAX_CM (55)
-
-
-#define DIM_ANGLE (400/4)
-#define DIM_DIST ((TELEMETRE_MAX_CM - TELEMETRE_MIN_CM)*3)
-
-
-
-#define ANGLE_STEP (STEP_PER_ROUND/DIM_ANGLE +1)
-#define DIST_STEP ((TELEMETRE_MAX - TELEMETRE_MIN) /DIM_DIST +1)
-
-
-typedef struct _lookup_h_l{
- uint16_t h:4;
- uint16_t l:12;
-}lookup_h_l;
-
-#define telemetre_to_cm(t) ((double)(TELEMETRE_A * ((double)(t)) + TELEMETRE_B))
-
-#define SCAN_DEFAULT_SPEED 600
-
-
-extern struct scan_params scan_params;
-
-
+++ /dev/null
-/*
- * 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_
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- * Olivier MATZ <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: sensor.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $
- *
- */
-
-#include <stdlib.h>
-
-#include <aversive.h>
-#include <aversive/error.h>
-
-#include <adc.h>
-#include <scheduler.h>
-#include <ax12.h>
-#include <pwm_ng.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <blocking_detection_manager.h>
-
-#include <parse.h>
-#include <rdline.h>
-
-#include "main.h"
-#include "sensor.h"
-
-/************ ADC */
-
-struct adc_infos {
- uint16_t config;
- int16_t value;
- int16_t prev_val;
- int16_t (*filter)(struct adc_infos *, int16_t);
-};
-
-/* reach 90% of the value in 4 samples */
-int16_t rii_light(struct adc_infos *adc, int16_t val)
-{
- adc->prev_val = val + (int32_t)adc->prev_val / 2;
- return adc->prev_val / 2;
-}
-
-/* reach 90% of the value in 8 samples */
-int16_t rii_medium(struct adc_infos *adc, int16_t val)
-{
- adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4;
- return adc->prev_val / 4;
-}
-
-/* reach 90% of the value in 16 samples */
-int16_t rii_strong(struct adc_infos *adc, int16_t val)
-{
- adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8;
- return adc->prev_val / 8;
-}
-
-
-#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
-
-/* define which ADC to poll, see in sensor.h */
-static struct adc_infos adc_infos[ADC_MAX] = {
-
- [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
- [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
- [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
- [ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium },
-
- /* add adc on "cap" pins if needed */
-/* [ADC_CAP1] = { .config = ADC_CONF(10) }, */
-/* [ADC_CAP2] = { .config = ADC_CONF(11) }, */
-/* [ADC_CAP3] = { .config = ADC_CONF(12) }, */
-/* [ADC_CAP4] = { .config = ADC_CONF(13) }, */
-};
-
-static void adc_event(int16_t result);
-
-/* called every 10 ms, see init below */
-static void do_adc(void *dummy)
-{
- /* launch first conversion */
- adc_launch(adc_infos[0].config);
-}
-
-static void adc_event(int16_t result)
-{
- static uint8_t i = 0;
-
- /* filter value if needed */
- if (adc_infos[i].filter)
- adc_infos[i].value = adc_infos[i].filter(&adc_infos[i],
- result);
- else
- adc_infos[i].value = result;
-
- i ++;
- if (i >= ADC_MAX)
- i = 0;
- else
- adc_launch(adc_infos[i].config);
-}
-
-int16_t sensor_get_adc(uint8_t i)
-{
- int16_t tmp;
- uint8_t flags;
-
- IRQ_LOCK(flags);
- tmp = adc_infos[i].value;
- IRQ_UNLOCK(flags);
- return tmp;
-}
-
-/************ boolean sensors */
-
-
-struct sensor_filter {
- uint8_t filter;
- uint8_t prev;
- uint8_t thres_off;
- uint8_t thres_on;
- uint8_t cpt;
- uint8_t invert;
-};
-
-/* pullup mapping:
- * CAP 5,6,7,8
- * voltage div mapping:
- * CAP 1
- */
-static struct sensor_filter sensor_filter[SENSOR_MAX] = {
- [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */
- [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
- [S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */
- [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
- [S_CAP5] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
- [S_CAP6] = { 1, 0, 0, 1, 0, 0 }, /* 5 */
- [S_CAP7] = { 1, 0, 0, 1, 0, 0 }, /* 6 */
- [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
- [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
- [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
- [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */
- [S_RESERVED4] = { 1, 0, 0, 1, 0, 0 }, /* 11 */
- [S_RESERVED5] = { 1, 0, 0, 1, 0, 0 }, /* 12 */
- [S_RESERVED6] = { 1, 0, 0, 1, 0, 0 }, /* 13 */
- [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */
- [S_RESERVED8] = { 1, 0, 0, 1, 0, 0 }, /* 15 */
-};
-
-/* value of filtered sensors */
-static uint16_t sensor_filtered = 0;
-
-/* sensor mapping :
- * 0-3: PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
- * 4-5: PORTL 0->1 (cap5 -> cap6)
- * 6-7: PORTE 3->4 (cap7 -> cap8)
- * 8-15: reserved
- */
-
-uint16_t sensor_get_all(void)
-{
- uint16_t tmp;
- uint8_t flags;
- IRQ_LOCK(flags);
- tmp = sensor_filtered;
- IRQ_UNLOCK(flags);
- return tmp;
-}
-
-uint8_t sensor_get(uint8_t i)
-{
- uint16_t tmp = sensor_get_all();
- return (tmp & _BV(i));
-}
-
-/* get the physical value of pins */
-static uint16_t sensor_read(void)
-{
- uint16_t tmp = 0;
- tmp |= (uint16_t)((PINK & (_BV(2)|_BV(3)|_BV(4)|_BV(5))) >> 2) << 0;
- tmp |= (uint16_t)((PINL & (_BV(0)|_BV(1))) >> 0) << 4;
- tmp |= (uint16_t)((PINE & (_BV(3)|_BV(4))) >> 3) << 6;
- /* add reserved sensors here */
- return tmp;
-}
-
-/* called every 10 ms, see init below */
-static void do_boolean_sensors(void *dummy)
-{
- uint8_t i;
- uint8_t flags;
- uint16_t sensor = sensor_read();
- uint16_t tmp = 0;
-
- for (i=0; i<SENSOR_MAX; i++) {
- if ((1 << i) & sensor) {
- if (sensor_filter[i].cpt < sensor_filter[i].filter)
- sensor_filter[i].cpt++;
- if (sensor_filter[i].cpt >= sensor_filter[i].thres_on)
- sensor_filter[i].prev = 1;
- }
- else {
- if (sensor_filter[i].cpt > 0)
- sensor_filter[i].cpt--;
- if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
- sensor_filter[i].prev = 0;
- }
-
- if (sensor_filter[i].prev) {
- tmp |= (1UL << i);
- }
- }
- IRQ_LOCK(flags);
- sensor_filtered = tmp;
- IRQ_UNLOCK(flags);
-}
-
-
-
-/************ global sensor init */
-#define BACKGROUND_ADC 0
-
-/* called every 10 ms, see init below */
-static void do_sensors(void *dummy)
-{
- if (BACKGROUND_ADC)
- do_adc(NULL);
- do_boolean_sensors(NULL);
-}
-
-void sensor_init(void)
-{
- adc_init();
- if (BACKGROUND_ADC)
- adc_register_event(adc_event);
- /* CS EVENT */
- scheduler_add_periodical_event_priority(do_sensors, NULL,
- 10000L / SCHEDULER_UNIT,
- ADC_PRIO);
-
-}
-
+++ /dev/null
-/*
- * Copyright Droids Corporation (2009)
- * Olivier MATZ <zer0@droids-corp.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: sensor.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $
- *
- */
-
-/* synchronize with sensor.c */
-#define ADC_CSENSE1 0
-#define ADC_CSENSE2 1
-#define ADC_CSENSE3 2
-#define ADC_CSENSE4 3
-#define ADC_MAX 4
-
-/* synchronize with sensor.c */
-#define S_CAP1 0
-#define S_CAP2 1
-#define S_CAP3 2
-#define S_CAP4 3
-#define S_CAP5 4
-#define S_CAP6 5
-#define S_CAP7 6
-#define S_CAP8 7
-#define S_RESERVED1 8
-#define S_RESERVED2 9
-#define S_RESERVED3 10
-#define S_RESERVED4 11
-#define S_RESERVED5 12
-#define S_RESERVED6 13
-#define S_RESERVED7 14
-#define S_RESERVED8 15
-#define SENSOR_MAX 16
-
-void sensor_init(void);
-
-/* get filtered values for adc */
-int16_t sensor_get_adc(uint8_t i);
-
-/* get filtered values of boolean sensors */
-uint16_t sensor_get_all(void);
-uint8_t sensor_get(uint8_t i);
+++ /dev/null
-/*
- * 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
-
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/* \r
- * Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
- * \r
- * This program is free software; you can redistribute it and/or modify\r
- * it under the terms of the GNU General Public License as published by\r
- * the Free Software Foundation; either version 2 of the License, or\r
- * (at your option) any later version.\r
- *\r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- *\r
- * You should have received a copy of the GNU General Public License\r
- * along with this program; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- *\r
- * Revision : $Id: uart_config.h,v 1.3 2009-05-27 20:04:07 zer0 Exp $\r
- *\r
- */\r
-\r
-/* Droids-corp 2004 - Zer0\r
- * config for uart module\r
- */\r
-\r
-#ifndef UART_CONFIG_H\r
-#define UART_CONFIG_H\r
-\r
-/*\r
- * UART1 definitions \r
- */\r
-\r
-/* compile uart1 fonctions, undefine it to pass compilation */\r
-#define UART1_COMPILE \r
-\r
-/* enable uart1 if == 1, disable if == 0 */\r
-#define UART1_ENABLED 1\r
-\r
-/* enable uart1 interrupts if == 1, disable if == 0 */\r
-#define UART1_INTERRUPT_ENABLED 1\r
-\r
-#define UART1_BAUDRATE 57600\r
-\r
-/* \r
- * if you enable this, the maximum baudrate you can reach is \r
- * higher, but the precision is lower. \r
- */\r
-#define UART1_USE_DOUBLE_SPEED 1\r
-\r
-#define UART1_RX_FIFO_SIZE 64\r
-#define UART1_TX_FIFO_SIZE 64\r
-#define UART1_NBITS 8\r
-\r
-#define UART1_PARITY UART_PARTITY_NONE\r
-\r
-#define UART1_STOP_BIT UART_STOP_BITS_1\r
-\r
-\r
-/*\r
- * UART3 definitions \r
- */\r
-\r
-/* compile uart3 fonctions, undefine it to pass compilation */\r
-#define UART3_COMPILE \r
-\r
-/* enable uart3 if == 1, disable if == 0 */\r
-#define UART3_ENABLED 1\r
-\r
-/* enable uart3 interrupts if == 1, disable if == 0 */\r
-#define UART3_INTERRUPT_ENABLED 1\r
-\r
-#define UART3_BAUDRATE 57600\r
-\r
-/* \r
- * if you enable this, the maximum baudrate you can reach is \r
- * higher, but the precision is lower. \r
- */\r
-#define UART3_USE_DOUBLE_SPEED 1\r
-//#define UART3_USE_DOUBLE_SPEED 1\r
-\r
-#define UART3_RX_FIFO_SIZE 64\r
-#define UART3_TX_FIFO_SIZE 64\r
-//#define UART3_NBITS 5\r
-//#define UART3_NBITS 6\r
-//#define UART3_NBITS 7\r
-#define UART3_NBITS 8\r
-//#define UART3_NBITS 9\r
-\r
-#define UART3_PARITY UART_PARTITY_NONE\r
-//#define UART3_PARITY UART_PARTITY_ODD\r
-//#define UART3_PARITY UART_PARTITY_EVEN\r
-\r
-#define UART3_STOP_BIT UART_STOP_BITS_1\r
-//#define UART3_STOP_BIT UART_STOP_BITS_2\r
-\r
-\r
-\r
-\r
-/* .... same for uart 1, 2, 3 ... */\r
-\r
-#endif\r
-\r
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
/*
- * Copyright Droids Corporation (2009)
+ * Copyright Droids Corporation (2010)
* Olivier Matz <zer0@droids-corp.org>
*
* This program is free software; you can redistribute it and/or modify
*
*/
+#include <math.h>
+
#include <aversive.h>
#include <aversive/wait.h>
#include <pwm_ng.h>
#include <parse.h>
#include <rdline.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <circles.h>
#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<<FRAME_RING_ORDER)
};
static struct frame_status static_beacon;
+static struct frame_status opp_beacon;
+static uint16_t tick = 0;
+#define MIN_DIST 200.
+#define MAX_DIST 3500.
+/* in ticks (=CS_PERIOD), age before the entry is removed from ring */
+#define MAX_CAP_AGE 30
/********************** CS */
#define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US)) /* too fast = 100 tr/s */
/* in tr / 1000s */
-#define CS_CONSIGN (25 * 1000L)
+#define CS_CONSIGN (10 * 1000L)
+
+/* 5% tolerance to validate captures, period is in */
+#define TIM3_UNIT 250000000L
+#define MOTOR_PERIOD_MIN ((uint32_t)((250000000L/CS_CONSIGN) * 0.95))
+#define MOTOR_PERIOD_MAX ((uint32_t)((250000000L/CS_CONSIGN) * 1.05))
/* pwm for laser:
* - clear on timer compare (CTC)
struct beacon_tsop beacon_tsop;
+static uint32_t current_motor_period;
+
void debug_serial(void)
{
#if 0
uint16_t diff_time = cur_time - status->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;
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;
}
/* 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();
}
/* decode frame */
-SIGNAL(SIG_TSOP) {
+SIGNAL(SIG_TSOP_STA) {
static uint8_t running = 0;
/* tsop status */
ref_time = ICR3;
cur_time = TCNT3;
- cur_tsop = TSOP_READ();
+ cur_tsop = TSOP_STA_READ();
/* avoid interruption stacking */
if (running)
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)
{
}
}
- /* 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)
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);
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,
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();
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;
*
*/
-#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];
/*
- * 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
* 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 <zer0@droids-corp.org>
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
*/
#include <stdio.h>
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
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 */
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)
*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
* Olivier MATZ <zer0@droids-corp.org>
*/
+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 */ );
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);
git.droids-corp.org / aversive.git / commitdiff