--- /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 is not set
+# CONFIG_FDEVOPEN_COMPAT is not set
+CONFIG_NO_PRINTF=y
+# CONFIG_MINIMAL_PRINTF is not set
+# CONFIG_STANDARD_PRINTF is not set
+CONFIG_FORMAT_IHEX=y
+# CONFIG_FORMAT_SREC is not set
+# CONFIG_FORMAT_BINARY is not set
+
+#
+# Base modules
+#
+
+#
+# Enable math library in generation options to see all modules
+#
+# CONFIG_MODULE_CIRBUF is not set
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+# CONFIG_MODULE_GEOMETRY is not set
+# CONFIG_MODULE_GEOMETRY_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER is not set
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+CONFIG_MODULE_SCHEDULER_TIMER0=y
+# CONFIG_MODULE_SCHEDULER_MANUAL is not set
+# CONFIG_MODULE_TIME is not set
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
+# CONFIG_MODULE_UART is not set
+# CONFIG_MODULE_UART_9BITS is not set
+# CONFIG_MODULE_UART_CREATE_CONFIG is not set
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+# CONFIG_MODULE_TIMER is not set
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
+# CONFIG_MODULE_ADC is not set
+# CONFIG_MODULE_ADC_CREATE_CONFIG is not set
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+# CONFIG_MODULE_VT100 is not set
+# CONFIG_MODULE_RDLINE is not set
+# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set
+# CONFIG_MODULE_RDLINE_KILL_BUF is not set
+# CONFIG_MODULE_RDLINE_HISTORY is not set
+# CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+
+#
+# Filters
+#
+# CONFIG_MODULE_PID is not set
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+# CONFIG_MODULE_QUADRAMP is not set
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+
+#
+# Some radio devices require SPI to be activated
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+
+#
+# Crypto modules depend on utils module
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+
+#
+# Encoding modules depend on utils module
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+
+#
+# Debug modules depend on utils module
+#
+# CONFIG_MODULE_DIAGNOSTIC is not set
+# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
+# CONFIG_MODULE_ERROR is not set
+# CONFIG_MODULE_ERROR_CREATE_CONFIG is not set
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
--- /dev/null
+TARGET = main
+
+# repertoire des modules
+AVERSIVE_DIR = ../../..
+# VALUE, absolute or relative path : example ../.. #
+
+CFLAGS += -Werror
+
+# atm128
+# address is 0xf000 (in words)
+# LDFLAGS += -Wl,--section-start=.text=1e000
+# UART_NUM = 0
+
+# atm2560
+# address is 0x1f800 (in words)
+LDFLAGS += -Wl,--section-start=.text=3f000
+UART_NUM = 1
+
+CFLAGS += -DUART_NUM=$(UART_NUM)
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC = $(TARGET).c
+
+# List Assembler source files here.
+# Make them always end in a capital .S. Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC =
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep
+ echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+ $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+
--- /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:06 zer0 Exp $
+ *
+ */
+
+/*
+ * A simple bootloader example.
+ */
+
+#include <aversive.h>
+#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
+
+#include <stdlib.h>
+#include <string.h>
+#include <util/crc16.h>
+#include <avr/boot.h>
+
+#define BRAKE_DDR() do { DDRJ |= 0xF0; } while(0)
+#define BRAKE_ON() do { PORTJ |= 0xF0; } while(0)
+#define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0)
+
+#define LED1_ON() sbi(PORTJ, 2)
+#define LED1_OFF() cbi(PORTJ, 2)
+
+#define LED2_ON() sbi(PORTJ, 3)
+#define LED2_OFF() cbi(PORTJ, 3)
+
+
+#define NOECHO
+
+#ifdef NOECHO
+#define echo(c) do {} while(0)
+#else
+#define echo(c) uart_send(c)
+#endif
+
+#if UART_NUM == 0
+
+#define UCSRxA UCSR0A
+#define UCSRxB UCSR0B
+#define UCSRxC UCSR0C
+#define RXCx RXC0
+#define UDRx UDR0
+#define UDREx UDRE0
+#define U2Xx U2X0
+#define RXENx RXEN0
+#define TXENx TXEN0
+#define UCSZx0 UCSZ00
+#define UCSZx1 UCSZ01
+#define UBRRx UBRR0
+
+#elif UART_NUM == 1
+
+#define UCSRxA UCSR1A
+#define UCSRxB UCSR1B
+#define UCSRxC UCSR1C
+#define RXCx RXC1
+#define UDRx UDR1
+#define UDREx UDRE1
+#define U2Xx U2X1
+#define RXENx RXEN1
+#define TXENx TXEN1
+#define UCSZx0 UCSZ10
+#define UCSZx1 UCSZ11
+#define UBRRx UBRR1
+
+#elif UART_NUM == 2
+
+#define UCSRxA UCSR2A
+#define UCSRxB UCSR2B
+#define UCSRxC UCSR2C
+#define RXCx RXC2
+#define UDRx UDR2
+#define UDREx UDRE2
+#define U2Xx U2X2
+#define RXENx RXEN2
+#define TXENx TXEN2
+#define UCSZx0 UCSZ20
+#define UCSZx1 UCSZ21
+#define UBRRx UBRR2
+
+#elif UART_NUM == 3
+
+#define UCSRxA UCSR3A
+#define UCSRxB UCSR3B
+#define UCSRxC UCSR3C
+#define RXCx RXC3
+#define UDRx UDR3
+#define UDREx UDRE3
+#define U2Xx U2X3
+#define RXENx RXEN3
+#define TXENx TXEN3
+#define UCSZx0 UCSZ30
+#define UCSZx1 UCSZ31
+#define UBRRx UBRR3
+
+#endif
+
+
+static char uart_recv(void)
+{
+ while ( !(UCSRxA & (1<<RXCx)) ) ;
+ return UDRx;
+}
+
+static void uart_send(char c)
+{
+ while ( !( UCSRxA & (1<<UDREx)) ) ;
+ UDRx = c;
+}
+
+static void uart_puts(const char *buf)
+{
+ while (*buf)
+ uart_send(*(buf++));
+}
+
+static int8_t bootloader_query_hex(uint32_t *val)
+{
+ uint32_t tmp = 0;
+ int c;
+
+ while (1) {
+ c = uart_recv();
+ echo(c);
+
+ if (c == '\n' || c == '\r') {
+ *val = tmp;
+ return 0;
+ }
+ else if (c >= '0' && c <= '9') {
+ tmp <<= 4;
+ tmp += (c - '0');
+ }
+ else if (c >= 'a' && c <= 'f') {
+ tmp <<= 4;
+ tmp += (c - 'a' + 10);
+ }
+ else if (c >= 'A' && c <= 'F') {
+ tmp <<= 4;
+ tmp += (c - 'A' + 10);
+ }
+ else
+ return -1;
+ }
+ return 0;
+}
+
+/* launch application */
+static void launch_app(void)
+{
+ uart_puts("Boot...");
+ MCUCR = (1 << IVCE);
+ MCUCR = (0 << IVSEL);
+ reset();
+}
+
+static void disp_digit(uint8_t x)
+{
+ if (x < 10)
+ x += '0';
+ else
+ x += 'a' - 10 ;
+ uart_send(x);
+}
+
+static void disp_hex8(uint8_t x)
+{
+ disp_digit(x>>4);
+ disp_digit(x&0xf);
+}
+
+static void disp_hex16(uint16_t x)
+{
+ disp_hex8(x>>8);
+ disp_hex8(x);
+}
+
+static void crc_app(void)
+{
+ uint32_t start_addr, addr, size;
+ uint8_t c;
+ uint16_t crc = 0xffff;
+ uint16_t sum = 0;
+
+ uart_puts("addr?\r\n");
+ if (bootloader_query_hex(&start_addr))
+ goto fail;
+ if (start_addr > FLASHEND)
+ goto fail;
+ uart_puts("size?\r\n");
+ if (bootloader_query_hex(&size))
+ goto fail;
+ if (start_addr + size > FLASHEND)
+ goto fail;
+ for (addr=start_addr; addr<start_addr+size; addr++) {
+#if 0
+ /* ignore the 2nd page, it contains microb infos */
+ if (addr >= 256 && addr < 512)
+ continue;
+#endif
+ c = pgm_read_byte_far(addr);
+ crc = _crc_ccitt_update(crc, c);
+ sum += c;
+ }
+ disp_hex16(crc);
+ disp_hex16(sum);
+ return;
+ fail:
+ uart_puts("KO");
+}
+
+static void read32(void)
+{
+ uint32_t start_addr, val = 0;
+ uint8_t c, i;
+
+ uart_puts("addr?\r\n");
+ if (bootloader_query_hex(&start_addr))
+ goto fail;
+ if (start_addr > FLASHEND)
+ goto fail;
+ for (i=0; i<4; i++) {
+ c = pgm_read_byte_far(start_addr+i);
+ val <<= 8;
+ val |= c;
+ }
+ disp_hex16(val);
+ return;
+ fail:
+ uart_puts("KO");
+}
+
+static void prog_page(void)
+{
+ int c;
+ uint32_t addr;
+ uint16_t i;
+ uint16_t crc = 0xffff;
+ uint16_t sum = 0;
+ uint8_t buf[SPM_PAGESIZE];
+
+#define SPM_PAGEMASK ((uint32_t)SPM_PAGESIZE-1)
+ uart_puts("addr?\r\n");
+ if (bootloader_query_hex(&addr))
+ goto fail;
+ if (addr > FLASHEND)
+ goto fail;
+ /* start_addr must be page aligned */
+ if (addr & SPM_PAGEMASK)
+ goto fail;
+
+ uart_puts("ok\r\n");
+
+ PORTJ = 0xF0;
+
+ /* data is received like the .bin format (which is already in
+ * little endian) */
+ for (i=0; i<SPM_PAGESIZE; i++) {
+ c = uart_recv();
+ crc = _crc_ccitt_update(crc, c);
+ sum += c;
+ buf[i] = c;
+ }
+ disp_hex16(crc);
+ disp_hex16(sum);
+ uart_puts(" (y?)\r\n");
+ c = uart_recv();
+ if (c != 'y')
+ goto fail;
+
+ /* erase page */
+ eeprom_busy_wait();
+ boot_page_erase(addr);
+ boot_spm_busy_wait();
+
+ /* Set up little-endian word and fill tmp buf. */
+ for (i=0; i<SPM_PAGESIZE; i+=2) {
+ uint16_t w = buf[i] + ((uint16_t)(buf[i+1]) << 8);
+ boot_page_fill(addr + i, w);
+ }
+
+ PORTJ = 0xFC;
+
+ boot_page_write(addr);
+ boot_spm_busy_wait();
+
+ /* Reenable RWW-section again. We need this if we want to jump
+ * back to the application after bootloading. */
+ boot_rww_enable();
+
+ uart_puts("OK");
+ return;
+ fail:
+ uart_puts("KO");
+}
+
+int main(void)
+{
+ int c;
+ uint32_t i=0;
+
+ /* disable all motors and switch on leds */
+ DDRJ = 0xFC;
+ PORTJ = 0xFC;
+
+ UCSRxA = _BV(U2Xx);
+ UCSRxB = _BV(RXENx) | _BV(TXENx);
+ UCSRxC = _BV(UCSZx1) | _BV(UCSZx0); /* 8 bits no parity 1 stop */
+ UBRRx = 34; /* 57600 at 16 Mhz */
+
+ /* move interrupt vector in bootloader section */
+ MCUCR = (1 << IVCE);
+ MCUCR = (1 << IVSEL);
+
+ sei();
+
+ uart_puts("\r\ncmd> ");
+
+ /* timeout */
+ while ( !(UCSRxA & (1<<RXCx)) ) {
+ i++;
+ if (i>1000000) /* wait about 1 sec */
+ launch_app();
+ }
+
+ while (1) {
+ uart_puts("\r\ncmd> ");
+ c = uart_recv();
+ if (c == 'x')
+ launch_app();
+ else if (c == 'c')
+ crc_app();
+ else if (c == 'p')
+ prog_page();
+ else if (c == 'd')
+ read32();
+ else
+ uart_puts("p:prog_page c:crc x:exec d:dump32");
+ }
+
+ return 0;
+}
--- /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:06 zer0 Exp $\r
+ *\r
+ */\r
+\r
+/* Droids-corp 2004 - Zer0\r
+ * config for uart module\r
+ */\r
+\r
+#ifndef UART_CONFIG_H\r
+#define UART_CONFIG_H\r
+\r
+#if UART_NUM == 0\r
+\r
+/* compile uart0 fonctions, undefine it to pass compilation */\r
+#define UART0_COMPILE \r
+\r
+/* enable uart0 if == 1, disable if == 0 */\r
+#define UART0_ENABLED 1\r
+\r
+/* enable uart0 interrupts if == 1, disable if == 0 */\r
+#define UART0_INTERRUPT_ENABLED 1\r
+\r
+#define UART0_BAUDRATE 57600\r
+\r
+#define UART0_USE_DOUBLE_SPEED 0\r
+\r
+#define UART0_RX_FIFO_SIZE 64\r
+#define UART0_TX_FIFO_SIZE 16\r
+\r
+#define UART0_NBITS 8\r
+#define UART0_PARITY UART_PARTITY_NONE\r
+#define UART0_STOP_BIT UART_STOP_BITS_1\r
+\r
+#elif UART_NUM == 1\r
+\r
+/* compile uart1 fonctions, undefine it to pass compilation */\r
+#define UART1_COMPILE \r
+\r
+/* enable uart1 if == 1, disable if == 0 */\r
+#define UART1_ENABLED 1\r
+\r
+/* enable uart1 interrupts if == 1, disable if == 0 */\r
+#define UART1_INTERRUPT_ENABLED 1\r
+\r
+#define UART1_BAUDRATE 57600\r
+\r
+#define UART1_USE_DOUBLE_SPEED 0\r
+\r
+#define UART1_RX_FIFO_SIZE 32\r
+#define UART1_TX_FIFO_SIZE 4\r
+\r
+#define UART1_NBITS 8\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+\r
+#elif UART_NUM == 2\r
+\r
+/* compile uart2 fonctions, undefine it to pass compilation */\r
+#define UART2_COMPILE \r
+\r
+/* enable uart2 if == 1, disable if == 0 */\r
+#define UART2_ENABLED 1\r
+\r
+/* enable uart2 interrupts if == 1, disable if == 0 */\r
+#define UART2_INTERRUPT_ENABLED 1\r
+\r
+#define UART2_BAUDRATE 57600\r
+\r
+#define UART2_USE_DOUBLE_SPEED 0\r
+\r
+#define UART2_RX_FIFO_SIZE 32\r
+#define UART2_TX_FIFO_SIZE 4\r
+\r
+#define UART2_NBITS 8\r
+#define UART2_PARITY UART_PARTITY_NONE\r
+#define UART2_STOP_BIT UART_STOP_BITS_1\r
+\r
+#elif UART_NUM == 3\r
+\r
+/* compile uart3 fonctions, undefine it to pass compilation */\r
+#define UART3_COMPILE \r
+\r
+/* enable uart3 if == 1, disable if == 0 */\r
+#define UART3_ENABLED 1\r
+\r
+/* enable uart3 interrupts if == 1, disable if == 0 */\r
+#define UART3_INTERRUPT_ENABLED 1\r
+\r
+#define UART3_BAUDRATE 57600\r
+\r
+#define UART3_USE_DOUBLE_SPEED 0\r
+\r
+#define UART3_RX_FIFO_SIZE 32\r
+#define UART3_TX_FIFO_SIZE 4\r
+\r
+#define UART3_NBITS 8\r
+#define UART3_PARITY UART_PARTITY_NONE\r
+#define UART3_STOP_BIT UART_STOP_BITS_1\r
+\r
+#endif /* uart num */\r
+\r
+#endif\r
+\r
--- /dev/null
+/* Default linker script, for normal executables */
+OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr")
+OUTPUT_ARCH(avr:6)
+MEMORY
+{
+ text (rx) : ORIGIN = 0, LENGTH = 1024K
+ data (rw!x) : ORIGIN = 0x800200, LENGTH = 0xfe00
+ eeprom (rw!x) : ORIGIN = 0x810000, LENGTH = 64K
+ fuse (rw!x) : ORIGIN = 0x820000, LENGTH = 1K
+ lock (rw!x) : ORIGIN = 0x830000, LENGTH = 1K
+ signature (rw!x) : ORIGIN = 0x840000, LENGTH = 1K
+}
+SECTIONS
+{
+ /* Read-only sections, merged into text segment: */
+ .hash : { *(.hash) }
+ .dynsym : { *(.dynsym) }
+ .dynstr : { *(.dynstr) }
+ .gnu.version : { *(.gnu.version) }
+ .gnu.version_d : { *(.gnu.version_d) }
+ .gnu.version_r : { *(.gnu.version_r) }
+ .rel.init : { *(.rel.init) }
+ .rela.init : { *(.rela.init) }
+ .rel.text :
+ {
+ *(.rel.text)
+ *(.rel.text.*)
+ *(.rel.gnu.linkonce.t*)
+ }
+ .rela.text :
+ {
+ *(.rela.text)
+ *(.rela.text.*)
+ *(.rela.gnu.linkonce.t*)
+ }
+ .rel.fini : { *(.rel.fini) }
+ .rela.fini : { *(.rela.fini) }
+ .rel.rodata :
+ {
+ *(.rel.rodata)
+ *(.rel.rodata.*)
+ *(.rel.gnu.linkonce.r*)
+ }
+ .rela.rodata :
+ {
+ *(.rela.rodata)
+ *(.rela.rodata.*)
+ *(.rela.gnu.linkonce.r*)
+ }
+ .rel.data :
+ {
+ *(.rel.data)
+ *(.rel.data.*)
+ *(.rel.gnu.linkonce.d*)
+ }
+ .rela.data :
+ {
+ *(.rela.data)
+ *(.rela.data.*)
+ *(.rela.gnu.linkonce.d*)
+ }
+ .rel.ctors : { *(.rel.ctors) }
+ .rela.ctors : { *(.rela.ctors) }
+ .rel.dtors : { *(.rel.dtors) }
+ .rela.dtors : { *(.rela.dtors) }
+ .rel.got : { *(.rel.got) }
+ .rela.got : { *(.rela.got) }
+ .rel.bss : { *(.rel.bss) }
+ .rela.bss : { *(.rela.bss) }
+ .rel.plt : { *(.rel.plt) }
+ .rela.plt : { *(.rela.plt) }
+ /* Internal text space or external memory. */
+ .text :
+ {
+ *(.vectors)
+ KEEP(*(.vectors))
+ . = 256 + ALIGN(256); /* placeholder for misc microb infos */
+ /* For data that needs to reside in the lower 64k of progmem. */
+ *(.progmem.gcc*)
+ *(.progmem*)
+ . = ALIGN(2048);
+ __trampolines_start = . ;
+ /* The jump trampolines for the 16-bit limited relocs will reside here. */
+ *(.trampolines)
+ *(.trampolines*)
+ __trampolines_end = . ;
+ /* For future tablejump instruction arrays for 3 byte pc devices.
+ We don't relax jump/call instructions within these sections. */
+ *(.jumptables)
+ *(.jumptables*)
+ /* For code that needs to reside in the lower 128k progmem. */
+ *(.lowtext)
+ *(.lowtext*)
+ __ctors_start = . ;
+ *(.ctors)
+ __ctors_end = . ;
+ __dtors_start = . ;
+ *(.dtors)
+ __dtors_end = . ;
+ KEEP(SORT(*)(.ctors))
+ KEEP(SORT(*)(.dtors))
+ /* From this point on, we don't bother about wether the insns are
+ below or above the 16 bits boundary. */
+ *(.init0) /* Start here after reset. */
+ KEEP (*(.init0))
+ *(.init1)
+ KEEP (*(.init1))
+ *(.init2) /* Clear __zero_reg__, set up stack pointer. */
+ KEEP (*(.init2))
+ *(.init3)
+ KEEP (*(.init3))
+ *(.init4) /* Initialize data and BSS. */
+ KEEP (*(.init4))
+ *(.init5)
+ KEEP (*(.init5))
+ *(.init6) /* C++ constructors. */
+ KEEP (*(.init6))
+ *(.init7)
+ KEEP (*(.init7))
+ *(.init8)
+ KEEP (*(.init8))
+ *(.init9) /* Call main(). */
+ KEEP (*(.init9))
+ *(.text.*) /* trucs de gcc ? */
+ . = ALIGN(2048);
+ uart*(.text)
+ pwm*(.text)
+ parse*(.text)
+ rdline*(.text)
+ vt100*(.text)
+ scheduler*(.text)
+ control_system*(.text)
+ pid*(.text)
+ f08*(.text)
+ f16*(.text)
+ f32*(.text)
+ f64*(.text)
+ vect2*(.text)
+ quadramp*(.text)
+ blocking*(.text)
+ obstacle*(.text)
+ trajectory*(.text)
+ position*(.text)
+ adc*(.text)
+ robot*(.text)
+ error*(.text)
+ encoders*(.text)
+ time*(.text)
+ cirbuf*(.text)
+ i2c*(.text)
+ spi*(.text)
+ ax12*(.text)
+ . = ALIGN(2048);
+ /* some libc stuff */
+ str*(.text)
+ mem*(.text)
+ printf*(.text)
+ vfprintf*(.text)
+ sprintf*(.text)
+ snprintf*(.text)
+ malloc*(.text)
+ free*(.text)
+ fdevopen*(.text)
+ fputc*(.text)
+ . = ALIGN(2048);
+ *(.text)
+ . = ALIGN(2);
+ *(.fini9) /* _exit() starts here. */
+ KEEP (*(.fini9))
+ *(.fini8)
+ KEEP (*(.fini8))
+ *(.fini7)
+ KEEP (*(.fini7))
+ *(.fini6) /* C++ destructors. */
+ KEEP (*(.fini6))
+ *(.fini5)
+ KEEP (*(.fini5))
+ *(.fini4)
+ KEEP (*(.fini4))
+ *(.fini3)
+ KEEP (*(.fini3))
+ *(.fini2)
+ KEEP (*(.fini2))
+ *(.fini1)
+ KEEP (*(.fini1))
+ *(.fini0) /* Infinite loop after program termination. */
+ KEEP (*(.fini0))
+ _etext = . ;
+ } > text
+ .data : AT (ADDR (.text) + SIZEOF (.text))
+ {
+ PROVIDE (__data_start = .) ;
+ *(.data)
+ *(.data*)
+ *(.rodata) /* We need to include .rodata here if gcc is used */
+ *(.rodata*) /* with -fdata-sections. */
+ *(.gnu.linkonce.d*)
+ . = ALIGN(2);
+ _edata = . ;
+ PROVIDE (__data_end = .) ;
+ } > data
+ .bss SIZEOF(.data) + ADDR(.data) :
+ {
+ PROVIDE (__bss_start = .) ;
+ *(.bss)
+ *(.bss*)
+ *(COMMON)
+ PROVIDE (__bss_end = .) ;
+ } > data
+ __data_load_start = LOADADDR(.data);
+ __data_load_end = __data_load_start + SIZEOF(.data);
+ /* Global data not cleared after reset. */
+ .noinit SIZEOF(.bss) + ADDR(.bss) :
+ {
+ PROVIDE (__noinit_start = .) ;
+ *(.noinit*)
+ PROVIDE (__noinit_end = .) ;
+ _end = . ;
+ PROVIDE (__heap_start = .) ;
+ } > data
+ .eeprom :
+ {
+ *(.eeprom*)
+ __eeprom_end = . ;
+ } > eeprom
+ .fuse :
+ {
+ KEEP(*(.fuse))
+ KEEP(*(.lfuse))
+ KEEP(*(.hfuse))
+ KEEP(*(.efuse))
+ } > fuse
+ .lock :
+ {
+ KEEP(*(.lock*))
+ } > lock
+ .signature :
+ {
+ KEEP(*(.signature*))
+ } > signature
+ /* Stabs debugging sections. */
+ .stab 0 : { *(.stab) }
+ .stabstr 0 : { *(.stabstr) }
+ .stab.excl 0 : { *(.stab.excl) }
+ .stab.exclstr 0 : { *(.stab.exclstr) }
+ .stab.index 0 : { *(.stab.index) }
+ .stab.indexstr 0 : { *(.stab.indexstr) }
+ .comment 0 : { *(.comment) }
+ /* DWARF debug sections.
+ Symbols in the DWARF debugging sections are relative to the beginning
+ of the section so we begin them at 0. */
+ /* DWARF 1 */
+ .debug 0 : { *(.debug) }
+ .line 0 : { *(.line) }
+ /* GNU DWARF 1 extensions */
+ .debug_srcinfo 0 : { *(.debug_srcinfo) }
+ .debug_sfnames 0 : { *(.debug_sfnames) }
+ /* DWARF 1.1 and DWARF 2 */
+ .debug_aranges 0 : { *(.debug_aranges) }
+ .debug_pubnames 0 : { *(.debug_pubnames) }
+ /* DWARF 2 */
+ .debug_info 0 : { *(.debug_info) *(.gnu.linkonce.wi.*) }
+ .debug_abbrev 0 : { *(.debug_abbrev) }
+ .debug_line 0 : { *(.debug_line) }
+ .debug_frame 0 : { *(.debug_frame) }
+ .debug_str 0 : { *(.debug_str) }
+ .debug_loc 0 : { *(.debug_loc) }
+ .debug_macinfo 0 : { *(.debug_macinfo) }
+}
--- /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: eeprom_mapping.h,v 1.2 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+#ifndef _EEPROM_MAPPING_H_
+#define _EEPROM_MAPPING_H_
+
+#define EEPROM_MAGIC_MAINBOARD 1
+#define EEPROM_MAGIC_MECHBOARD 2
+#define EEPROM_MAGIC_SENSORBOARD 3
+
+#define EEPROM_MAGIC_ADDRESS ((uint8_t *)0x100)
+
+#define EEPROM_TIME_ADDRESS ((uint16_t *)0x110)
+
+#endif
--- /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_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+#ifndef _I2C_COMMANDS_H_
+#define _I2C_COMMANDS_H_
+
+#define I2C_MAINBOARD_ADDR 1
+#define I2C_MECHBOARD_ADDR 2
+#define I2C_SENSORBOARD_ADDR 3
+
+#define I2C_LEFT_SIDE 0
+#define I2C_RIGHT_SIDE 1
+#define I2C_AUTO_SIDE 2 /* for prepare_pickup */
+#define I2C_CENTER_SIDE 3 /* for prepare_pickup */
+
+#define I2C_COLOR_RED 0
+#define I2C_COLOR_GREEN 1
+
+#define I2C_AUTOBUILD_DEFAULT_DIST 210
+
+struct i2c_cmd_hdr {
+ uint8_t cmd;
+};
+
+/****/
+/* commands that do not need and answer */
+/****/
+
+#define I2C_CMD_GENERIC_LED_CONTROL 0x00
+
+struct i2c_cmd_led_control {
+ struct i2c_cmd_hdr hdr;
+ uint8_t led_num:7;
+ uint8_t state:1;
+};
+
+/****/
+
+#define I2C_CMD_GENERIC_SET_COLOR 0x01
+
+struct i2c_cmd_generic_color {
+ struct i2c_cmd_hdr hdr;
+ uint8_t color;
+};
+
+/****/
+
+#define I2C_CMD_MECHBOARD_SET_MODE 0x02
+
+struct i2c_cmd_mechboard_set_mode {
+ struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_MODE_MANUAL 0x00
+#define I2C_MECHBOARD_MODE_HARVEST 0x01
+#define I2C_MECHBOARD_MODE_PREPARE_PICKUP 0x02
+#define I2C_MECHBOARD_MODE_PICKUP 0x03
+#define I2C_MECHBOARD_MODE_PREPARE_BUILD 0x04
+#define I2C_MECHBOARD_MODE_AUTOBUILD 0x05
+#define I2C_MECHBOARD_MODE_WAIT 0x06
+#define I2C_MECHBOARD_MODE_INIT 0x07
+#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08
+#define I2C_MECHBOARD_MODE_GET_LINTEL 0x09
+#define I2C_MECHBOARD_MODE_PUT_LINTEL 0x0A
+#define I2C_MECHBOARD_MODE_PREPARE_EJECT 0x0B
+#define I2C_MECHBOARD_MODE_EJECT 0x0C
+#define I2C_MECHBOARD_MODE_CLEAR 0x0D
+#define I2C_MECHBOARD_MODE_LAZY_HARVEST 0x0E
+#define I2C_MECHBOARD_MODE_LOADED 0x0F
+#define I2C_MECHBOARD_MODE_PREPARE_INSIDE 0x10
+#define I2C_MECHBOARD_MODE_STORE 0x11
+#define I2C_MECHBOARD_MODE_LAZY_PICKUP 0x12
+#define I2C_MECHBOARD_MODE_MANIVELLE 0x13
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE 0x14
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC 0x15
+#define I2C_MECHBOARD_MODE_EXIT 0xFF
+ uint8_t mode;
+ union {
+ struct {
+
+ } manual;
+
+ struct {
+ uint8_t side;
+ uint8_t next_mode;
+ } prep_pickup;
+
+ struct {
+ uint8_t level_l;
+ uint8_t level_r;
+ } prep_build;
+
+ struct {
+ uint8_t side;
+ } push_temple_disc;
+
+ struct {
+ uint8_t level;
+ } push_temple;
+
+ struct {
+ uint8_t level_left;
+ uint8_t level_right;
+ uint8_t count_left;
+ uint8_t count_right;
+ uint8_t distance_left;
+ uint8_t distance_right;
+ uint8_t do_lintel;
+ } autobuild;
+
+ struct {
+ uint8_t level_l;
+ uint8_t level_r;
+ } prep_inside;
+ };
+};
+
+/****/
+
+/* only valid in manual mode */
+#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03
+
+struct i2c_cmd_mechboard_arm_goto {
+ struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_ARM_LEFT 0
+#define I2C_MECHBOARD_ARM_RIGHT 1
+#define I2C_MECHBOARD_ARM_BOTH 2
+ uint8_t which;
+
+ uint8_t height; /* in cm */
+ uint8_t distance; /* in cm */
+};
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04
+
+struct i2c_cmd_sensorboard_start_beacon {
+ struct i2c_cmd_hdr hdr;
+ uint8_t enable;
+};
+
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05
+
+struct i2c_cmd_sensorboard_scanner {
+ struct i2c_cmd_hdr hdr;
+
+#define I2C_SENSORBOARD_SCANNER_STOP 0x00
+#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01
+#define I2C_SENSORBOARD_SCANNER_START 0x02
+ uint8_t mode;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06
+struct i2c_cmd_sensorboard_calib_scanner {
+ struct i2c_cmd_hdr hdr;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07
+struct i2c_cmd_sensorboard_scanner_algo {
+ struct i2c_cmd_hdr hdr;
+
+#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1
+#define I2C_SCANNER_ALGO_CHECK_TEMPLE 2
+#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3
+ uint8_t algo;
+
+ union {
+ struct {
+#define I2C_SCANNER_ZONE_0 0
+#define I2C_SCANNER_ZONE_1 1
+#define I2C_SCANNER_ZONE_DISC 2
+ uint8_t working_zone;
+ int16_t center_x;
+ int16_t center_y;
+ } drop_zone;
+
+ struct {
+ uint8_t level;
+ int16_t temple_x;
+ int16_t temple_y;
+ } check_temple;
+ };
+};
+
+/****/
+/* requests and their answers */
+/****/
+
+
+#define I2C_REQ_MECHBOARD_STATUS 0x80
+
+struct i2c_req_mechboard_status {
+ struct i2c_cmd_hdr hdr;
+
+ int16_t pump_left1_current;
+ int16_t pump_left2_current;
+};
+
+#define I2C_ANS_MECHBOARD_STATUS 0x81
+
+struct i2c_ans_mechboard_status {
+ struct i2c_cmd_hdr hdr;
+ /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */
+ uint8_t mode;
+
+#define I2C_MECHBOARD_STATUS_F_READY 0x00
+#define I2C_MECHBOARD_STATUS_F_BUSY 0x01
+#define I2C_MECHBOARD_STATUS_F_EXCPT 0x02
+ uint8_t status;
+
+ uint8_t lintel_count;
+
+ /* flag is there if column was taken by this pump. Note that
+ * we should also check ADC (current) to see if the column is
+ * still there. */
+#define I2C_MECHBOARD_COLUMN_L1 0x01
+#define I2C_MECHBOARD_COLUMN_L2 0x02
+#define I2C_MECHBOARD_COLUMN_R1 0x04
+#define I2C_MECHBOARD_COLUMN_R2 0x08
+ uint8_t column_flags;
+
+ int16_t pump_left1;
+ int16_t pump_right1;
+ int16_t pump_left2;
+ int16_t pump_right2;
+
+#define I2C_MECHBOARD_CURRENT_COLUMN 85
+ int16_t pump_right1_current;
+ int16_t pump_right2_current;
+
+ uint16_t servo_lintel_left;
+ uint16_t servo_lintel_right;
+};
+
+#define I2C_REQ_SENSORBOARD_STATUS 0x82
+
+struct i2c_req_sensorboard_status {
+ struct i2c_cmd_hdr hdr;
+
+ /* position sent by mainboard */
+ int16_t x;
+ int16_t y;
+ int16_t a;
+
+ /* PWM for pickup */
+ uint8_t enable_pickup_wheels;
+};
+
+#define I2C_ANS_SENSORBOARD_STATUS 0x83
+
+struct i2c_ans_sensorboard_status {
+ struct i2c_cmd_hdr hdr;
+
+ uint8_t status;
+#define I2C_OPPONENT_NOT_THERE -1000
+ int16_t opponent_x;
+ int16_t opponent_y;
+ int16_t opponent_a;
+ int16_t opponent_d;
+
+#define I2C_SCAN_DONE 1
+#define I2C_SCAN_MAX_COLUMN 2
+ uint8_t scan_status;
+
+#define I2C_COLUMN_NO_DROPZONE -1
+ int8_t dropzone_h;
+ int16_t dropzone_x;
+ int16_t dropzone_y;
+};
+
+#endif /* _I2C_PROTOCOL_H_ */
--- /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_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+#ifndef _I2C_COMMANDS_H_
+#define _I2C_COMMANDS_H_
+
+#define I2C_MAINBOARD_ADDR 1
+#define I2C_MECHBOARD_ADDR 2
+#define I2C_SENSORBOARD_ADDR 3
+
+#define I2C_LEFT_SIDE 0
+#define I2C_RIGHT_SIDE 1
+#define I2C_AUTO_SIDE 2 /* for prepare_pickup */
+#define I2C_CENTER_SIDE 3 /* for prepare_pickup */
+
+#define I2C_COLOR_RED 0
+#define I2C_COLOR_GREEN 1
+
+#define I2C_AUTOBUILD_DEFAULT_DIST 210
+
+struct i2c_cmd_hdr {
+ uint8_t cmd;
+};
+
+/****/
+/* commands that do not need and answer */
+/****/
+
+#define I2C_CMD_GENERIC_LED_CONTROL 0x00
+
+struct i2c_cmd_led_control {
+ struct i2c_cmd_hdr hdr;
+ uint8_t led_num:7;
+ uint8_t state:1;
+};
+
+/****/
+
+#define I2C_CMD_GENERIC_SET_COLOR 0x01
+
+struct i2c_cmd_generic_color {
+ struct i2c_cmd_hdr hdr;
+ uint8_t color;
+};
+
+/****/
+
+#define I2C_CMD_MECHBOARD_SET_MODE 0x02
+
+struct i2c_cmd_mechboard_set_mode {
+ struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_MODE_MANUAL 0x00
+#define I2C_MECHBOARD_MODE_HARVEST 0x01
+#define I2C_MECHBOARD_MODE_PREPARE_PICKUP 0x02
+#define I2C_MECHBOARD_MODE_PICKUP 0x03
+#define I2C_MECHBOARD_MODE_PREPARE_BUILD 0x04
+#define I2C_MECHBOARD_MODE_AUTOBUILD 0x05
+#define I2C_MECHBOARD_MODE_WAIT 0x06
+#define I2C_MECHBOARD_MODE_INIT 0x07
+#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08
+#define I2C_MECHBOARD_MODE_GET_LINTEL 0x09
+#define I2C_MECHBOARD_MODE_PUT_LINTEL 0x0A
+#define I2C_MECHBOARD_MODE_PREPARE_EJECT 0x0B
+#define I2C_MECHBOARD_MODE_EJECT 0x0C
+#define I2C_MECHBOARD_MODE_CLEAR 0x0D
+#define I2C_MECHBOARD_MODE_LAZY_HARVEST 0x0E
+#define I2C_MECHBOARD_MODE_LOADED 0x0F
+#define I2C_MECHBOARD_MODE_PREPARE_INSIDE 0x10
+#define I2C_MECHBOARD_MODE_STORE 0x11
+#define I2C_MECHBOARD_MODE_LAZY_PICKUP 0x12
+#define I2C_MECHBOARD_MODE_MANIVELLE 0x13
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE 0x14
+#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC 0x15
+#define I2C_MECHBOARD_MODE_EXIT 0xFF
+ uint8_t mode;
+ union {
+ struct {
+
+ } manual;
+
+ struct {
+ uint8_t side;
+ uint8_t next_mode;
+ } prep_pickup;
+
+ struct {
+ uint8_t level_l;
+ uint8_t level_r;
+ } prep_build;
+
+ struct {
+ uint8_t side;
+ } push_temple_disc;
+
+ struct {
+ uint8_t level_left;
+ uint8_t level_right;
+ uint8_t count_left;
+ uint8_t count_right;
+ uint8_t distance_left;
+ uint8_t distance_right;
+ uint8_t do_lintel;
+ } autobuild;
+
+ struct {
+ uint8_t level_l;
+ uint8_t level_r;
+ } prep_inside;
+ };
+};
+
+/****/
+
+/* only valid in manual mode */
+#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03
+
+struct i2c_cmd_mechboard_arm_goto {
+ struct i2c_cmd_hdr hdr;
+#define I2C_MECHBOARD_ARM_LEFT 0
+#define I2C_MECHBOARD_ARM_RIGHT 1
+#define I2C_MECHBOARD_ARM_BOTH 2
+ uint8_t which;
+
+ uint8_t height; /* in cm */
+ uint8_t distance; /* in cm */
+};
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04
+
+struct i2c_cmd_sensorboard_start_beacon {
+ struct i2c_cmd_hdr hdr;
+ uint8_t enable;
+};
+
+
+/****/
+
+#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05
+
+struct i2c_cmd_sensorboard_scanner {
+ struct i2c_cmd_hdr hdr;
+
+#define I2C_SENSORBOARD_SCANNER_STOP 0x00
+#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01
+#define I2C_SENSORBOARD_SCANNER_START 0x02
+ uint8_t mode;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06
+struct i2c_cmd_sensorboard_calib_scanner {
+ struct i2c_cmd_hdr hdr;
+};
+
+/*****/
+
+#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07
+struct i2c_cmd_sensorboard_scanner_algo {
+ struct i2c_cmd_hdr hdr;
+
+#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1
+#define I2C_SCANNER_ALGO_CHECK_TEMPLE 2
+#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3
+ uint8_t algo;
+
+ union {
+ struct {
+#define I2C_SCANNER_ZONE_0 0
+#define I2C_SCANNER_ZONE_1 1
+#define I2C_SCANNER_ZONE_DISC 2
+ uint8_t working_zone;
+ int16_t center_x;
+ int16_t center_y;
+ } drop_zone;
+
+ struct {
+ uint8_t level;
+ int16_t temple_x;
+ int16_t temple_y;
+ } check_temple;
+ };
+};
+
+/****/
+/* requests and their answers */
+/****/
+
+
+#define I2C_REQ_MECHBOARD_STATUS 0x80
+
+struct i2c_req_mechboard_status {
+ struct i2c_cmd_hdr hdr;
+
+ int16_t pump_left1_current;
+ int16_t pump_left2_current;
+};
+
+#define I2C_ANS_MECHBOARD_STATUS 0x81
+
+struct i2c_ans_mechboard_status {
+ struct i2c_cmd_hdr hdr;
+ /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */
+ uint8_t mode;
+
+#define I2C_MECHBOARD_STATUS_F_READY 0x00
+#define I2C_MECHBOARD_STATUS_F_BUSY 0x01
+#define I2C_MECHBOARD_STATUS_F_EXCPT 0x02
+ uint8_t status;
+
+ uint8_t lintel_count;
+
+ /* flag is there if column was taken by this pump. Note that
+ * we should also check ADC (current) to see if the column is
+ * still there. */
+#define I2C_MECHBOARD_COLUMN_L1 0x01
+#define I2C_MECHBOARD_COLUMN_L2 0x02
+#define I2C_MECHBOARD_COLUMN_R1 0x04
+#define I2C_MECHBOARD_COLUMN_R2 0x08
+ uint8_t column_flags;
+
+ int16_t pump_left1;
+ int16_t pump_right1;
+ int16_t pump_left2;
+ int16_t pump_right2;
+
+#define I2C_MECHBOARD_CURRENT_COLUMN 85
+ int16_t pump_right1_current;
+ int16_t pump_right2_current;
+
+ uint16_t servo_lintel_left;
+ uint16_t servo_lintel_right;
+};
+
+#define I2C_REQ_SENSORBOARD_STATUS 0x82
+
+struct i2c_req_sensorboard_status {
+ struct i2c_cmd_hdr hdr;
+
+ /* position sent by mainboard */
+ int16_t x;
+ int16_t y;
+ int16_t a;
+
+ /* PWM for pickup */
+ uint8_t enable_pickup_wheels;
+};
+
+#define I2C_ANS_SENSORBOARD_STATUS 0x83
+
+struct i2c_ans_sensorboard_status {
+ struct i2c_cmd_hdr hdr;
+
+ uint8_t status;
+#define I2C_OPPONENT_NOT_THERE -1000
+ int16_t opponent_x;
+ int16_t opponent_y;
+ int16_t opponent_a;
+ int16_t opponent_d;
+
+#define I2C_SCAN_DONE 1
+#define I2C_SCAN_MAX_COLUMN 2
+ uint8_t scan_status;
+
+#define I2C_COLUMN_NO_DROPZONE -1
+ int8_t dropzone_h;
+ int16_t dropzone_x;
+ int16_t dropzone_y;
+};
+
+#endif /* _I2C_PROTOCOL_H_ */
--- /dev/null
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+# CONFIG_MCU_ATMEGA8 is not set
+# CONFIG_MCU_ATMEGA48 is not set
+# CONFIG_MCU_ATMEGA88 is not set
+# CONFIG_MCU_ATMEGA8515 is not set
+# CONFIG_MCU_ATMEGA8535 is not set
+# CONFIG_MCU_ATTINY13 is not set
+# CONFIG_MCU_ATTINY2313 is not set
+# CONFIG_MCU_ATMEGA16 is not set
+# CONFIG_MCU_ATMEGA161 is not set
+# CONFIG_MCU_ATMEGA162 is not set
+# CONFIG_MCU_ATMEGA163 is not set
+# CONFIG_MCU_ATMEGA165 is not set
+# CONFIG_MCU_ATMEGA168 is not set
+# CONFIG_MCU_ATMEGA169 is not set
+# CONFIG_MCU_ATMEGA32 is not set
+# CONFIG_MCU_ATMEGA323 is not set
+# CONFIG_MCU_ATMEGA325 is not set
+# CONFIG_MCU_ATMEGA3250 is not set
+# CONFIG_MCU_ATMEGA64 is not set
+# CONFIG_MCU_ATMEGA645 is not set
+# CONFIG_MCU_ATMEGA6450 is not set
+# CONFIG_MCU_ATMEGA128 is not set
+# CONFIG_MCU_ATMEGA1281 is not set
+# CONFIG_MCU_AT90CAN128 is not set
+# CONFIG_MCU_AT94K is not set
+# CONFIG_MCU_AT90S1200 is not set
+CONFIG_MCU_ATMEGA2560=y
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+CONFIG_MATH_LIB=y
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+# CONFIG_STANDARD_PRINTF is not set
+CONFIG_ADVANCED_PRINTF=y
+# CONFIG_FORMAT_IHEX is not set
+# CONFIG_FORMAT_SREC is not set
+CONFIG_FORMAT_BINARY=y
+
+#
+# Base modules
+#
+CONFIG_MODULE_CIRBUF=y
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+CONFIG_MODULE_FIXED_POINT=y
+CONFIG_MODULE_VECT2=y
+CONFIG_MODULE_GEOMETRY=y
+CONFIG_MODULE_SCHEDULER=y
+CONFIG_MODULE_SCHEDULER_STATS=y
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+# CONFIG_MODULE_SCHEDULER_TIMER0 is not set
+CONFIG_MODULE_SCHEDULER_MANUAL=y
+CONFIG_MODULE_TIME=y
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
+CONFIG_MODULE_UART_CREATE_CONFIG=y
+CONFIG_MODULE_SPI=y
+CONFIG_MODULE_SPI_CREATE_CONFIG=y
+CONFIG_MODULE_I2C=y
+CONFIG_MODULE_I2C_MASTER=y
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+CONFIG_MODULE_I2C_CREATE_CONFIG=y
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+CONFIG_MODULE_TIMER=y
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+CONFIG_MODULE_PWM_NG=y
+CONFIG_MODULE_ADC=y
+CONFIG_MODULE_ADC_CREATE_CONFIG=y
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+CONFIG_MODULE_VT100=y
+CONFIG_MODULE_RDLINE=y
+CONFIG_MODULE_RDLINE_CREATE_CONFIG=y
+CONFIG_MODULE_RDLINE_KILL_BUF=y
+CONFIG_MODULE_RDLINE_HISTORY=y
+CONFIG_MODULE_PARSE=y
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+CONFIG_MODULE_AX12=y
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+CONFIG_MODULE_ENCODERS_SPI=y
+CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y
+
+#
+# Robot specific modules
+#
+CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+CONFIG_MODULE_POSITION_MANAGER=y
+CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
+CONFIG_MODULE_TRAJECTORY_MANAGER=y
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y
+
+#
+# Control system modules
+#
+CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+CONFIG_MODULE_PID=y
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+CONFIG_MODULE_QUADRAMP=y
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+CONFIG_MODULE_DIAGNOSTIC=y
+CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
+CONFIG_MODULE_ERROR=y
+CONFIG_MODULE_ERROR_CREATE_CONFIG=y
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
--- /dev/null
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+# CONFIG_MCU_ATMEGA8 is not set
+# CONFIG_MCU_ATMEGA48 is not set
+# CONFIG_MCU_ATMEGA88 is not set
+# CONFIG_MCU_ATMEGA8515 is not set
+# CONFIG_MCU_ATMEGA8535 is not set
+# CONFIG_MCU_ATTINY13 is not set
+# CONFIG_MCU_ATTINY2313 is not set
+# CONFIG_MCU_ATMEGA16 is not set
+# CONFIG_MCU_ATMEGA161 is not set
+# CONFIG_MCU_ATMEGA162 is not set
+# CONFIG_MCU_ATMEGA163 is not set
+# CONFIG_MCU_ATMEGA165 is not set
+# CONFIG_MCU_ATMEGA168 is not set
+# CONFIG_MCU_ATMEGA169 is not set
+# CONFIG_MCU_ATMEGA32 is not set
+# CONFIG_MCU_ATMEGA323 is not set
+# CONFIG_MCU_ATMEGA325 is not set
+# CONFIG_MCU_ATMEGA3250 is not set
+# CONFIG_MCU_ATMEGA64 is not set
+# CONFIG_MCU_ATMEGA645 is not set
+# CONFIG_MCU_ATMEGA6450 is not set
+# CONFIG_MCU_ATMEGA128 is not set
+# CONFIG_MCU_ATMEGA1281 is not set
+# CONFIG_MCU_AT90CAN128 is not set
+# CONFIG_MCU_AT94K is not set
+# CONFIG_MCU_AT90S1200 is not set
+CONFIG_MCU_ATMEGA2560=y
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+CONFIG_MATH_LIB=y
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+# CONFIG_STANDARD_PRINTF is not set
+CONFIG_ADVANCED_PRINTF=y
+# CONFIG_FORMAT_IHEX is not set
+# CONFIG_FORMAT_SREC is not set
+CONFIG_FORMAT_BINARY=y
+
+#
+# Base modules
+#
+CONFIG_MODULE_CIRBUF=y
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+CONFIG_MODULE_FIXED_POINT=y
+CONFIG_MODULE_VECT2=y
+CONFIG_MODULE_GEOMETRY=y
+CONFIG_MODULE_SCHEDULER=y
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+# CONFIG_MODULE_SCHEDULER_TIMER0 is not set
+CONFIG_MODULE_SCHEDULER_MANUAL=y
+CONFIG_MODULE_TIME=y
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
+CONFIG_MODULE_UART_CREATE_CONFIG=y
+CONFIG_MODULE_SPI=y
+CONFIG_MODULE_SPI_CREATE_CONFIG=y
+CONFIG_MODULE_I2C=y
+CONFIG_MODULE_I2C_MASTER=y
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+CONFIG_MODULE_I2C_CREATE_CONFIG=y
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+CONFIG_MODULE_TIMER=y
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+CONFIG_MODULE_PWM_NG=y
+CONFIG_MODULE_ADC=y
+CONFIG_MODULE_ADC_CREATE_CONFIG=y
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+CONFIG_MODULE_VT100=y
+CONFIG_MODULE_RDLINE=y
+CONFIG_MODULE_RDLINE_CREATE_CONFIG=y
+CONFIG_MODULE_RDLINE_KILL_BUF=y
+CONFIG_MODULE_RDLINE_HISTORY=y
+CONFIG_MODULE_PARSE=y
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+CONFIG_MODULE_AX12=y
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+CONFIG_MODULE_ENCODERS_SPI=y
+CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y
+
+#
+# Robot specific modules
+#
+CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+CONFIG_MODULE_POSITION_MANAGER=y
+CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
+CONFIG_MODULE_TRAJECTORY_MANAGER=y
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE=y
+CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y
+
+#
+# Control system modules
+#
+CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+CONFIG_MODULE_PID=y
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+CONFIG_MODULE_QUADRAMP=y
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+CONFIG_MODULE_DIAGNOSTIC=y
+CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
+CONFIG_MODULE_ERROR=y
+CONFIG_MODULE_ERROR_CREATE_CONFIG=y
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
--- /dev/null
+TARGET = main
+
+# repertoire des modules
+AVERSIVE_DIR = ../../..
+
+SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
+SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
+SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
+SRC += strat_utils.c strat_base.c strat_avoid.c strat.c
+SRC += strat_static_columns.c strat_lintel.c
+SRC += strat_column_disp.c strat_building.c strat_scan.c
+
+ASRC =
+
+CFLAGS += -Wall -Werror
+#CFLAGS += -DHOMOLOGATION
+CFLAGS += -DTEST_BEACON
+LDFLAGS = -T ../common/avr6.x
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+AVRDUDE_DELAY=50
+
+program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep
+ echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+ $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+
--- /dev/null
+TARGET = main
+
+# repertoire des modules
+AVERSIVE_DIR = ../../..
+
+SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
+SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
+SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
+SRC += strat_utils.c strat_base.c strat_avoid.c strat.c
+SRC += strat_static_columns.c strat_lintel.c
+SRC += strat_column_disp.c strat_building.c strat_scan.c
+
+ASRC =
+
+CFLAGS += -Wall -Werror
+#CFLAGS += -DHOMOLOGATION
+LDFLAGS = -T ../common/avr6.x
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+AVRDUDE_DELAY=50
+
+program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep
+ echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+ $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+
--- /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.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+
+#include "main.h"
+
+void pwm_set_and_save(void *pwm, int32_t val)
+{
+ /* we need to do the saturation here, before saving the
+ * value */
+ if (val > 4095)
+ val = 4095;
+ if (val < -4095)
+ val = -4095;
+
+ if (pwm == LEFT_PWM)
+ mainboard.pwm_l = val;
+ else if (pwm == RIGHT_PWM)
+ mainboard.pwm_r = val;
+ pwm_ng_set(pwm, val);
+}
+
+void pickup_wheels_on(void)
+{
+ mainboard.enable_pickup_wheels = 1;
+}
+
+void pickup_wheels_off(void)
+{
+ mainboard.enable_pickup_wheels = 0;
+}
+
--- /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:41 zer0 Exp $
+ *
+ */
+
+void pwm_set_and_save(void *pwm, int32_t val);
+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.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/list.h>
+#include <aversive/error.h>
+
+#include <i2c.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+
+/*
+ * Cmdline interface for AX12. Use the PC to command a daisy-chain of
+ * AX12 actuators with a nice command line interface.
+ *
+ * The circuit should be as following:
+ *
+ * |----------|
+ * | uart3|------->--- PC (baudrate=57600)
+ * | |-------<---
+ * | atmega128|
+ * | |
+ * | uart0|---->---+-- AX12 (baudrate 115200)
+ * | |----<---|
+ * |----------|
+ *
+ * Note that RX and TX pins of UART1 are connected together to provide
+ * a half-duplex UART emulation.
+ *
+ */
+
+#define UART_AX12_NUM 0
+#define UCSRxB UCSR0B
+#define AX12_TIMEOUT 5000UL /* in us */
+
+/********************************* AX12 commands */
+
+/*
+ * We use synchronous access (not interrupt driven) to the hardware
+ * UART, because we have to be sure that the transmission/reception is
+ * really finished when we return from the functions.
+ *
+ * We don't use the CM-5 circuit as described in the AX12
+ * documentation, we simply connect TX and RX and use TXEN + RXEN +
+ * DDR to manage the port directions.
+ */
+
+static volatile uint8_t ax12_state = AX12_STATE_READ;
+extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
+static volatile uint8_t ax12_nsent = 0;
+
+/* Called by ax12 module to send a character on serial line. Count the
+ * number of transmitted bytes. It will be used in ax12_recv_char() to
+ * drop the bytes that we transmitted. */
+static int8_t ax12_send_char(uint8_t c)
+{
+ uart_send(UART_AX12_NUM, c);
+ ax12_nsent++;
+ return 0;
+}
+
+/* for atmega256 */
+#ifndef TXEN
+#define TXEN TXEN0
+#endif
+
+/* called by uart module when the character has been written in
+ * UDR. It does not mean that the byte is physically transmitted. */
+static void ax12_send_callback(char c)
+{
+ if (ax12_state == AX12_STATE_READ) {
+ /* disable TX when last byte is pushed. */
+ if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+ UCSRxB &= ~(1<<TXEN);
+ }
+}
+
+/* Called by ax12 module when we want to receive a char. Note that we
+ * also receive the bytes we sent ! So we need to drop them. */
+static int16_t ax12_recv_char(void)
+{
+ microseconds t = time_get_us2();
+ int c;
+ while (1) {
+ c = uart_recv_nowait(UART_AX12_NUM);
+ if (c != -1) {
+ if (ax12_nsent == 0)
+ return c;
+ ax12_nsent --;
+ }
+
+ /* 5 ms timeout */
+ if ((time_get_us2() - t) > AX12_TIMEOUT)
+ return -1;
+ }
+ return c;
+}
+
+/* called by ax12 module when we want to switch serial line. As we
+ * work in interruption mode, this function can be called to switch
+ * back in read mode even if the bytes are not really transmitted on
+ * the line. That's why in this case we do nothing, we will fall back
+ * in read mode in any case when xmit is finished -- see in
+ * ax12_send_callback() -- */
+static void ax12_switch_uart(uint8_t state)
+{
+ uint8_t flags;
+
+ if (state == AX12_STATE_WRITE) {
+ IRQ_LOCK(flags);
+ ax12_nsent=0;
+ while (uart_recv_nowait(UART_AX12_NUM) != -1);
+ UCSRxB |= (1<<TXEN);
+ ax12_state = AX12_STATE_WRITE;
+ IRQ_UNLOCK(flags);
+ }
+ else {
+ IRQ_LOCK(flags);
+ if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM]))
+ UCSRxB &= ~(1<<TXEN);
+ ax12_state = AX12_STATE_READ;
+ IRQ_UNLOCK(flags);
+ }
+}
+
+
+void ax12_user_init(void)
+{
+ /* AX12 */
+ AX12_init(&gen.ax12);
+ AX12_set_hardware_send(&gen.ax12, ax12_send_char);
+ AX12_set_hardware_recv(&gen.ax12, ax12_recv_char);
+ AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart);
+ uart_register_tx_event(UART_AX12_NUM, ax12_send_callback);
+}
--- /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.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "strat_base.h"
+
+
+/******** See in commands.c for the list of commands. */
+extern parse_pgm_ctx_t main_ctx[];
+
+static void write_char(char c)
+{
+ uart_send(CMDLINE_UART, c);
+}
+
+static void
+valid_buffer(const char *buf, uint8_t size)
+{
+ int8_t ret;
+
+ /* reset CTRL-C for trajectory interruption each time we
+ * receive a new command */
+ interrupt_traj_reset();
+
+ ret = parse(main_ctx, buf);
+ if (ret == PARSE_AMBIGUOUS)
+ printf_P(PSTR("Ambiguous command\r\n"));
+ else if (ret == PARSE_NOMATCH)
+ printf_P(PSTR("Command not found\r\n"));
+ else if (ret == PARSE_BAD_ARGS)
+ printf_P(PSTR("Bad arguments\r\n"));
+}
+
+static int8_t
+complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
+ int16_t *state)
+{
+ return complete(main_ctx, buf, state, dstbuf, dstsize);
+}
+
+
+/* sending "pop" on cmdline uart resets the robot */
+void emergency(char c)
+{
+ static uint8_t i = 0;
+
+ /* interrupt traj here */
+ if (c == '\003')
+ interrupt_traj();
+
+ if ((i == 0 && c == 'p') ||
+ (i == 1 && c == 'o') ||
+ (i == 2 && c == 'p'))
+ i++;
+ else if ( !(i == 1 && c == 'p') )
+ i = 0;
+ if (i == 3)
+ reset();
+}
+
+/* log function, add a command to configure
+ * it dynamically */
+void mylog(struct error * e, ...)
+{
+ va_list ap;
+ u16 stream_flags = stdout->flags;
+ uint8_t i;
+ time_h tv;
+
+ if (e->severity > ERROR_SEVERITY_ERROR) {
+ if (gen.log_level < e->severity)
+ return;
+
+ for (i=0; i<NB_LOGS+1; i++)
+ if (gen.logs[i] == e->err_num)
+ break;
+ if (i == NB_LOGS+1)
+ return;
+ }
+
+ va_start(ap, e);
+ tv = time_get_time();
+ printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
+
+ printf_P(PSTR("(%d,%d,%d) "),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+
+ vfprintf_P(stdout, e->text, ap);
+ printf_P(PSTR("\r\n"));
+ va_end(ap);
+ stdout->flags = stream_flags;
+}
+
+int cmdline_interact(void)
+{
+ const char *history, *buffer;
+ int8_t ret, same = 0;
+ int16_t c;
+
+ rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
+ snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > ");
+ rdline_newline(&gen.rdl, gen.prompt);
+
+ while (1) {
+ c = uart_recv_nowait(CMDLINE_UART);
+ if (c == -1)
+ continue;
+ ret = rdline_char_in(&gen.rdl, c);
+ if (ret != 2 && ret != 0) {
+ buffer = rdline_get_buffer(&gen.rdl);
+ history = rdline_get_history_item(&gen.rdl, 0);
+ if (history) {
+ same = !memcmp(buffer, history, strlen(history)) &&
+ buffer[strlen(history)] == '\n';
+ }
+ else
+ same = 0;
+ if (strlen(buffer) > 1 && !same)
+ rdline_add_history(&gen.rdl, buffer);
+ rdline_newline(&gen.rdl, gen.prompt);
+ }
+ }
+
+ return 0;
+}
--- /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:24:33 zer0 Exp $
+ *
+ */
+
+#define CMDLINE_UART 1
+
+/* uart rx callback for reset() */
+void emergency(char c);
+
+/* log function */
+void mylog(struct error * e, ...);
+
+/* launch cmdline */
+int cmdline_interact(void);
+
+static inline uint8_t cmdline_keypressed(void) {
+ return (uart_recv_nowait(CMDLINE_UART) != -1);
+}
+
+static inline int16_t cmdline_getchar(void) {
+ return uart_recv_nowait(CMDLINE_UART);
+}
+
+static inline uint8_t cmdline_getchar_wait(void) {
+ return uart_recv(CMDLINE_UART);
+}
--- /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.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdlib.h>
+#include <aversive/pgmspace.h>
+#include <parse.h>
+
+/* commands_gen.c */
+extern parse_pgm_inst_t cmd_reset;
+extern parse_pgm_inst_t cmd_bootloader;
+extern parse_pgm_inst_t cmd_encoders;
+extern parse_pgm_inst_t cmd_pwm;
+extern parse_pgm_inst_t cmd_adc;
+extern parse_pgm_inst_t cmd_sensor;
+extern parse_pgm_inst_t cmd_log;
+extern parse_pgm_inst_t cmd_log_show;
+extern parse_pgm_inst_t cmd_log_type;
+extern parse_pgm_inst_t cmd_stack_space;
+extern parse_pgm_inst_t cmd_scheduler;
+
+/* commands_ax12.c */
+extern parse_pgm_inst_t cmd_baudrate;
+extern parse_pgm_inst_t cmd_uint16_read;
+extern parse_pgm_inst_t cmd_uint16_write;
+extern parse_pgm_inst_t cmd_uint8_read;
+extern parse_pgm_inst_t cmd_uint8_write;
+
+/* commands_cs.c */
+extern parse_pgm_inst_t cmd_gain;
+extern parse_pgm_inst_t cmd_gain_show;
+extern parse_pgm_inst_t cmd_speed;
+extern parse_pgm_inst_t cmd_speed_show;
+extern parse_pgm_inst_t cmd_derivate_filter;
+extern parse_pgm_inst_t cmd_derivate_filter_show;
+extern parse_pgm_inst_t cmd_consign;
+extern parse_pgm_inst_t cmd_maximum;
+extern parse_pgm_inst_t cmd_maximum_show;
+extern parse_pgm_inst_t cmd_quadramp;
+extern parse_pgm_inst_t cmd_quadramp_show;
+extern parse_pgm_inst_t cmd_cs_status;
+extern parse_pgm_inst_t cmd_blocking_i;
+extern parse_pgm_inst_t cmd_blocking_i_show;
+
+/* commands_mainboard.c */
+extern parse_pgm_inst_t cmd_event;
+extern parse_pgm_inst_t cmd_spi_test;
+extern parse_pgm_inst_t cmd_opponent;
+extern parse_pgm_inst_t cmd_opponent_set;
+extern parse_pgm_inst_t cmd_start;
+extern parse_pgm_inst_t cmd_interact;
+extern parse_pgm_inst_t cmd_color;
+extern parse_pgm_inst_t cmd_rs;
+extern parse_pgm_inst_t cmd_i2cdebug;
+extern parse_pgm_inst_t cmd_mechboard_show;
+extern parse_pgm_inst_t cmd_mechboard_setmode1;
+extern parse_pgm_inst_t cmd_mechboard_setmode2;
+extern parse_pgm_inst_t cmd_mechboard_setmode3;
+extern parse_pgm_inst_t cmd_mechboard_setmode4;
+extern parse_pgm_inst_t cmd_mechboard_setmode5;
+extern parse_pgm_inst_t cmd_pickup_wheels;
+extern parse_pgm_inst_t cmd_beacon_start;
+extern parse_pgm_inst_t cmd_pump_current;
+extern parse_pgm_inst_t cmd_build_test;
+extern parse_pgm_inst_t cmd_column_test;
+extern parse_pgm_inst_t cmd_column_test2;
+extern parse_pgm_inst_t cmd_lintel_test;
+extern parse_pgm_inst_t cmd_pickup_test;
+extern parse_pgm_inst_t cmd_scan_test;
+extern parse_pgm_inst_t cmd_scan_test2;
+extern parse_pgm_inst_t cmd_time_monitor;
+extern parse_pgm_inst_t cmd_scanner;
+extern parse_pgm_inst_t cmd_build_z1;
+#ifdef TEST_BEACON
+extern parse_pgm_inst_t cmd_beacon_opp_dump;
+#endif
+extern parse_pgm_inst_t cmd_test;
+
+/* commands_traj.c */
+extern parse_pgm_inst_t cmd_traj_speed;
+extern parse_pgm_inst_t cmd_traj_speed_show;
+extern parse_pgm_inst_t cmd_trajectory;
+extern parse_pgm_inst_t cmd_trajectory_show;
+extern parse_pgm_inst_t cmd_rs_gains;
+extern parse_pgm_inst_t cmd_rs_gains_show;
+extern parse_pgm_inst_t cmd_track;
+extern parse_pgm_inst_t cmd_track_show;
+extern parse_pgm_inst_t cmd_pt_list;
+extern parse_pgm_inst_t cmd_pt_list_append;
+extern parse_pgm_inst_t cmd_pt_list_del;
+extern parse_pgm_inst_t cmd_pt_list_show;
+extern parse_pgm_inst_t cmd_goto1;
+extern parse_pgm_inst_t cmd_goto2;
+extern parse_pgm_inst_t cmd_goto3;
+extern parse_pgm_inst_t cmd_position;
+extern parse_pgm_inst_t cmd_position_set;
+extern parse_pgm_inst_t cmd_strat_infos;
+extern parse_pgm_inst_t cmd_strat_conf;
+extern parse_pgm_inst_t cmd_strat_conf2;
+extern parse_pgm_inst_t cmd_strat_conf3;
+extern parse_pgm_inst_t cmd_strat_conf4;
+extern parse_pgm_inst_t cmd_subtraj;
+
+/* in progmem */
+parse_pgm_ctx_t main_ctx[] = {
+
+ /* commands_gen.c */
+ (parse_pgm_inst_t *)&cmd_reset,
+ (parse_pgm_inst_t *)&cmd_bootloader,
+ (parse_pgm_inst_t *)&cmd_encoders,
+ (parse_pgm_inst_t *)&cmd_pwm,
+ (parse_pgm_inst_t *)&cmd_adc,
+ (parse_pgm_inst_t *)&cmd_sensor,
+ (parse_pgm_inst_t *)&cmd_log,
+ (parse_pgm_inst_t *)&cmd_log_show,
+ (parse_pgm_inst_t *)&cmd_log_type,
+ (parse_pgm_inst_t *)&cmd_stack_space,
+ (parse_pgm_inst_t *)&cmd_scheduler,
+
+ /* commands_ax12.c */
+ (parse_pgm_inst_t *)&cmd_baudrate,
+ (parse_pgm_inst_t *)&cmd_uint16_read,
+ (parse_pgm_inst_t *)&cmd_uint16_write,
+ (parse_pgm_inst_t *)&cmd_uint8_read,
+ (parse_pgm_inst_t *)&cmd_uint8_write,
+
+ /* commands_cs.c */
+ (parse_pgm_inst_t *)&cmd_gain,
+ (parse_pgm_inst_t *)&cmd_gain_show,
+ (parse_pgm_inst_t *)&cmd_speed,
+ (parse_pgm_inst_t *)&cmd_speed_show,
+ (parse_pgm_inst_t *)&cmd_consign,
+ (parse_pgm_inst_t *)&cmd_derivate_filter,
+ (parse_pgm_inst_t *)&cmd_derivate_filter_show,
+ (parse_pgm_inst_t *)&cmd_maximum,
+ (parse_pgm_inst_t *)&cmd_maximum_show,
+ (parse_pgm_inst_t *)&cmd_quadramp,
+ (parse_pgm_inst_t *)&cmd_quadramp_show,
+ (parse_pgm_inst_t *)&cmd_cs_status,
+ (parse_pgm_inst_t *)&cmd_blocking_i,
+ (parse_pgm_inst_t *)&cmd_blocking_i_show,
+
+ /* commands_mainboard.c */
+ (parse_pgm_inst_t *)&cmd_event,
+ (parse_pgm_inst_t *)&cmd_spi_test,
+ (parse_pgm_inst_t *)&cmd_opponent,
+ (parse_pgm_inst_t *)&cmd_opponent_set,
+ (parse_pgm_inst_t *)&cmd_start,
+ (parse_pgm_inst_t *)&cmd_interact,
+ (parse_pgm_inst_t *)&cmd_color,
+ (parse_pgm_inst_t *)&cmd_rs,
+ (parse_pgm_inst_t *)&cmd_i2cdebug,
+ (parse_pgm_inst_t *)&cmd_mechboard_show,
+ (parse_pgm_inst_t *)&cmd_mechboard_setmode1,
+ (parse_pgm_inst_t *)&cmd_mechboard_setmode2,
+ (parse_pgm_inst_t *)&cmd_mechboard_setmode3,
+ (parse_pgm_inst_t *)&cmd_mechboard_setmode4,
+ (parse_pgm_inst_t *)&cmd_mechboard_setmode5,
+ (parse_pgm_inst_t *)&cmd_pickup_wheels,
+ (parse_pgm_inst_t *)&cmd_beacon_start,
+ (parse_pgm_inst_t *)&cmd_pump_current,
+ (parse_pgm_inst_t *)&cmd_build_test,
+ (parse_pgm_inst_t *)&cmd_column_test,
+ (parse_pgm_inst_t *)&cmd_column_test2,
+ (parse_pgm_inst_t *)&cmd_lintel_test,
+ (parse_pgm_inst_t *)&cmd_pickup_test,
+ (parse_pgm_inst_t *)&cmd_scan_test,
+ (parse_pgm_inst_t *)&cmd_scan_test2,
+ (parse_pgm_inst_t *)&cmd_time_monitor,
+ (parse_pgm_inst_t *)&cmd_scanner,
+ (parse_pgm_inst_t *)&cmd_build_z1,
+#ifdef TEST_BEACON
+ (parse_pgm_inst_t *)&cmd_beacon_opp_dump,
+#endif
+ (parse_pgm_inst_t *)&cmd_test,
+
+ /* commands_traj.c */
+ (parse_pgm_inst_t *)&cmd_traj_speed,
+ (parse_pgm_inst_t *)&cmd_traj_speed_show,
+ (parse_pgm_inst_t *)&cmd_trajectory,
+ (parse_pgm_inst_t *)&cmd_trajectory_show,
+ (parse_pgm_inst_t *)&cmd_rs_gains,
+ (parse_pgm_inst_t *)&cmd_rs_gains_show,
+ (parse_pgm_inst_t *)&cmd_track,
+ (parse_pgm_inst_t *)&cmd_track_show,
+ (parse_pgm_inst_t *)&cmd_pt_list,
+ (parse_pgm_inst_t *)&cmd_pt_list_append,
+ (parse_pgm_inst_t *)&cmd_pt_list_del,
+ (parse_pgm_inst_t *)&cmd_pt_list_show,
+ (parse_pgm_inst_t *)&cmd_goto1,
+ (parse_pgm_inst_t *)&cmd_goto2,
+ (parse_pgm_inst_t *)&cmd_position,
+ (parse_pgm_inst_t *)&cmd_position_set,
+ (parse_pgm_inst_t *)&cmd_strat_infos,
+ (parse_pgm_inst_t *)&cmd_strat_conf,
+ (parse_pgm_inst_t *)&cmd_strat_conf2,
+ (parse_pgm_inst_t *)&cmd_strat_conf3,
+ (parse_pgm_inst_t *)&cmd_strat_conf4,
+ (parse_pgm_inst_t *)&cmd_subtraj,
+ NULL,
+};
--- /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-05-02 10:08:09 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+
+uint8_t addr_from_string(const char *s)
+{
+ /* 16 bits */
+ if (!strcmp_P(s, PSTR("cw_angle_limit")))
+ return AA_CW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("ccw_angle_limit")))
+ return AA_CCW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("max_torque")))
+ return AA_MAX_TORQUE_L;
+ if (!strcmp_P(s, PSTR("down_calibration")))
+ return AA_DOWN_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("up_calibration")))
+ return AA_UP_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("torque_limit")))
+ return AA_TORQUE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("position")))
+ return AA_PRESENT_POSITION_L;
+ if (!strcmp_P(s, PSTR("speed")))
+ return AA_PRESENT_SPEED_L;
+ if (!strcmp_P(s, PSTR("load")))
+ return AA_PRESENT_LOAD_L;
+ if (!strcmp_P(s, PSTR("moving_speed")))
+ return AA_MOVING_SPEED_L;
+ if (!strcmp_P(s, PSTR("model")))
+ return AA_MODEL_NUMBER_L;
+ if (!strcmp_P(s, PSTR("goal_pos")))
+ return AA_GOAL_POSITION_L;
+ if (!strcmp_P(s, PSTR("punch")))
+ return AA_PUNCH_L;
+
+ /* 8 bits */
+ if (!strcmp_P(s, PSTR("firmware")))
+ return AA_FIRMWARE;
+ if (!strcmp_P(s, PSTR("id")))
+ return AA_ID;
+ if (!strcmp_P(s, PSTR("baudrate")))
+ return AA_BAUD_RATE;
+ if (!strcmp_P(s, PSTR("delay")))
+ return AA_DELAY_TIME;
+ if (!strcmp_P(s, PSTR("high_lim_temp")))
+ return AA_HIGHEST_LIMIT_TEMP;
+ if (!strcmp_P(s, PSTR("low_lim_volt")))
+ return AA_LOWEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("high_lim_volt")))
+ return AA_HIGHEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("status_return")))
+ return AA_STATUS_RETURN_LEVEL;
+ if (!strcmp_P(s, PSTR("alarm_led")))
+ return AA_ALARM_LED;
+ if (!strcmp_P(s, PSTR("alarm_shutdown")))
+ return AA_ALARM_SHUTDOWN;
+ if (!strcmp_P(s, PSTR("torque_enable")))
+ return AA_TORQUE_ENABLE;
+ if (!strcmp_P(s, PSTR("led")))
+ return AA_LED;
+ if (!strcmp_P(s, PSTR("cw_comp_margin")))
+ return AA_CW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("ccw_comp_margin")))
+ return AA_CCW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("cw_comp_slope")))
+ return AA_CW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("ccw_comp_slope")))
+ return AA_CCW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("voltage")))
+ return AA_PRESENT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("temp")))
+ return AA_PRESENT_TEMP;
+ if (!strcmp_P(s, PSTR("reginst")))
+ return AA_PRESENT_REGINST;
+ if (!strcmp_P(s, PSTR("moving")))
+ return AA_MOVING;
+ if (!strcmp_P(s, PSTR("lock")))
+ return AA_LOCK;
+
+ return 0;
+}
+
+/**********************************************************/
+/* Ax12_Stress */
+
+/* this structure is filled when cmd_ax12_stress is parsed successfully */
+struct cmd_ax12_stress_result {
+ fixed_string_t arg0;
+ uint8_t id;
+};
+
+/* function called when cmd_ax12_stress is parsed successfully */
+static void cmd_ax12_stress_parsed(void *parsed_result, void *data)
+{
+ struct cmd_ax12_stress_result *res = parsed_result;
+ int i, nb_errs = 0;
+ uint8_t val;
+ microseconds t = time_get_us2();
+
+ for (i=0; i<1000; i++) {
+ if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
+ nb_errs ++;
+ }
+
+ printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
+ t = (time_get_us2() - t) / 1000;
+ printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
+}
+
+prog_char str_ax12_stress_arg0[] = "ax12_stress";
+parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
+parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
+
+prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands";
+parse_pgm_inst_t cmd_ax12_stress = {
+ .f = cmd_ax12_stress_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ax12_stress,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ax12_stress_arg0,
+ (prog_void *)&cmd_ax12_stress_id,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_baudrate is parsed successfully */
+struct cmd_baudrate_result {
+ fixed_string_t arg0;
+ uint32_t arg1;
+};
+
+/* function called when cmd_baudrate is parsed successfully */
+static void cmd_baudrate_parsed(void * parsed_result, void * data)
+{
+ struct cmd_baudrate_result *res = parsed_result;
+ struct uart_config c;
+
+ printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+ uart_getconf(1, &c);
+ c.baudrate = res->arg1;
+ uart_setconf(1, &c);
+ printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+}
+
+prog_char str_baudrate_arg0[] = "baudrate";
+parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
+parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
+
+prog_char help_baudrate[] = "Change ax12 baudrate";
+parse_pgm_inst_t cmd_baudrate = {
+ .f = cmd_baudrate_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_baudrate,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_baudrate_arg0,
+ (prog_void *)&cmd_baudrate_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint16 */
+
+
+/* this structure is filled when cmd_uint16_read is parsed successfully */
+struct cmd_uint16_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint16_t val;
+};
+
+/* function called when cmd_uint16_read is parsed successfully */
+static void cmd_uint16_read_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint16_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+ ret = AX12_read_int(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint16_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
+prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#down_calibration#up_calibration#torque_limit#"
+ "position#speed#load#punch";
+parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
+parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
+
+prog_char help_uint16_read[] = "Read uint16 value (type, num)";
+parse_pgm_inst_t cmd_uint16_read = {
+ .f = cmd_uint16_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0,
+ (prog_void *)&cmd_uint16_arg1,
+ (prog_void *)&cmd_uint16_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint16_write is parsed successfully */
+static void cmd_uint16_write_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t addr = addr_from_string(res->arg1);
+ printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = AX12_write_int(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint16_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
+prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#torque_limit#punch";
+parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
+parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
+
+prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
+parse_pgm_inst_t cmd_uint16_write = {
+ .f = cmd_uint16_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0_w,
+ (prog_void *)&cmd_uint16_arg1_w,
+ (prog_void *)&cmd_uint16_num,
+ (prog_void *)&cmd_uint16_val,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint8 */
+
+
+/* this structure is filled when cmd_uint8_read is parsed successfully */
+struct cmd_uint8_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint8_t val;
+};
+
+/* function called when cmd_uint8_read is parsed successfully */
+static void cmd_uint8_read_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+
+ ret = AX12_read_byte(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint8_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
+prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "voltage#temp#reginst#moving#lock";
+parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
+parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
+
+prog_char help_uint8_read[] = "Read uint8 value (type, num)";
+parse_pgm_inst_t cmd_uint8_read = {
+ .f = cmd_uint8_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0,
+ (prog_void *)&cmd_uint8_arg1,
+ (prog_void *)&cmd_uint8_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint8_write is parsed successfully */
+static void cmd_uint8_write_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t addr = addr_from_string(res->arg1);
+ uint8_t ret;
+ printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint8_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
+prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "reginst#lock";
+parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
+parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
+
+prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
+parse_pgm_inst_t cmd_uint8_write = {
+ .f = cmd_uint8_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0_w,
+ (prog_void *)&cmd_uint8_arg1_w,
+ (prog_void *)&cmd_uint8_num,
+ (prog_void *)&cmd_uint8_val,
+ NULL,
+ },
+};
--- /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.4 2009-05-02 10:08:09 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cs.h"
+#include "cmdline.h"
+
+struct csb_list {
+ const prog_char *name;
+ struct cs_block *csb;
+};
+
+prog_char csb_angle_str[] = "angle";
+prog_char csb_distance_str[] = "distance";
+struct csb_list csb_list[] = {
+ { .name = csb_angle_str, .csb = &mainboard.angle },
+ { .name = csb_distance_str, .csb = &mainboard.distance },
+};
+
+struct cmd_cs_result {
+ fixed_string_t cmdname;
+ fixed_string_t csname;
+};
+
+/* token to be used for all cs-related commands */
+prog_char str_csb_name[] = "angle#distance";
+parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
+
+struct cs_block *cs_from_name(const char *name)
+{
+ int i;
+
+ for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
+ if (!strcmp_P(name, csb_list[i].name))
+ return csb_list[i].csb;
+ }
+ return NULL;
+}
+
+/**********************************************************/
+/* Gains for control system */
+
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_result {
+ struct cmd_cs_result cs;
+ int16_t p;
+ int16_t i;
+ int16_t d;
+};
+
+/* function called when cmd_gain is parsed successfully */
+static void cmd_gain_parsed(void * parsed_result, void *show)
+{
+ struct cmd_gain_result *res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_gains(&csb->pid, res->p, res->i, res->d);
+
+ printf_P(PSTR("%s %s %d %d %d\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ pid_get_gain_P(&csb->pid),
+ pid_get_gain_I(&csb->pid),
+ pid_get_gain_D(&csb->pid));
+}
+
+prog_char str_gain_arg0[] = "gain";
+parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
+parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
+parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
+parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
+
+prog_char help_gain[] = "Set gain values for PID";
+parse_pgm_inst_t cmd_gain = {
+ .f = cmd_gain_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_gain,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_p,
+ (prog_void *)&cmd_gain_i,
+ (prog_void *)&cmd_gain_d,
+ NULL,
+ },
+};
+
+/* show */
+/* this structure is filled when cmd_gain is parsed successfully */
+struct cmd_gain_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_gain_show_arg[] = "show";
+parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
+
+prog_char help_gain_show[] = "Show gain values for PID";
+parse_pgm_inst_t cmd_gain_show = {
+ .f = cmd_gain_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_gain_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Speeds for control system */
+
+/* this structure is filled when cmd_speed is parsed successfully */
+struct cmd_speed_result {
+ struct cmd_cs_result cs;
+ uint16_t s;
+};
+
+/* function called when cmd_speed is parsed successfully */
+static void cmd_speed_parsed(void *parsed_result, void *show)
+{
+ struct cmd_speed_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+#if notyet
+ if (!show)
+ ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
+
+ printf_P(PSTR("%s %lu\r\n"),
+ res->cs.csname,
+ ext.r_b.var_pos);
+#else
+ printf_P(PSTR("TODO\r\n"));
+#endif
+}
+
+prog_char str_speed_arg0[] = "speed";
+parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
+parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
+
+prog_char help_speed[] = "Set speed values for ramp filter";
+parse_pgm_inst_t cmd_speed = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_speed,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_s,
+ NULL,
+ },
+};
+
+/* show */
+struct cmd_speed_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
+
+prog_char help_speed_show[] = "Show speed values for ramp filter";
+parse_pgm_inst_t cmd_speed_show = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_speed_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Derivate_Filters for control system */
+
+/* this structure is filled when cmd_derivate_filter is parsed successfully */
+struct cmd_derivate_filter_result {
+ struct cmd_cs_result cs;
+ uint8_t size;
+};
+
+/* function called when cmd_derivate_filter is parsed successfully */
+static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
+{
+ struct cmd_derivate_filter_result * res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_derivate_filter(&csb->pid, res->size);
+
+ printf_P(PSTR("%s %s %u\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ pid_get_derivate_filter(&csb->pid));
+}
+
+prog_char str_derivate_filter_arg0[] = "derivate_filter";
+parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
+parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
+
+prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
+parse_pgm_inst_t cmd_derivate_filter = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_derivate_filter,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_size,
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_derivate_filter_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_derivate_filter_show_arg[] = "show";
+parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
+
+prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
+parse_pgm_inst_t cmd_derivate_filter_show = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_derivate_filter_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_show_arg,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Consign for control system */
+
+/* this structure is filled when cmd_consign is parsed successfully */
+struct cmd_consign_result {
+ struct cmd_cs_result cs;
+ int32_t p;
+};
+
+/* function called when cmd_consign is parsed successfully */
+static void cmd_consign_parsed(void * parsed_result, void *data)
+{
+ struct cmd_consign_result * res = parsed_result;
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ cs_set_consign(&csb->cs, res->p);
+}
+
+prog_char str_consign_arg0[] = "consign";
+parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
+parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
+
+prog_char help_consign[] = "Set consign value";
+parse_pgm_inst_t cmd_consign = {
+ .f = cmd_consign_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_consign,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_consign_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_consign_p,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Maximums for control system */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_result {
+ struct cmd_cs_result cs;
+ uint32_t in;
+ uint32_t i;
+ uint32_t out;
+};
+
+/* function called when cmd_maximum is parsed successfully */
+static void cmd_maximum_parsed(void *parsed_result, void *show)
+{
+ struct cmd_maximum_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ pid_set_maximums(&csb->pid, res->in, res->i, res->out);
+
+ printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
+ res->cs.csname,
+ pid_get_max_in(&csb->pid),
+ pid_get_max_I(&csb->pid),
+ pid_get_max_out(&csb->pid));
+}
+
+prog_char str_maximum_arg0[] = "maximum";
+parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
+parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
+parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
+parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
+
+prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
+parse_pgm_inst_t cmd_maximum = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_maximum,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_in,
+ (prog_void *)&cmd_maximum_i,
+ (prog_void *)&cmd_maximum_out,
+ NULL,
+ },
+};
+
+/* show */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+prog_char str_maximum_show_arg[] = "show";
+parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
+
+prog_char help_maximum_show[] = "Show maximum values for PID";
+parse_pgm_inst_t cmd_maximum_show = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_maximum_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Quadramp for control system */
+
+/* this structure is filled when cmd_quadramp is parsed successfully */
+struct cmd_quadramp_result {
+ struct cmd_cs_result cs;
+ uint32_t ap;
+ uint32_t an;
+ uint32_t sp;
+ uint32_t sn;
+};
+
+/* function called when cmd_quadramp is parsed successfully */
+static void cmd_quadramp_parsed(void *parsed_result, void *show)
+{
+ struct cmd_quadramp_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show) {
+ quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
+ quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
+ }
+
+ printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
+ res->cs.csname,
+ csb->qr.var_2nd_ord_pos,
+ csb->qr.var_2nd_ord_neg,
+ csb->qr.var_1st_ord_pos,
+ csb->qr.var_1st_ord_neg);
+}
+
+prog_char str_quadramp_arg0[] = "quadramp";
+parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+
+prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
+parse_pgm_inst_t cmd_quadramp = {
+ .f = cmd_quadramp_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_quadramp,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_ap,
+ (prog_void *)&cmd_quadramp_an,
+ (prog_void *)&cmd_quadramp_sp,
+ (prog_void *)&cmd_quadramp_sn,
+
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_quadramp_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_quadramp_show_arg[] = "show";
+parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
+
+prog_char help_quadramp_show[] = "Get quadramp values for control system";
+parse_pgm_inst_t cmd_quadramp_show = {
+ .f = cmd_quadramp_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_quadramp_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_show_arg,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* cs_status show for control system */
+
+/* this structure is filled when cmd_cs_status is parsed successfully */
+struct cmd_cs_status_result {
+ struct cmd_cs_result cs;
+ fixed_string_t arg;
+};
+
+/* function called when cmd_cs_status is parsed successfully */
+static void cmd_cs_status_parsed(void *parsed_result, void *data)
+{
+ struct cmd_cs_status_result *res = parsed_result;
+ struct cs_block *csb;
+ uint8_t loop = 0;
+ uint8_t print_pid = 0, print_cs = 0;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+ if (strcmp_P(res->arg, PSTR("on")) == 0) {
+ csb->on = 1;
+ printf_P(PSTR("%s is on\r\n"), res->cs.csname);
+ return;
+ }
+ else if (strcmp_P(res->arg, PSTR("off")) == 0) {
+ csb->on = 0;
+ printf_P(PSTR("%s is off\r\n"), res->cs.csname);
+ return;
+ }
+ else if (strcmp_P(res->arg, PSTR("show")) == 0) {
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
+ loop = 1;
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
+ print_pid = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
+ print_pid = 1;
+ loop = 1;
+ }
+
+ printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
+ do {
+ if (print_cs)
+ dump_cs(res->cs.csname, &csb->cs);
+ if (print_pid)
+ dump_pid(res->cs.csname, &csb->pid);
+ wait_ms(100);
+ } while(loop && !cmdline_keypressed());
+}
+
+prog_char str_cs_status_arg0[] = "cs_status";
+parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
+prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
+parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
+
+prog_char help_cs_status[] = "Show cs status";
+parse_pgm_inst_t cmd_cs_status = {
+ .f = cmd_cs_status_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_cs_status,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_cs_status_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_cs_status_arg,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Blocking_I for control system */
+
+/* this structure is filled when cmd_blocking_i is parsed successfully */
+struct cmd_blocking_i_result {
+ struct cmd_cs_result cs;
+ int32_t k1;
+ int32_t k2;
+ uint32_t i;
+ uint16_t cpt;
+};
+
+/* function called when cmd_blocking_i is parsed successfully */
+static void cmd_blocking_i_parsed(void *parsed_result, void *show)
+{
+ struct cmd_blocking_i_result * res = parsed_result;
+
+ struct cs_block *csb;
+
+ csb = cs_from_name(res->cs.csname);
+ if (csb == NULL) {
+ printf_P(PSTR("null csb\r\n"));
+ return;
+ }
+
+ if (!show)
+ bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
+ res->i, res->cpt);
+
+ printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
+ res->cs.cmdname,
+ res->cs.csname,
+ csb->bd.k1,
+ csb->bd.k2,
+ csb->bd.i_thres,
+ csb->bd.cpt_thres);
+}
+
+prog_char str_blocking_i_arg0[] = "blocking";
+parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
+parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
+parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
+parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
+parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
+
+prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
+parse_pgm_inst_t cmd_blocking_i = {
+ .f = cmd_blocking_i_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_blocking_i,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_k1,
+ (prog_void *)&cmd_blocking_i_k2,
+ (prog_void *)&cmd_blocking_i_i,
+ (prog_void *)&cmd_blocking_i_cpt,
+ NULL,
+ },
+};
+
+/* show */
+
+struct cmd_blocking_i_show_result {
+ struct cmd_cs_result cs;
+ fixed_string_t show;
+};
+
+prog_char str_blocking_i_show_arg[] = "show";
+parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg);
+
+prog_char help_blocking_i_show[] = "Show blocking detection values";
+parse_pgm_inst_t cmd_blocking_i_show = {
+ .f = cmd_blocking_i_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_blocking_i_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_show_arg,
+ NULL,
+ },
+};
+
+
--- /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.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <encoders_spi.h>
+#include <adc.h>
+
+#include <scheduler.h>
+#include <scheduler_stats.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include <diagnostic.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "sensor.h"
+
+/**********************************************************/
+/* Reset */
+
+/* this structure is filled when cmd_reset is parsed successfully */
+struct cmd_reset_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_reset is parsed successfully */
+static void cmd_reset_parsed(void * parsed_result, void * data)
+{
+ reset();
+}
+
+prog_char str_reset_arg0[] = "reset";
+parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
+
+prog_char help_reset[] = "Reset the board";
+parse_pgm_inst_t cmd_reset = {
+ .f = cmd_reset_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_reset,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_reset_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Bootloader */
+
+/* this structure is filled when cmd_bootloader is parsed successfully */
+struct cmd_bootloader_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_bootloader is parsed successfully */
+static void cmd_bootloader_parsed(void *parsed_result, void *data)
+{
+ bootloader();
+}
+
+prog_char str_bootloader_arg0[] = "bootloader";
+parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
+
+prog_char help_bootloader[] = "Launch the bootloader";
+parse_pgm_inst_t cmd_bootloader = {
+ .f = cmd_bootloader_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_bootloader,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_bootloader_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Encoders tests */
+
+/* this structure is filled when cmd_encoders is parsed successfully */
+struct cmd_encoders_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_encoders is parsed successfully */
+static void cmd_encoders_parsed(void *parsed_result, void *data)
+{
+ struct cmd_encoders_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ encoders_spi_set_value((void *)0, 0);
+ encoders_spi_set_value((void *)1, 0);
+ encoders_spi_set_value((void *)2, 0);
+ encoders_spi_set_value((void *)3, 0);
+ return;
+ }
+
+ /* show */
+ while(!cmdline_keypressed()) {
+ printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
+ encoders_spi_get_value((void *)0),
+ encoders_spi_get_value((void *)1),
+ encoders_spi_get_value((void *)2),
+ encoders_spi_get_value((void *)3));
+ wait_ms(100);
+ }
+}
+
+prog_char str_encoders_arg0[] = "encoders";
+parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
+prog_char str_encoders_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
+
+prog_char help_encoders[] = "Show encoders values";
+parse_pgm_inst_t cmd_encoders = {
+ .f = cmd_encoders_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_encoders,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_encoders_arg0,
+ (prog_void *)&cmd_encoders_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Scheduler show */
+
+/* this structure is filled when cmd_scheduler is parsed successfully */
+struct cmd_scheduler_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_scheduler is parsed successfully */
+static void cmd_scheduler_parsed(void *parsed_result, void *data)
+{
+ scheduler_dump_events();
+ scheduler_stats_dump();
+}
+
+prog_char str_scheduler_arg0[] = "scheduler";
+parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
+prog_char str_scheduler_arg1[] = "show";
+parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
+
+prog_char help_scheduler[] = "Show scheduler events";
+parse_pgm_inst_t cmd_scheduler = {
+ .f = cmd_scheduler_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_scheduler,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_scheduler_arg0,
+ (prog_void *)&cmd_scheduler_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Pwms tests */
+
+/* this structure is filled when cmd_pwm is parsed successfully */
+struct cmd_pwm_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t arg2;
+};
+
+/* function called when cmd_pwm is parsed successfully */
+static void cmd_pwm_parsed(void * parsed_result, void * data)
+{
+ void * pwm_ptr = NULL;
+ struct cmd_pwm_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("1(4A)")))
+ pwm_ptr = &gen.pwm1_4A;
+ else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
+ pwm_ptr = &gen.pwm2_4B;
+ else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
+ pwm_ptr = &gen.pwm3_1A;
+ else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
+ pwm_ptr = &gen.pwm4_1B;
+
+ else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
+ pwm_ptr = &gen.servo1;
+ else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
+ pwm_ptr = &gen.servo2;
+ else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
+ pwm_ptr = &gen.servo3;
+ else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
+ pwm_ptr = &gen.servo4;
+
+ if (pwm_ptr)
+ pwm_ng_set(pwm_ptr, res->arg2);
+
+ printf_P(PSTR("done\r\n"));
+}
+
+prog_char str_pwm_arg0[] = "pwm";
+parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
+prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)";
+parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
+parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
+
+prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
+parse_pgm_inst_t cmd_pwm = {
+ .f = cmd_pwm_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pwm,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pwm_arg0,
+ (prog_void *)&cmd_pwm_arg1,
+ (prog_void *)&cmd_pwm_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Adcs tests */
+
+/* this structure is filled when cmd_adc is parsed successfully */
+struct cmd_adc_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_adc is parsed successfully */
+static void cmd_adc_parsed(void *parsed_result, void *data)
+{
+ struct cmd_adc_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("ADC values: "));
+ for (i=0; i<ADC_MAX; i++) {
+ printf_P(PSTR("%.4d "), sensor_get_adc(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_adc_arg0[] = "adc";
+parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
+prog_char str_adc_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
+
+prog_char help_adc[] = "Show adc values";
+parse_pgm_inst_t cmd_adc = {
+ .f = cmd_adc_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_adc,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_adc_arg0,
+ (prog_void *)&cmd_adc_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Sensors tests */
+
+/* this structure is filled when cmd_sensor is parsed successfully */
+struct cmd_sensor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_sensor is parsed successfully */
+static void cmd_sensor_parsed(void *parsed_result, void *data)
+{
+ struct cmd_sensor_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("SENSOR values: "));
+ for (i=0; i<SENSOR_MAX; i++) {
+ printf_P(PSTR("%d "), !!sensor_get(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_sensor_arg0[] = "sensor";
+parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
+prog_char str_sensor_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
+
+prog_char help_sensor[] = "Show sensor values";
+parse_pgm_inst_t cmd_sensor = {
+ .f = cmd_sensor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sensor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_sensor_arg0,
+ (prog_void *)&cmd_sensor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+ fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const prog_char uart_log[] = "uart";
+static const prog_char rs_log[] = "rs";
+static const prog_char traj_log[] = "traj";
+static const prog_char i2c_log[] = "i2c";
+static const prog_char oa_log[] = "oa";
+static const prog_char strat_log[] = "strat";
+static const prog_char i2cproto_log[] = "i2cproto";
+static const prog_char sensor_log[] = "sensor";
+static const prog_char block_log[] = "bd";
+static const prog_char cs_log[] = "cs";
+
+struct log_name_and_num {
+ const prog_char * name;
+ uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+ { uart_log, E_UART },
+ { rs_log, E_ROBOT_SYSTEM },
+ { traj_log, E_TRAJECTORY },
+ { i2c_log, E_I2C },
+ { oa_log, E_OA },
+ { strat_log, E_USER_STRAT },
+ { i2cproto_log, E_USER_I2C_PROTO },
+ { sensor_log, E_USER_SENSOR },
+ { block_log, E_BLOCKING_DETECTION_MANAGER },
+ { cs_log, E_USER_CS },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (!strcmp_P(s, log_name_and_num[i].name)) {
+ return log_name_and_num[i].num;
+ }
+ }
+ return 0;
+}
+
+const prog_char *
+log_num2name(uint8_t num)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (num == log_name_and_num[i].num) {
+ return log_name_and_num[i].name;
+ }
+ }
+ return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+ uint8_t i, empty=1;
+ const prog_char * name;
+
+ printf_P(PSTR("log level is %d\r\n"), gen.log_level);
+ for (i=0; i<NB_LOGS; i++) {
+ name = log_num2name(gen.logs[i]);
+ if (name) {
+ printf_P(PSTR("log type %S is on\r\n"), name);
+ empty = 0;
+ }
+ }
+ if (empty)
+ printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, void * data)
+{
+ struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("level"))) {
+ gen.log_level = res->arg2;
+ }
+
+ /* else it is a show */
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg0[] = "log";
+parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
+prog_char str_log_arg1[] = "level";
+parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
+parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
+
+prog_char help_log[] = "Set log options: level (0 -> 5)";
+parse_pgm_inst_t cmd_log = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1,
+ (prog_void *)&cmd_log_arg2,
+ NULL,
+ },
+};
+
+prog_char str_log_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
+
+prog_char help_log_show[] = "Show configured logs";
+parse_pgm_inst_t cmd_log_show = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_show,
+ NULL,
+ },
+};
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_type_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_type_parsed(void * parsed_result, void * data)
+{
+ struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
+ uint8_t lognum;
+ uint8_t i;
+
+ lognum = log_name2num(res->arg2);
+ if (lognum == 0) {
+ printf_P(PSTR("Cannot find log num\r\n"));
+ return;
+ }
+
+ if (!strcmp_P(res->arg3, PSTR("on"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ printf_P(PSTR("Already on\r\n"));
+ return;
+ }
+ }
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == 0) {
+ gen.logs[i] = lognum;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("no more room\r\n"));
+ }
+ }
+ else if (!strcmp_P(res->arg3, PSTR("off"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ gen.logs[i] = 0;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("already off\r\n"));
+ }
+ }
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg1_type[] = "type";
+parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+prog_char str_log_arg2_type[] = "uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor#bd#cs";
+parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+prog_char str_log_arg3[] = "on#off";
+parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+prog_char help_log_type[] = "Set log type";
+parse_pgm_inst_t cmd_log_type = {
+ .f = cmd_log_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_type,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_type,
+ (prog_void *)&cmd_log_arg2_type,
+ (prog_void *)&cmd_log_arg3,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(void *parsed_result, void *data)
+{
+ printf("res stack: %d\r\n", min_stack_space_available());
+
+}
+
+prog_char str_stack_space_arg0[] = "stack_space";
+parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+prog_char help_stack_space[] = "Display remaining stack space";
+parse_pgm_inst_t cmd_stack_space = {
+ .f = cmd_stack_space_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_stack_space,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_stack_space_arg0,
+ NULL,
+ },
+};
--- /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_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <avr/eeprom.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+#include <i2c.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "../common/eeprom_mapping.h"
+
+#include "main.h"
+#include "sensor.h"
+#include "cmdline.h"
+#include "strat.h"
+#include "strat_utils.h"
+#include "strat_base.h"
+#include "i2c_protocol.h"
+#include "actuator.h"
+
+struct cmd_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+
+/* function called when cmd_event is parsed successfully */
+static void cmd_event_parsed(void *parsed_result, void *data)
+{
+ u08 bit=0;
+
+ struct cmd_event_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("all"))) {
+ bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
+ DO_BD | DO_TIMER | DO_POWER;
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ mainboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ mainboard.flags &= bit;
+ else { /* show */
+ printf_P(PSTR("encoders is %s\r\n"),
+ (DO_ENCODERS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("cs is %s\r\n"),
+ (DO_CS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("rs is %s\r\n"),
+ (DO_RS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("pos is %s\r\n"),
+ (DO_POS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("bd is %s\r\n"),
+ (DO_BD & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("timer is %s\r\n"),
+ (DO_TIMER & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("power is %s\r\n"),
+ (DO_POWER & mainboard.flags) ? "on":"off");
+ }
+ return;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("encoders")))
+ bit = DO_ENCODERS;
+ else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ strat_hardstop();
+ bit = DO_CS;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("rs")))
+ bit = DO_RS;
+ else if (!strcmp_P(res->arg1, PSTR("pos")))
+ bit = DO_POS;
+ else if (!strcmp_P(res->arg1, PSTR("bd")))
+ bit = DO_BD;
+ else if (!strcmp_P(res->arg1, PSTR("timer"))) {
+ time_reset();
+ bit = DO_TIMER;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("power")))
+ bit = DO_POWER;
+
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ mainboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off"))) {
+ if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ pwm_ng_set(LEFT_PWM, 0);
+ pwm_ng_set(RIGHT_PWM, 0);
+ }
+ mainboard.flags &= (~bit);
+ }
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ (bit & mainboard.flags) ? "on":"off");
+}
+
+prog_char str_event_arg0[] = "event";
+parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
+prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
+parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
+prog_char str_event_arg2[] = "on#off#show";
+parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
+
+prog_char help_event[] = "Enable/disable events";
+parse_pgm_inst_t cmd_event = {
+ .f = cmd_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_event_arg0,
+ (prog_void *)&cmd_event_arg1,
+ (prog_void *)&cmd_event_arg2,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Spi_Test */
+
+/* this structure is filled when cmd_spi_test is parsed successfully */
+struct cmd_spi_test_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_spi_test is parsed successfully */
+static void cmd_spi_test_parsed(void * parsed_result, void * data)
+{
+ uint16_t i = 0, ret = 0, ret2 = 0;
+
+ if (mainboard.flags & DO_ENCODERS) {
+ printf_P(PSTR("Disable encoder event first\r\n"));
+ return;
+ }
+
+ do {
+ spi_slave_select(0);
+ ret = spi_send_and_receive_byte(i);
+ ret2 = spi_send_and_receive_byte(i);
+ spi_slave_deselect(0);
+
+ if ((i & 0x7ff) == 0)
+ printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
+ i, ret, ret2);
+
+ i++;
+ } while(!cmdline_keypressed());
+}
+
+prog_char str_spi_test_arg0[] = "spi_test";
+parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
+
+prog_char help_spi_test[] = "Test the SPI";
+parse_pgm_inst_t cmd_spi_test = {
+ .f = cmd_spi_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_spi_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_spi_test_arg0,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* Opponent tests */
+
+/* this structure is filled when cmd_opponent is parsed successfully */
+struct cmd_opponent_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+};
+
+/* function called when cmd_opponent is parsed successfully */
+static void cmd_opponent_parsed(void *parsed_result, void *data)
+{
+ int16_t x,y,d,a;
+
+ if (get_opponent_xyda(&x, &y, &d, &a))
+ printf_P(PSTR("No opponent\r\n"));
+ else
+ printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
+}
+
+prog_char str_opponent_arg0[] = "opponent";
+parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
+prog_char str_opponent_arg1[] = "show";
+parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
+
+prog_char help_opponent[] = "Show (x,y) opponent";
+parse_pgm_inst_t cmd_opponent = {
+ .f = cmd_opponent_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_opponent,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_opponent_arg0,
+ (prog_void *)&cmd_opponent_arg1,
+ NULL,
+ },
+};
+
+
+prog_char str_opponent_arg1_set[] = "set";
+parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
+parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
+parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
+
+prog_char help_opponent_set[] = "Set (x,y) opponent";
+parse_pgm_inst_t cmd_opponent_set = {
+ .f = cmd_opponent_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_opponent_set,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_opponent_arg0,
+ (prog_void *)&cmd_opponent_arg1_set,
+ (prog_void *)&cmd_opponent_arg2,
+ (prog_void *)&cmd_opponent_arg3,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Start */
+
+/* this structure is filled when cmd_start is parsed successfully */
+struct cmd_start_result {
+ fixed_string_t arg0;
+ fixed_string_t color;
+ fixed_string_t debug;
+};
+
+/* function called when cmd_start is parsed successfully */
+static void cmd_start_parsed(void *parsed_result, void *data)
+{
+ struct cmd_start_result *res = parsed_result;
+ uint8_t old_level = gen.log_level;
+
+ gen.logs[NB_LOGS] = E_USER_STRAT;
+ if (!strcmp_P(res->debug, PSTR("debug"))) {
+ strat_infos.dump_enabled = 1;
+ gen.log_level = 5;
+ }
+ else {
+ strat_infos.dump_enabled = 0;
+ gen.log_level = 0;
+ }
+
+ if (!strcmp_P(res->color, PSTR("red"))) {
+ mainboard.our_color = I2C_COLOR_RED;
+ i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
+ i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
+ }
+ else if (!strcmp_P(res->color, PSTR("green"))) {
+ mainboard.our_color = I2C_COLOR_GREEN;
+ i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
+ i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
+ }
+
+ printf_P(PSTR("Check that lintel is loaded\r\n"));
+ while(!cmdline_keypressed());
+
+ printf_P(PSTR("Press a key when beacon ready\r\n"));
+ i2c_sensorboard_set_beacon(0);
+ while(!cmdline_keypressed());
+ i2c_sensorboard_set_beacon(1);
+
+ strat_start();
+
+ gen.logs[NB_LOGS] = 0;
+ gen.log_level = old_level;
+}
+
+prog_char str_start_arg0[] = "start";
+parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
+prog_char str_start_color[] = "green#red";
+parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
+prog_char str_start_debug[] = "debug#match";
+parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
+
+prog_char help_start[] = "Start the robot";
+parse_pgm_inst_t cmd_start = {
+ .f = cmd_start_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_start,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_start_arg0,
+ (prog_void *)&cmd_start_color,
+ (prog_void *)&cmd_start_debug,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* Interact */
+
+/* this structure is filled when cmd_interact is parsed successfully */
+struct cmd_interact_result {
+ fixed_string_t arg0;
+};
+
+static void print_cs(void)
+{
+ printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
+ "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
+ cs_get_consign(&mainboard.distance.cs),
+ cs_get_consign(&mainboard.angle.cs),
+ cs_get_filtered_consign(&mainboard.distance.cs),
+ cs_get_filtered_consign(&mainboard.angle.cs),
+ cs_get_error(&mainboard.distance.cs),
+ cs_get_error(&mainboard.angle.cs),
+ cs_get_out(&mainboard.distance.cs),
+ cs_get_out(&mainboard.angle.cs));
+}
+
+static void print_pos(void)
+{
+ printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+}
+
+static void print_time(void)
+{
+ printf_P(PSTR("time %d\r\n"),time_get_s());
+}
+
+
+static void print_sensors(void)
+{
+#ifdef notyet
+ if (sensor_start_switch())
+ printf_P(PSTR("Start switch | "));
+ else
+ printf_P(PSTR(" | "));
+
+ if (IR_DISP_SENSOR())
+ printf_P(PSTR("IR disp | "));
+ else
+ printf_P(PSTR(" | "));
+
+ printf_P(PSTR("\r\n"));
+#endif
+}
+
+static void print_pid(void)
+{
+ printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
+ "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
+ pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
+ pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
+ pid_get_value_out(&mainboard.distance.pid),
+ pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
+ pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
+ pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
+ pid_get_value_out(&mainboard.angle.pid));
+}
+
+#define PRINT_POS (1<<0)
+#define PRINT_PID (1<<1)
+#define PRINT_CS (1<<2)
+#define PRINT_SENSORS (1<<3)
+#define PRINT_TIME (1<<4)
+#define PRINT_BLOCKING (1<<5)
+
+static void cmd_interact_parsed(void * parsed_result, void * data)
+{
+ int c;
+ int8_t cmd;
+ uint8_t print = 0;
+ struct vt100 vt100;
+
+ vt100_init(&vt100);
+
+ printf_P(PSTR("Display debugs:\r\n"
+ " 1:pos\r\n"
+ " 2:pid\r\n"
+ " 3:cs\r\n"
+ " 4:sensors\r\n"
+ " 5:time\r\n"
+ /* " 6:blocking\r\n" */
+ "Commands:\r\n"
+ " arrows:move\r\n"
+ " space:stop\r\n"
+ " q:quit\r\n"));
+
+ /* stop motors */
+ mainboard.flags &= (~DO_CS);
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
+
+ while(1) {
+ if (print & PRINT_POS) {
+ print_pos();
+ }
+
+ if (print & PRINT_PID) {
+ print_pid();
+ }
+
+ if (print & PRINT_CS) {
+ print_cs();
+ }
+
+ if (print & PRINT_SENSORS) {
+ print_sensors();
+ }
+
+ if (print & PRINT_TIME) {
+ print_time();
+ }
+/* if (print & PRINT_BLOCKING) { */
+/* printf_P(PSTR("%s %s blocking=%d\r\n"), */
+/* mainboard.blocking ? "BLOCK1":" ", */
+/* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
+/* rs_get_blocking(&mainboard.rs)); */
+/* } */
+
+ c = cmdline_getchar();
+ if (c == -1) {
+ wait_ms(10);
+ continue;
+ }
+ cmd = vt100_parser(&vt100, c);
+ if (cmd == -2) {
+ wait_ms(10);
+ continue;
+ }
+
+ if (cmd == -1) {
+ switch(c) {
+ case '1': print ^= PRINT_POS; break;
+ case '2': print ^= PRINT_PID; break;
+ case '3': print ^= PRINT_CS; break;
+ case '4': print ^= PRINT_SENSORS; break;
+ case '5': print ^= PRINT_TIME; break;
+ case '6': print ^= PRINT_BLOCKING; break;
+
+ case 'q':
+ if (mainboard.flags & DO_CS)
+ strat_hardstop();
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
+ return;
+ case ' ':
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
+ break;
+ default:
+ break;
+ }
+ }
+ else {
+ switch(cmd) {
+ case KEY_UP_ARR:
+ pwm_set_and_save(LEFT_PWM, 1200);
+ pwm_set_and_save(RIGHT_PWM, 1200);
+ break;
+ case KEY_LEFT_ARR:
+ pwm_set_and_save(LEFT_PWM, -1200);
+ pwm_set_and_save(RIGHT_PWM, 1200);
+ break;
+ case KEY_DOWN_ARR:
+ pwm_set_and_save(LEFT_PWM, -1200);
+ pwm_set_and_save(RIGHT_PWM, -1200);
+ break;
+ case KEY_RIGHT_ARR:
+ pwm_set_and_save(LEFT_PWM, 1200);
+ pwm_set_and_save(RIGHT_PWM, -1200);
+ break;
+ }
+ }
+ wait_ms(10);
+ }
+}
+
+prog_char str_interact_arg0[] = "interact";
+parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
+
+prog_char help_interact[] = "Interactive mode";
+parse_pgm_inst_t cmd_interact = {
+ .f = cmd_interact_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_interact,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_interact_arg0,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Color */
+
+/* this structure is filled when cmd_color is parsed successfully */
+struct cmd_color_result {
+ fixed_string_t arg0;
+ fixed_string_t color;
+};
+
+/* function called when cmd_color is parsed successfully */
+static void cmd_color_parsed(void *parsed_result, void *data)
+{
+ struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
+ if (!strcmp_P(res->color, PSTR("red"))) {
+ mainboard.our_color = I2C_COLOR_RED;
+ i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
+ i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
+ }
+ else if (!strcmp_P(res->color, PSTR("green"))) {
+ mainboard.our_color = I2C_COLOR_GREEN;
+ i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
+ i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
+ }
+ printf_P(PSTR("Done\r\n"));
+}
+
+prog_char str_color_arg0[] = "color";
+parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
+prog_char str_color_color[] = "green#red";
+parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
+
+prog_char help_color[] = "Set our color";
+parse_pgm_inst_t cmd_color = {
+ .f = cmd_color_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_color,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_color_arg0,
+ (prog_void *)&cmd_color_color,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Rs tests */
+
+/* this structure is filled when cmd_rs is parsed successfully */
+struct cmd_rs_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_rs is parsed successfully */
+static void cmd_rs_parsed(void *parsed_result, void *data)
+{
+ // struct cmd_rs_result *res = parsed_result;
+ do {
+ printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
+ cs_get_consign(&mainboard.angle.cs),
+ cs_get_filtered_feedback(&mainboard.angle.cs),
+ cs_get_out(&mainboard.angle.cs));
+ printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
+ cs_get_consign(&mainboard.distance.cs),
+ cs_get_filtered_feedback(&mainboard.distance.cs),
+ cs_get_out(&mainboard.distance.cs));
+ printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
+ mainboard.pwm_r);
+ wait_ms(100);
+ } while(!cmdline_keypressed());
+}
+
+prog_char str_rs_arg0[] = "rs";
+parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
+prog_char str_rs_arg1[] = "show";
+parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
+
+prog_char help_rs[] = "Show rs (robot system) values";
+parse_pgm_inst_t cmd_rs = {
+ .f = cmd_rs_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_arg0,
+ (prog_void *)&cmd_rs_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* I2cdebug */
+
+/* this structure is filled when cmd_i2cdebug is parsed successfully */
+struct cmd_i2cdebug_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_i2cdebug is parsed successfully */
+static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
+{
+ i2c_debug();
+ i2c_protocol_debug();
+}
+
+prog_char str_i2cdebug_arg0[] = "i2cdebug";
+parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
+
+prog_char help_i2cdebug[] = "I2c debug infos";
+parse_pgm_inst_t cmd_i2cdebug = {
+ .f = cmd_i2cdebug_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_i2cdebug,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_i2cdebug_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Mechboard_Show */
+
+/* this structure is filled when cmd_mechboard_show is parsed successfully */
+struct cmd_mechboard_show_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_mechboard_show is parsed successfully */
+static void cmd_mechboard_show_parsed(void * parsed_result, void * data)
+{
+ printf_P(PSTR("mode = %x\r\n"), mechboard.mode);
+ printf_P(PSTR("status = %x\r\n"), mechboard.status);
+ printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count);
+
+ printf_P(PSTR("column_count = %d\r\n"), get_column_count());
+ printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"),
+ pump_left1_is_full(), pump_left2_is_full(),
+ pump_right1_is_full(), pump_right2_is_full());
+
+ printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1);
+ printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2);
+ printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1);
+ printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2);
+
+ printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left);
+ printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right);
+
+}
+
+prog_char str_mechboard_show_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0);
+prog_char str_mechboard_show_arg1[] = "show";
+parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1);
+
+prog_char help_mechboard_show[] = "show mechboard status";
+parse_pgm_inst_t cmd_mechboard_show = {
+ .f = cmd_mechboard_show_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_mechboard_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_mechboard_show_arg0,
+ (prog_void *)&cmd_mechboard_show_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Mechboard_Setmode1 */
+
+/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
+struct cmd_mechboard_setmode1_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_mechboard_setmode1 is parsed successfully */
+static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data)
+{
+ struct cmd_mechboard_setmode1_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("init")))
+ i2c_mechboard_mode_init();
+ else if (!strcmp_P(res->arg1, PSTR("manual")))
+ i2c_mechboard_mode_manual();
+ else if (!strcmp_P(res->arg1, PSTR("pickup")))
+ i2c_mechboard_mode_pickup();
+ else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
+ i2c_mechboard_mode_lazy_harvest();
+ else if (!strcmp_P(res->arg1, PSTR("harvest")))
+ i2c_mechboard_mode_harvest();
+ else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
+ i2c_mechboard_mode_prepare_get_lintel();
+ else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
+ i2c_mechboard_mode_get_lintel();
+ else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
+ i2c_mechboard_mode_put_lintel();
+ else if (!strcmp_P(res->arg1, PSTR("init")))
+ i2c_mechboard_mode_init();
+ else if (!strcmp_P(res->arg1, PSTR("eject")))
+ i2c_mechboard_mode_init();
+ else if (!strcmp_P(res->arg1, PSTR("clear")))
+ i2c_mechboard_mode_clear();
+ else if (!strcmp_P(res->arg1, PSTR("loaded")))
+ i2c_mechboard_mode_loaded();
+ else if (!strcmp_P(res->arg1, PSTR("store")))
+ i2c_mechboard_mode_store();
+ else if (!strcmp_P(res->arg1, PSTR("manivelle")))
+ i2c_mechboard_mode_manivelle();
+ else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
+ i2c_mechboard_mode_lazy_pickup();
+}
+
+prog_char str_mechboard_setmode1_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0);
+prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup";
+parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1);
+
+prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)";
+parse_pgm_inst_t cmd_mechboard_setmode1 = {
+ .f = cmd_mechboard_setmode1_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_mechboard_setmode1,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_mechboard_setmode1_arg0,
+ (prog_void *)&cmd_mechboard_setmode1_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Mechboard_Setmode2 */
+
+/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
+struct cmd_mechboard_setmode2_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_mechboard_setmode2 is parsed successfully */
+static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data)
+{
+ struct cmd_mechboard_setmode2_result *res = parsed_result;
+ uint8_t side = I2C_LEFT_SIDE;
+
+ if (!strcmp_P(res->arg2, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("right")))
+ side = I2C_RIGHT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("center")))
+ side = I2C_CENTER_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("auto")))
+ side = I2C_AUTO_SIDE;
+
+ if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
+ i2c_mechboard_mode_prepare_pickup(side);
+ else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
+ i2c_mechboard_mode_push_temple_disc(side);
+}
+
+prog_char str_mechboard_setmode2_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0);
+prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc";
+parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1);
+prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center";
+parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2);
+
+prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)";
+parse_pgm_inst_t cmd_mechboard_setmode2 = {
+ .f = cmd_mechboard_setmode2_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_mechboard_setmode2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_mechboard_setmode2_arg0,
+ (prog_void *)&cmd_mechboard_setmode2_arg1,
+ (prog_void *)&cmd_mechboard_setmode2_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Mechboard_Setmode3 */
+
+/* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */
+struct cmd_mechboard_setmode3_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t level;
+};
+
+/* function called when cmd_mechboard_setmode3 is parsed successfully */
+static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data)
+{
+ struct cmd_mechboard_setmode3_result *res = parsed_result;
+ if (!strcmp_P(res->arg1, PSTR("autobuild")))
+ i2c_mechboard_mode_simple_autobuild(res->level);
+ else if (!strcmp_P(res->arg1, PSTR("prepare_build")))
+ i2c_mechboard_mode_prepare_build_both(res->level);
+ else if (!strcmp_P(res->arg1, PSTR("prepare_inside")))
+ i2c_mechboard_mode_prepare_inside_both(res->level);
+ else if (!strcmp_P(res->arg1, PSTR("push_temple")))
+ i2c_mechboard_mode_push_temple(res->level);
+}
+
+prog_char str_mechboard_setmode3_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0);
+prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple";
+parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1);
+parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8);
+
+prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)";
+parse_pgm_inst_t cmd_mechboard_setmode3 = {
+ .f = cmd_mechboard_setmode3_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_mechboard_setmode3,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_mechboard_setmode3_arg0,
+ (prog_void *)&cmd_mechboard_setmode3_arg1,
+ (prog_void *)&cmd_mechboard_setmode3_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Mechboard_Setmode4 */
+
+/* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */
+struct cmd_mechboard_setmode4_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t level_l;
+ uint8_t count_l;
+ uint8_t dist_l;
+ uint8_t level_r;
+ uint8_t count_r;
+ uint8_t dist_r;
+ uint8_t do_lintel;
+};
+
+/* function called when cmd_mechboard_setmode4 is parsed successfully */
+static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data)
+{
+ struct cmd_mechboard_setmode4_result *res = parsed_result;
+ i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l,
+ res->level_r, res->count_r, res->dist_r,
+ res->do_lintel);
+}
+
+prog_char str_mechboard_setmode4_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0);
+prog_char str_mechboard_setmode4_arg1[] = "autobuild";
+parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1);
+parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8);
+parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8);
+parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8);
+parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8);
+parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8);
+parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8);
+parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8);
+
+prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
+parse_pgm_inst_t cmd_mechboard_setmode4 = {
+ .f = cmd_mechboard_setmode4_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_mechboard_setmode4,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_mechboard_setmode4_arg0,
+ (prog_void *)&cmd_mechboard_setmode4_arg1,
+ (prog_void *)&cmd_mechboard_setmode4_arg2,
+ (prog_void *)&cmd_mechboard_setmode4_arg3,
+ (prog_void *)&cmd_mechboard_setmode4_arg4,
+ (prog_void *)&cmd_mechboard_setmode4_arg5,
+ (prog_void *)&cmd_mechboard_setmode4_arg6,
+ (prog_void *)&cmd_mechboard_setmode4_arg7,
+ (prog_void *)&cmd_mechboard_setmode4_arg8,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Mechboard_Setmode5 */
+
+/* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */
+struct cmd_mechboard_setmode5_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_mechboard_setmode5 is parsed successfully */
+static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data)
+{
+ struct cmd_mechboard_setmode5_result *res = parsed_result;
+ uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
+
+ if (!strcmp_P(res->arg2, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("right")))
+ side = I2C_RIGHT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("center")))
+ side = I2C_CENTER_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("auto")))
+ side = I2C_AUTO_SIDE;
+
+ if (!strcmp_P(res->arg3, PSTR("harvest")))
+ next_mode = I2C_MECHBOARD_MODE_HARVEST;
+ else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
+ next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
+ else if (!strcmp_P(res->arg3, PSTR("pickup")))
+ next_mode = I2C_MECHBOARD_MODE_PICKUP;
+ else if (!strcmp_P(res->arg3, PSTR("clear")))
+ next_mode = I2C_MECHBOARD_MODE_CLEAR;
+ else if (!strcmp_P(res->arg3, PSTR("store")))
+ next_mode = I2C_MECHBOARD_MODE_STORE;
+ else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
+ next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
+
+ if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
+ i2c_mechboard_mode_prepare_pickup_next(side, next_mode);
+}
+
+prog_char str_mechboard_setmode5_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0);
+prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup";
+parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1);
+prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center";
+parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2);
+prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
+parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3);
+
+prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)";
+parse_pgm_inst_t cmd_mechboard_setmode5 = {
+ .f = cmd_mechboard_setmode5_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_mechboard_setmode5,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_mechboard_setmode5_arg0,
+ (prog_void *)&cmd_mechboard_setmode5_arg1,
+ (prog_void *)&cmd_mechboard_setmode5_arg2,
+ (prog_void *)&cmd_mechboard_setmode5_arg3,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* pickup wheels */
+
+/* this structure is filled when cmd_pickup_wheels is parsed successfully */
+struct cmd_pickup_wheels_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_pickup_wheels is parsed successfully */
+static void cmd_pickup_wheels_parsed(void *parsed_result, void *data)
+{
+ struct cmd_pickup_wheels_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("on")))
+ pickup_wheels_on();
+ else
+ pickup_wheels_off();
+}
+
+prog_char str_pickup_wheels_arg0[] = "pickup_wheels";
+parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0);
+prog_char str_pickup_wheels_arg1[] = "on#off";
+parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1);
+
+prog_char help_pickup_wheels[] = "Enable/disable pickup wheels";
+parse_pgm_inst_t cmd_pickup_wheels = {
+ .f = cmd_pickup_wheels_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pickup_wheels,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pickup_wheels_arg0,
+ (prog_void *)&cmd_pickup_wheels_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Beacon_Start */
+
+/* this structure is filled when cmd_beacon_start is parsed successfully */
+struct cmd_beacon_start_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_beacon_start is parsed successfully */
+static void cmd_beacon_start_parsed(void *parsed_result, void *data)
+{
+ struct cmd_beacon_start_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("start")))
+ i2c_sensorboard_set_beacon(1);
+ else
+ i2c_sensorboard_set_beacon(0);
+}
+
+prog_char str_beacon_start_arg0[] = "beacon";
+parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
+prog_char str_beacon_start_arg1[] = "start#stop";
+parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
+
+prog_char help_beacon_start[] = "Beacon enabled/disable";
+parse_pgm_inst_t cmd_beacon_start = {
+ .f = cmd_beacon_start_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_beacon_start,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_beacon_start_arg0,
+ (prog_void *)&cmd_beacon_start_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Pump_Current */
+
+/* this structure is filled when cmd_pump_current is parsed successfully */
+struct cmd_pump_current_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_pump_current is parsed successfully */
+static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
+ sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4),
+ mechboard.pump_right1_current, mechboard.pump_right2_current);
+}
+
+prog_char str_pump_current_arg0[] = "pump_current";
+parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
+prog_char str_pump_current_arg1[] = "show";
+parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
+
+prog_char help_pump_current[] = "dump pump current";
+parse_pgm_inst_t cmd_pump_current = {
+ .f = cmd_pump_current_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pump_current,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pump_current_arg0,
+ (prog_void *)&cmd_pump_current_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Build_Test */
+
+/* this structure is filled when cmd_build_test is parsed successfully */
+struct cmd_build_test_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_build_test is parsed successfully */
+static void cmd_build_test_parsed(void *parsed_result, void *data)
+{
+ //struct cmd_build_test_result *res = parsed_result;
+
+ printf_P(PSTR("lintel must be there\r\n"));
+ i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
+ I2C_MECHBOARD_MODE_HARVEST);
+ wait_ms(500);
+
+ printf_P(PSTR("Insert 4 colums\r\n"));
+ while (get_column_count() != 4);
+
+ i2c_mechboard_mode_prepare_build_both(0);
+ trajectory_d_rel(&mainboard.traj, 200);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ wait_ms(500);
+
+ i2c_mechboard_mode_simple_autobuild(0);
+ wait_ms(100);
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
+
+ trajectory_d_rel(&mainboard.traj, -200);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
+ I2C_MECHBOARD_MODE_HARVEST);
+
+ while (get_column_count() != 3);
+
+ i2c_mechboard_mode_prepare_build_both(3);
+ trajectory_d_rel(&mainboard.traj, 200);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ wait_ms(500);
+
+ i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST,
+ 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0);
+ i2cproto_wait_update();
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
+
+ trajectory_d_rel(&mainboard.traj, -200);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
+ wait_ms(500);
+
+ i2c_mechboard_mode_harvest();
+ while (get_column_count() != 3);
+
+ i2c_mechboard_mode_prepare_build_both(5);
+ trajectory_d_rel(&mainboard.traj, 200);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ wait_ms(1000);
+
+ i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST,
+ 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0);
+ i2cproto_wait_update();
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
+
+ trajectory_d_rel(&mainboard.traj, -200);
+}
+
+prog_char str_build_test_arg0[] = "build_test";
+parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0);
+
+prog_char help_build_test[] = "Build_Test function";
+parse_pgm_inst_t cmd_build_test = {
+ .f = cmd_build_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_build_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_build_test_arg0,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Column_Test */
+
+/* this structure is filled when cmd_column_test is parsed successfully */
+struct cmd_column_test_result {
+ fixed_string_t arg0;
+ uint8_t level;
+ int16_t dist;
+ int8_t a1;
+ int8_t a2;
+ int8_t a3;
+ int16_t arm_dist;
+ int8_t nb_col;
+};
+
+/* function called when cmd_column_test is parsed successfully */
+static void cmd_column_test_parsed(void *parsed_result, void *data)
+{
+ struct cmd_column_test_result *res = parsed_result;
+ uint8_t level = res->level, debug = 0;
+ uint8_t c, push = 0;
+
+ /* default conf */
+ if (data) {
+ res->dist = 70;
+ res->a1 = -20;
+ res->a2 = 40;
+ res->a3 = -20;
+ res->arm_dist = 220;
+ res->nb_col = 2;
+ }
+
+ if (!strcmp_P(res->arg0, PSTR("column_test_debug")))
+ debug = 1;
+ if (!strcmp_P(res->arg0, PSTR("column_test_push")))
+ push = 1;
+
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
+
+ /* Go to disc */
+
+ trajectory_d_rel(&mainboard.traj, 200);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ /* go back, insert colums */
+
+ trajectory_d_rel(&mainboard.traj, -200);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
+ I2C_MECHBOARD_MODE_HARVEST);
+ printf_P(PSTR("Insert 4 colums\r\n"));
+ while (get_column_count() != 4);
+
+ /* build with left arm */
+
+ i2c_mechboard_mode_prepare_inside_both(level);
+ trajectory_d_rel(&mainboard.traj, 200-(res->dist));
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+
+ trajectory_a_rel(&mainboard.traj, res->a1);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+
+ i2c_mechboard_mode_prepare_build_select(level, -1);
+ time_wait_ms(200);
+ if (debug)
+ c = cmdline_getchar_wait();
+ i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist,
+ 0, 0, res->arm_dist, 0);
+ while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+ i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+ /* build with right arm */
+
+ trajectory_a_rel(&mainboard.traj, res->a2);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+ /* only ok for nb_col == 2 */
+ if ((level + res->nb_col) >= 7)
+ i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1);
+ else
+ i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col);
+ time_wait_ms(200);
+ if (debug)
+ c = cmdline_getchar_wait();
+ i2c_mechboard_mode_autobuild(0, 0, res->arm_dist,
+ level + res->nb_col, res->nb_col,
+ res->arm_dist, 0);
+ while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
+
+
+ if (push) {
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
+ time_wait_ms(500);
+ trajectory_d_rel(&mainboard.traj, 100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+ else if (level == 1 || level == 0) {
+ trajectory_d_rel(&mainboard.traj, -100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple(level);
+ time_wait_ms(400);
+ strat_set_speed(200, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 120);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+ i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+
+ trajectory_a_rel(&mainboard.traj, res->a3);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ if (debug)
+ c = cmdline_getchar_wait();
+ /* go back, insert colums */
+
+ trajectory_d_rel(&mainboard.traj, -100);
+
+ return;
+}
+
+prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push";
+parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0);
+parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8);
+
+prog_char help_column_test[] = "Column_Test function (level)";
+parse_pgm_inst_t cmd_column_test = {
+ .f = cmd_column_test_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_column_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_column_test_arg0,
+ (prog_void *)&cmd_column_test_arg1,
+ NULL,
+ },
+};
+
+parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16);
+parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8);
+parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8);
+parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8);
+parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16);
+parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8);
+
+prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)";
+parse_pgm_inst_t cmd_column_test2 = {
+ .f = cmd_column_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_column_test2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_column_test_arg0,
+ (prog_void *)&cmd_column_test_arg1,
+ (prog_void *)&cmd_column_test_arg2,
+ (prog_void *)&cmd_column_test_arg3,
+ (prog_void *)&cmd_column_test_arg4,
+ (prog_void *)&cmd_column_test_arg5,
+ (prog_void *)&cmd_column_test_arg6,
+ (prog_void *)&cmd_column_test_arg7,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Pickup_Test */
+
+/* this structure is filled when cmd_pickup_test is parsed successfully */
+struct cmd_pickup_test_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t dist;
+};
+
+/* return red or green sensor */
+#define COLOR_IR_SENSOR() \
+ ({ \
+ uint8_t __ret = 0; \
+ if (side == I2C_RIGHT_SIDE) \
+ __ret = sensor_get(S_DISP_RIGHT); \
+ else \
+ __ret = sensor_get(S_DISP_LEFT); \
+ __ret; \
+ }) \
+/* column dispensers */
+#define COL_SCAN_MARGIN 200
+/* distance between the wheel axis and the IR sensor */
+
+/* function called when cmd_pickup_test is parsed successfully */
+static void cmd_pickup_test_parsed(void *parsed_result, void *data)
+{
+ uint8_t err, side, first_try = 1;
+ int8_t cols_count_before, cols_count_after, cols;
+ struct cmd_pickup_test_result *res = parsed_result;
+ int16_t pos1, pos2, pos;
+ microseconds us;
+ int16_t dist = res->dist;
+ uint8_t timeout = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else
+ side = I2C_RIGHT_SIDE;
+
+ i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
+ cols_count_before = get_column_count();
+ position_set(&mainboard.pos, 0, 0, 0);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -1000);
+ err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
+ if (err) /* we should not reach end */
+ goto fail;
+ pos1 = position_get_x_s16(&mainboard.pos);
+ printf_P(PSTR("pos1 = %d\r\n"), pos1);
+
+ err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
+ if (err)
+ goto fail;
+ pos2 = position_get_x_s16(&mainboard.pos);
+ printf_P(PSTR("pos2 = %d\r\n"), pos2);
+
+ pos = ABS(pos1 - pos2);
+ printf_P(PSTR("pos = %d\r\n"), pos);
+
+ trajectory_d_rel(&mainboard.traj, -dist + pos/2);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ if (side == I2C_LEFT_SIDE)
+ trajectory_a_rel(&mainboard.traj, 90);
+ else
+ trajectory_a_rel(&mainboard.traj, -90);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ pickup_wheels_on();
+ retry:
+ if (first_try)
+ i2c_mechboard_mode_lazy_harvest();
+ else
+ i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
+ first_try = 0;
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, 300);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
+
+ trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ position_set(&mainboard.pos, 0, 0, 0);
+ if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
+ strat_eject_col(90, 0);
+ goto retry;
+ }
+
+ /* start to pickup with finger / arms */
+
+ printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__);
+ i2c_mechboard_mode_pickup();
+ WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
+ I2C_MECHBOARD_MODE_PICKUP, 100);
+ us = time_get_us2();
+ cols = get_column_count();
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) {
+ if (get_column_count() != cols) {
+ cols = get_column_count();
+ us = time_get_us2();
+ }
+ if ((get_column_count() - cols_count_before) >= 4) {
+ printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__);
+ break;
+ }
+ /* 1 second timeout */
+ if (time_get_us2() - us > 1500000L) {
+ printf_P(PSTR("%s timeout\r\n"), __FUNCTION__);
+ timeout = 1;
+ break;
+ }
+ }
+
+ /* eject if we found a bad color column */
+
+ if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
+ strat_eject_col(90, 0);
+ goto retry;
+ }
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -250);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
+
+ cols_count_after = get_column_count();
+ cols = cols_count_after - cols_count_before;
+ DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
+
+ pickup_wheels_off();
+ i2c_mechboard_mode_clear();
+
+ wait_ms(1000);
+ return;
+ fail:
+ printf_P(PSTR("failed\r\n"));
+ strat_hardstop();
+}
+
+prog_char str_pickup_test_arg0[] = "pickup_test";
+parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0);
+prog_char str_pickup_test_arg1[] = "left#right";
+parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1);
+parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16);
+
+prog_char help_pickup_test[] = "Pickup_Test function";
+parse_pgm_inst_t cmd_pickup_test = {
+ .f = cmd_pickup_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pickup_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pickup_test_arg0,
+ (prog_void *)&cmd_pickup_test_arg1,
+ (prog_void *)&cmd_pickup_test_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Lintel_Test */
+
+/* this structure is filled when cmd_lintel_test is parsed successfully */
+struct cmd_lintel_test_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_lintel_test is parsed successfully */
+static void cmd_lintel_test_parsed(void *parsed_result, void *data)
+{
+ uint8_t err, first_try = 1, right_ok, left_ok;
+ int16_t left_cur, right_cur;
+
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(500);
+ retry:
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, 500);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err) && err != END_BLOCKING)
+ goto fail;
+
+ i2c_mechboard_mode_get_lintel();
+ time_wait_ms(500);
+
+ left_cur = sensor_get_adc(ADC_CSENSE3);
+ left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
+ right_cur = mechboard.pump_right1_current;
+ right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
+
+ printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"),
+ left_ok, left_cur, right_ok, right_cur);
+ if (first_try) {
+ if (!right_ok && !left_ok) {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ }
+ else if (right_ok && !left_ok) {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ strat_set_speed(500, 500);
+ trajectory_d_a_rel(&mainboard.traj, -150, 30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ trajectory_d_a_rel(&mainboard.traj, -140, -30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ first_try = 0;
+ goto retry;
+ }
+ else if (!right_ok && left_ok) {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ strat_set_speed(500, 500);
+ trajectory_d_a_rel(&mainboard.traj, -150, -30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ trajectory_d_a_rel(&mainboard.traj, -140, 30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ first_try = 0;
+ goto retry;
+ }
+ /* else, lintel is ok */
+ else {
+ i2c_mechboard_mode_put_lintel();
+ }
+ }
+ else {
+ if (right_ok && left_ok) {
+ /* lintel is ok */
+ i2c_mechboard_mode_put_lintel();
+ }
+ else {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ }
+ }
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -250);
+ err = wait_traj_end(TRAJ_FLAGS_STD);
+ return;
+
+fail:
+ printf_P(PSTR("fail\r\n"));
+ return;
+}
+
+prog_char str_lintel_test_arg0[] = "lintel_test";
+parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0);
+
+prog_char help_lintel_test[] = "Lintel_Test function";
+parse_pgm_inst_t cmd_lintel_test = {
+ .f = cmd_lintel_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_lintel_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_lintel_test_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Scan_Test */
+
+/* this structure is filled when cmd_scan_test is parsed successfully */
+struct cmd_scan_test_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t start_dist;
+ int16_t scan_dist;
+ int16_t scan_speed;
+ int16_t center_x;
+ int16_t center_y;
+ uint8_t level;
+};
+
+#define SCAN_MODE_CHECK_TEMPLE 0
+#define SCAN_MODE_SCAN_COL 1
+#define SCAN_MODE_SCAN_TEMPLE 2
+#define SCAN_MODE_TRAJ_ONLY 3
+
+/* function called when cmd_scan_test is parsed successfully */
+static void cmd_scan_test_parsed(void *parsed_result, void *data)
+{
+ uint8_t err, mode=0, c;
+ int16_t pos1x, pos1y, dist;
+ struct cmd_scan_test_result *res = parsed_result;
+ int16_t back_mm = 0;
+
+ int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
+
+ double center_abs_x, center_abs_y;
+ double ckpt_rel_d, ckpt_rel_a;
+ double ckpt_abs_x, ckpt_abs_y;
+
+ if (!strcmp_P(res->arg1, PSTR("traj_only")))
+ mode = SCAN_MODE_TRAJ_ONLY;
+ else if (!strcmp_P(res->arg1, PSTR("check_temple")))
+ mode = SCAN_MODE_CHECK_TEMPLE;
+ else if (!strcmp_P(res->arg1, PSTR("scan_col")))
+ mode = SCAN_MODE_SCAN_COL;
+ else if (!strcmp_P(res->arg1, PSTR("scan_temple")))
+ mode = SCAN_MODE_SCAN_TEMPLE;
+
+ /* go to disc */
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
+ trajectory_d_rel(&mainboard.traj, 400);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (err != END_BLOCKING)
+ return;
+
+ /* save absolute position of disc */
+ rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y);
+
+ /* go back and prepare to scan */
+ strat_set_speed(1000, 1000);
+ trajectory_d_a_rel(&mainboard.traj, -140, 130);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ return;
+
+ /* prepare scanner arm */
+ if (mode != SCAN_MODE_TRAJ_ONLY)
+ i2c_sensorboard_scanner_prepare();
+ time_wait_ms(250);
+
+ strat_set_speed(res->scan_speed, 1000);
+
+ pos1x = position_get_x_s16(&mainboard.pos);
+ pos1y = position_get_y_s16(&mainboard.pos);
+ trajectory_d_rel(&mainboard.traj, -res->scan_dist);
+
+ while (1) {
+ err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (err != 0)
+ break;
+
+ dist = distance_from_robot(pos1x, pos1y);
+
+ if (dist > res->start_dist)
+ break;
+
+ if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
+ err = END_ERROR;
+ break;
+ }
+ }
+
+ if (err) {
+ if (TRAJ_SUCCESS(err))
+ err = END_ERROR; /* should not reach end */
+ strat_hardstop();
+ trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (mode != SCAN_MODE_TRAJ_ONLY)
+ i2c_sensorboard_scanner_stop();
+ return;
+ }
+
+ /* start the scanner */
+
+ if (mode != SCAN_MODE_TRAJ_ONLY)
+ i2c_sensorboard_scanner_start();
+
+ err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
+ TRAJ_FLAGS_NO_NEAR);
+ if (err == 0)
+ err = END_ERROR;
+ if (!TRAJ_SUCCESS(err)) {
+ strat_hardstop();
+ trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (mode != SCAN_MODE_TRAJ_ONLY)
+ i2c_sensorboard_scanner_stop();
+ return;
+ }
+
+ if (mode == SCAN_MODE_TRAJ_ONLY)
+ return;
+
+ wait_scan_done(10000);
+
+ i2c_sensorboard_scanner_stop();
+
+ if (mode == SCAN_MODE_CHECK_TEMPLE) {
+ i2c_sensorboard_scanner_algo_check(res->level,
+ res->center_x, res->center_y);
+ i2cproto_wait_update();
+ wait_scan_done(10000);
+ scanner_dump_state();
+
+ if (sensorboard.dropzone_h == -1) {
+ printf_P(PSTR("-- try to build a temple\r\n"));
+ res->center_x = 15;
+ res->center_y = 13;
+ mode = SCAN_MODE_SCAN_TEMPLE;
+ }
+ }
+
+ if (mode == SCAN_MODE_SCAN_TEMPLE) {
+ i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
+ res->center_x,
+ res->center_y);
+ i2cproto_wait_update();
+ wait_scan_done(10000);
+ scanner_dump_state();
+
+ if (sensorboard.dropzone_h == -1 ||
+ strat_scan_get_checkpoint(mode, &ckpt_rel_x,
+ &ckpt_rel_y, &back_mm)) {
+ printf_P(PSTR("-- try to build a column\r\n"));
+ mode = SCAN_MODE_SCAN_COL;
+ }
+ }
+
+ if (mode == SCAN_MODE_SCAN_COL) {
+ i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
+ res->center_x, res->center_y);
+ i2cproto_wait_update();
+ wait_scan_done(10000);
+ scanner_dump_state();
+
+ if (sensorboard.dropzone_h == -1 ||
+ strat_scan_get_checkpoint(mode, &ckpt_rel_x,
+ &ckpt_rel_y, &back_mm)) {
+ return;
+ }
+ }
+
+ if (sensorboard.dropzone_h == -1)
+ return;
+
+ if (mode == SCAN_MODE_CHECK_TEMPLE) {
+ ckpt_rel_x = 220;
+ ckpt_rel_y = 100;
+ }
+
+
+ printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y);
+
+ rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
+ abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
+
+ printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y);
+
+ printf_P(PSTR("ok ? (y/n)\r\n"));
+
+ c = cmdline_getchar_wait();
+
+ if (c != 'y')
+ return;
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ /* intermediate checkpoint for some positions */
+ if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
+ trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ return;
+ }
+
+ trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ return;
+
+ trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ return;
+
+ c = cmdline_getchar_wait();
+
+ pos1x = position_get_x_s16(&mainboard.pos);
+ pos1y = position_get_y_s16(&mainboard.pos);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 200);
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
+ TRAJ_FLAGS_SMALL_DIST);
+ if (err == 0) {
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
+ trajectory_d_rel(&mainboard.traj, 400);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+ if (err != END_BLOCKING)
+ return;
+
+ if (back_mm) {
+ trajectory_d_rel(&mainboard.traj, -back_mm);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+}
+
+prog_char str_scan_test_arg0[] = "scan_test";
+parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0);
+prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple";
+parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1);
+parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16);
+parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16);
+parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16);
+parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16);
+parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16);
+
+prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)";
+parse_pgm_inst_t cmd_scan_test = {
+ .f = cmd_scan_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_scan_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_scan_test_arg0,
+ (prog_void *)&cmd_scan_test_arg1,
+ (prog_void *)&cmd_scan_test_arg2,
+ (prog_void *)&cmd_scan_test_arg3,
+ (prog_void *)&cmd_scan_test_arg4,
+ (prog_void *)&cmd_scan_test_arg5,
+ (prog_void *)&cmd_scan_test_arg6,
+ NULL,
+ },
+};
+
+prog_char str_scan_test_arg1b[] = "check_temple";
+parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b);
+parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8);
+
+prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)";
+parse_pgm_inst_t cmd_scan_test2 = {
+ .f = cmd_scan_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_scan_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_scan_test_arg0,
+ (prog_void *)&cmd_scan_test_arg1b,
+ (prog_void *)&cmd_scan_test_arg2,
+ (prog_void *)&cmd_scan_test_arg3,
+ (prog_void *)&cmd_scan_test_arg4,
+ (prog_void *)&cmd_scan_test_arg5,
+ (prog_void *)&cmd_scan_test_arg6,
+ (prog_void *)&cmd_scan_test_arg7,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Time_Monitor */
+
+/* this structure is filled when cmd_time_monitor is parsed successfully */
+struct cmd_time_monitor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_time_monitor is parsed successfully */
+static void cmd_time_monitor_parsed(void *parsed_result, void *data)
+{
+ struct cmd_time_monitor_result *res = parsed_result;
+ uint16_t seconds;
+
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
+ }
+ seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
+ printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
+}
+
+prog_char str_time_monitor_arg0[] = "time_monitor";
+parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
+prog_char str_time_monitor_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
+
+prog_char help_time_monitor[] = "Show since how long we are running";
+parse_pgm_inst_t cmd_time_monitor = {
+ .f = cmd_time_monitor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_time_monitor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_time_monitor_arg0,
+ (prog_void *)&cmd_time_monitor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Scanner */
+
+/* this structure is filled when cmd_scanner is parsed successfully */
+struct cmd_scanner_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_scanner is parsed successfully */
+static void cmd_scanner_parsed(void *parsed_result, void *data)
+{
+ struct cmd_scanner_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("prepare"))) {
+ i2c_sensorboard_scanner_prepare();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("stop"))) {
+ i2c_sensorboard_scanner_stop();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("start"))) {
+ i2c_sensorboard_scanner_start();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("algo_col"))) {
+ i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
+ 15, 15);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("algo_check"))) {
+ i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX
+ }
+ else if (!strcmp_P(res->arg1, PSTR("calib"))) {
+ i2c_sensorboard_scanner_calib();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("show"))) {
+ scanner_dump_state();
+ }
+}
+
+prog_char str_scanner_arg0[] = "scanner";
+parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0);
+prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib";
+parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1);
+
+prog_char help_scanner[] = "send commands to scanner";
+parse_pgm_inst_t cmd_scanner = {
+ .f = cmd_scanner_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_scanner,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_scanner_arg0,
+ (prog_void *)&cmd_scanner_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Build_Z1 */
+
+/* this structure is filled when cmd_build_z1 is parsed successfully */
+struct cmd_build_z1_result {
+ fixed_string_t arg0;
+ uint8_t level;
+ int16_t d1;
+ int16_t d2;
+ int16_t d3;
+};
+
+/* function called when cmd_build_z1 is parsed successfully */
+static void cmd_build_z1_parsed(void *parsed_result, void *data)
+{
+ struct cmd_build_z1_result *res = parsed_result;
+
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 400);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ trajectory_d_rel(&mainboard.traj, -200);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
+ I2C_MECHBOARD_MODE_HARVEST);
+
+ while (get_column_count() != 4);
+
+ i2c_mechboard_mode_prepare_build_both(res->level);
+ time_wait_ms(500);
+
+ trajectory_d_rel(&mainboard.traj, 400);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, -res->d1);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
+ res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
+ 1);
+ WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
+ I2C_MECHBOARD_MODE_AUTOBUILD, 100);
+ WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
+ I2C_MECHBOARD_MODE_AUTOBUILD, 10000);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, -res->d2);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple(1);
+ time_wait_ms(400);
+ strat_set_speed(200, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, res->d3);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+}
+
+prog_char str_build_z1_arg0[] = "build_z1";
+parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0);
+parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8);
+parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16);
+parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16);
+parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16);
+
+prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)";
+parse_pgm_inst_t cmd_build_z1 = {
+ .f = cmd_build_z1_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_build_z1,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_build_z1_arg0,
+ (prog_void *)&cmd_build_z1_arg1,
+ (prog_void *)&cmd_build_z1_arg2,
+ (prog_void *)&cmd_build_z1_arg3,
+ (prog_void *)&cmd_build_z1_arg4,
+ NULL,
+ },
+};
+
+#ifdef TEST_BEACON
+/**********************************************************/
+/* Beacon_Opp_Dump */
+
+/* this structure is filled when cmd_beacon_opp_dump is parsed successfully */
+struct cmd_beacon_opp_dump_result {
+ fixed_string_t arg0;
+};
+
+void beacon_dump_samples(void);
+
+/* function called when cmd_beacon_opp_dump is parsed successfully */
+static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data)
+{
+ beacon_dump_samples();
+}
+
+prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump";
+parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0);
+
+prog_char help_beacon_opp_dump[] = "Dump beacon samples";
+parse_pgm_inst_t cmd_beacon_opp_dump = {
+ .f = cmd_beacon_opp_dump_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_beacon_opp_dump,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_beacon_opp_dump_arg0,
+ NULL,
+ },
+};
+#endif
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+ fixed_string_t arg0;
+ int32_t radius;
+};
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+ double radius_mm,
+ double *speed_d,
+ double *speed_a);
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(void *parsed_result, void *data)
+{
+ struct cmd_test_result *res = parsed_result;
+ double d,a;
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
+ printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
+}
+
+prog_char str_test_arg0[] = "test";
+parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
+parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
+
+prog_char help_test[] = "Test function";
+parse_pgm_inst_t cmd_test = {
+ .f = cmd_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_test_arg0,
+ (prog_void *)&cmd_test_arg1,
+ NULL,
+ },
+};
--- /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_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+#include <encoders_spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cs.h"
+#include "cmdline.h"
+#include "strat_utils.h"
+#include "strat_base.h"
+#include "strat_avoid.h"
+#include "strat.h"
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
+
+/**********************************************************/
+/* Traj_Speeds for trajectory_manager */
+
+/* this structure is filled when cmd_traj_speed is parsed successfully */
+struct cmd_traj_speed_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint16_t s;
+};
+
+/* function called when cmd_traj_speed is parsed successfully */
+static void cmd_traj_speed_parsed(void *parsed_result, void *data)
+{
+ struct cmd_traj_speed_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("angle"))) {
+ trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+ trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
+ }
+ /* else it is a "show" */
+
+ printf_P(PSTR("angle %u, distance %u\r\n"),
+ mainboard.traj.a_speed,
+ mainboard.traj.d_speed);
+}
+
+prog_char str_traj_speed_arg0[] = "traj_speed";
+parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
+prog_char str_traj_speed_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
+parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
+
+prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
+parse_pgm_inst_t cmd_traj_speed = {
+ .f = cmd_traj_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_traj_speed,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_traj_speed_arg0,
+ (prog_void *)&cmd_traj_speed_arg1,
+ (prog_void *)&cmd_traj_speed_s,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_traj_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
+
+prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
+parse_pgm_inst_t cmd_traj_speed_show = {
+ .f = cmd_traj_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_traj_speed_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_traj_speed_arg0,
+ (prog_void *)&cmd_traj_speed_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* trajectory window configuration */
+
+/* this structure is filled when cmd_trajectory is parsed successfully */
+struct cmd_trajectory_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float d_win;
+ float a_win;
+ float a_start;
+};
+
+
+/* function called when cmd_trajectory is parsed successfully */
+static void cmd_trajectory_parsed(void * parsed_result, void * data)
+{
+ struct cmd_trajectory_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ trajectory_set_windows(&mainboard.traj, res->d_win,
+ res->a_win, res->a_start);
+ }
+
+ printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
+ DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
+}
+
+prog_char str_trajectory_arg0[] = "trajectory";
+parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
+prog_char str_trajectory_arg1[] = "set";
+parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
+parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
+parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
+parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
+
+prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
+parse_pgm_inst_t cmd_trajectory = {
+ .f = cmd_trajectory_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_trajectory,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_trajectory_arg0,
+ (prog_void *)&cmd_trajectory_arg1,
+ (prog_void *)&cmd_trajectory_d,
+ (prog_void *)&cmd_trajectory_a,
+ (prog_void *)&cmd_trajectory_as,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_trajectory_show_arg[] = "show";
+parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
+
+prog_char help_trajectory_show[] = "Show trajectory window configuration";
+parse_pgm_inst_t cmd_trajectory_show = {
+ .f = cmd_trajectory_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_trajectory_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_trajectory_arg0,
+ (prog_void *)&cmd_trajectory_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* rs_gains configuration */
+
+/* this structure is filled when cmd_rs_gains is parsed successfully */
+struct cmd_rs_gains_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float left;
+ float right;
+};
+
+/* function called when cmd_rs_gains is parsed successfully */
+static void cmd_rs_gains_parsed(void * parsed_result, void * data)
+{
+ struct cmd_rs_gains_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
+ rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
+ }
+ printf_P(PSTR("rs_gains set "));
+ f64_print(mainboard.rs.left_ext_gain);
+ printf_P(PSTR(" "));
+ f64_print(mainboard.rs.right_ext_gain);
+ printf_P(PSTR("\r\n"));
+}
+
+prog_char str_rs_gains_arg0[] = "rs_gains";
+parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
+prog_char str_rs_gains_arg1[] = "set";
+parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
+parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
+parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
+
+prog_char help_rs_gains[] = "Set rs_gains (left, right)";
+parse_pgm_inst_t cmd_rs_gains = {
+ .f = cmd_rs_gains_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs_gains,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_arg1,
+ (prog_void *)&cmd_rs_gains_l,
+ (prog_void *)&cmd_rs_gains_r,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_rs_gains_show_arg[] = "show";
+parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
+
+prog_char help_rs_gains_show[] = "Show rs_gains";
+parse_pgm_inst_t cmd_rs_gains_show = {
+ .f = cmd_rs_gains_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs_gains_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* track configuration */
+
+/* this structure is filled when cmd_track is parsed successfully */
+struct cmd_track_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float val;
+};
+
+/* function called when cmd_track is parsed successfully */
+static void cmd_track_parsed(void * parsed_result, void * data)
+{
+ struct cmd_track_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
+ }
+ printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
+}
+
+prog_char str_track_arg0[] = "track";
+parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
+prog_char str_track_arg1[] = "set";
+parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
+parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
+
+prog_char help_track[] = "Set track in mm";
+parse_pgm_inst_t cmd_track = {
+ .f = cmd_track_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_track,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_track_arg0,
+ (prog_void *)&cmd_track_arg1,
+ (prog_void *)&cmd_track_val,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_track_show_arg[] = "show";
+parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
+
+prog_char help_track_show[] = "Show track";
+parse_pgm_inst_t cmd_track_show = {
+ .f = cmd_track_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_track_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_track_arg0,
+ (prog_void *)&cmd_track_show_arg,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* Pt_Lists for testing traj */
+
+#define PT_LIST_SIZE 10
+static struct xy_point pt_list[PT_LIST_SIZE];
+static uint16_t pt_list_len = 0;
+
+/* this structure is filled when cmd_pt_list is parsed successfully */
+struct cmd_pt_list_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint16_t arg2;
+ int16_t arg3;
+ int16_t arg4;
+};
+
+/* function called when cmd_pt_list is parsed successfully */
+static void cmd_pt_list_parsed(void * parsed_result, void * data)
+{
+ struct cmd_pt_list_result * res = parsed_result;
+ uint8_t i, why=0;
+
+ if (!strcmp_P(res->arg1, PSTR("append"))) {
+ res->arg2 = pt_list_len;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("insert")) ||
+ !strcmp_P(res->arg1, PSTR("append"))) {
+ if (res->arg2 > pt_list_len) {
+ printf_P(PSTR("Index too large\r\n"));
+ return;
+ }
+ if (pt_list_len >= PT_LIST_SIZE) {
+ printf_P(PSTR("List is too large\r\n"));
+ return;
+ }
+ memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
+ PT_LIST_SIZE-1-res->arg2);
+ pt_list[res->arg2].x = res->arg3;
+ pt_list[res->arg2].y = res->arg4;
+ pt_list_len++;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("del"))) {
+ if (pt_list_len <= 0) {
+ printf_P(PSTR("Error: list empty\r\n"));
+ return;
+ }
+ if (res->arg2 > pt_list_len) {
+ printf_P(PSTR("Index too large\r\n"));
+ return;
+ }
+ memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
+ (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
+ pt_list_len--;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ pt_list_len = 0;
+ }
+
+ /* else it is a "show" or a "start" */
+ if (pt_list_len == 0) {
+ printf_P(PSTR("List empty\r\n"));
+ return;
+ }
+ for (i=0 ; i<pt_list_len ; i++) {
+ printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
+ if (!strcmp_P(res->arg1, PSTR("start"))) {
+ trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
+ why = wait_traj_end(0xFF); /* all */
+ }
+ else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
+ while (1) {
+ why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
+ printf("next point\r\n");
+ if (why != END_OBSTACLE)
+ break;
+ }
+ }
+ if (why & (~(END_TRAJ | END_NEAR)))
+ trajectory_stop(&mainboard.traj);
+ }
+}
+
+prog_char str_pt_list_arg0[] = "pt_list";
+parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
+prog_char str_pt_list_arg1[] = "insert";
+parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
+parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
+parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
+parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
+
+prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
+parse_pgm_inst_t cmd_pt_list = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_arg1,
+ (prog_void *)&cmd_pt_list_arg2,
+ (prog_void *)&cmd_pt_list_arg3,
+ (prog_void *)&cmd_pt_list_arg4,
+ NULL,
+ },
+};
+
+/* append */
+
+prog_char str_pt_list_arg1_append[] = "append";
+parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
+
+prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
+parse_pgm_inst_t cmd_pt_list_append = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list_append,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_arg1_append,
+ (prog_void *)&cmd_pt_list_arg3,
+ (prog_void *)&cmd_pt_list_arg4,
+ NULL,
+ },
+};
+
+/* del */
+
+prog_char str_pt_list_del_arg[] = "del";
+parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
+
+prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
+parse_pgm_inst_t cmd_pt_list_del = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list_del,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_del_arg,
+ (prog_void *)&cmd_pt_list_arg2,
+ NULL,
+ },
+};
+/* show */
+
+prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
+parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
+
+prog_char help_pt_list_show[] = "Show, start or reset pt_list";
+parse_pgm_inst_t cmd_pt_list_show = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_show_arg,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* Goto function */
+
+/* this structure is filled when cmd_goto is parsed successfully */
+struct cmd_goto_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+ int32_t arg4;
+};
+
+/* function called when cmd_goto is parsed successfully */
+static void cmd_goto_parsed(void * parsed_result, void * data)
+{
+ struct cmd_goto_result * res = parsed_result;
+ uint8_t err;
+ microseconds t1, t2;
+
+ interrupt_traj_reset();
+ if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
+ trajectory_a_rel(&mainboard.traj, res->arg2);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
+ trajectory_d_rel(&mainboard.traj, res->arg2);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
+ trajectory_a_abs(&mainboard.traj, res->arg2);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
+ trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
+ trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
+ trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
+ trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
+ err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
+ if (err != END_TRAJ && err != END_NEAR)
+ strat_hardstop();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
+ err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
+ if (err != END_TRAJ && err != END_NEAR)
+ strat_hardstop();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
+ trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
+ trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
+ trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
+ }
+ t1 = time_get_us2();
+ while ((err = test_traj_end(0xFF)) == 0) {
+ t2 = time_get_us2();
+ if (t2 - t1 > 200000) {
+ dump_cs_debug("angle", &mainboard.angle.cs);
+ dump_cs_debug("distance", &mainboard.distance.cs);
+ t1 = t2;
+ }
+ }
+ if (err != END_TRAJ && err != END_NEAR)
+ strat_hardstop();
+ printf_P(PSTR("returned %s\r\n"), get_err(err));
+}
+
+prog_char str_goto_arg0[] = "goto";
+parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
+prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
+parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
+parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
+
+/* 1 params */
+prog_char help_goto1[] = "Change orientation of the mainboard";
+parse_pgm_inst_t cmd_goto1 = {
+ .f = cmd_goto_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_goto1,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_goto_arg0,
+ (prog_void *)&cmd_goto_arg1_a,
+ (prog_void *)&cmd_goto_arg2,
+ NULL,
+ },
+};
+
+prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy";
+parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
+parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
+
+/* 2 params */
+prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
+parse_pgm_inst_t cmd_goto2 = {
+ .f = cmd_goto_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_goto2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_goto_arg0,
+ (prog_void *)&cmd_goto_arg1_b,
+ (prog_void *)&cmd_goto_arg2,
+ (prog_void *)&cmd_goto_arg3,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Position tests */
+
+/* this structure is filled when cmd_position is parsed successfully */
+struct cmd_position_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+ int32_t arg4;
+};
+
+#define AUTOPOS_SPEED_FAST 200
+static void auto_position(void)
+{
+ uint8_t err;
+ uint16_t old_spdd, old_spda;
+
+ interrupt_traj_reset();
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
+
+ trajectory_d_rel(&mainboard.traj, -300);
+ err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+ strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
+
+ trajectory_d_rel(&mainboard.traj, 120);
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+
+ trajectory_a_rel(&mainboard.traj, COLOR_A(90));
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+
+ trajectory_d_rel(&mainboard.traj, -300);
+ err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+ strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
+ COLOR_A(90));
+
+ trajectory_d_rel(&mainboard.traj, 120);
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+
+ trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+
+ strat_set_speed(old_spdd, old_spda);
+ return;
+
+intr:
+ strat_hardstop();
+ strat_set_speed(old_spdd, old_spda);
+}
+
+/* function called when cmd_position is parsed successfully */
+static void cmd_position_parsed(void * parsed_result, void * data)
+{
+ struct cmd_position_result * res = parsed_result;
+
+ /* display raw position values */
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ position_set(&mainboard.pos, 0, 0, 0);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("set"))) {
+ position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
+ mainboard.our_color = I2C_COLOR_GREEN;
+ i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
+ i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
+ auto_position();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
+ mainboard.our_color = I2C_COLOR_RED;
+ i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
+ i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
+ auto_position();
+ }
+
+ /* else it's just a "show" */
+ printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
+ position_get_x_double(&mainboard.pos),
+ position_get_y_double(&mainboard.pos),
+ DEG(position_get_a_rad_double(&mainboard.pos)));
+}
+
+prog_char str_position_arg0[] = "position";
+parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
+prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
+parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
+
+prog_char help_position[] = "Show/reset (x,y,a) position";
+parse_pgm_inst_t cmd_position = {
+ .f = cmd_position_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_position,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_position_arg0,
+ (prog_void *)&cmd_position_arg1,
+ NULL,
+ },
+};
+
+
+prog_char str_position_arg1_set[] = "set";
+parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
+parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
+parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
+parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
+
+prog_char help_position_set[] = "Set (x,y,a) position";
+parse_pgm_inst_t cmd_position_set = {
+ .f = cmd_position_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_position_set,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_position_arg0,
+ (prog_void *)&cmd_position_arg1_set,
+ (prog_void *)&cmd_position_arg2,
+ (prog_void *)&cmd_position_arg3,
+ (prog_void *)&cmd_position_arg4,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_infos is parsed successfully */
+struct cmd_strat_infos_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_infos is parsed successfully */
+static void cmd_strat_infos_parsed(void *parsed_result, void *data)
+{
+ struct cmd_strat_infos_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ strat_reset_infos();
+ }
+ strat_infos.dump_enabled = 1;
+ strat_dump_infos(__FUNCTION__);
+}
+
+prog_char str_strat_infos_arg0[] = "strat_infos";
+parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
+prog_char str_strat_infos_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
+
+prog_char help_strat_infos[] = "reset/show strat_infos";
+parse_pgm_inst_t cmd_strat_infos = {
+ .f = cmd_strat_infos_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_infos,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_infos_arg0,
+ (prog_void *)&cmd_strat_infos_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf is parsed successfully */
+struct cmd_strat_conf_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_conf is parsed successfully */
+static void cmd_strat_conf_parsed(void *parsed_result, void *data)
+{
+ struct cmd_strat_conf_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("base"))) {
+ strat_infos.conf.flags = 0;
+ strat_infos.conf.scan_our_min_time = 90;
+ strat_infos.conf.delay_between_our_scan = 90;
+ strat_infos.conf.scan_opp_min_time = 90;
+ strat_infos.conf.delay_between_opp_scan = 90;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("big3"))) {
+ strat_infos.conf.flags =
+ STRAT_CONF_STORE_STATIC2 |
+ STRAT_CONF_BIG_3_TEMPLE;
+ strat_infos.conf.scan_our_min_time = 90;
+ strat_infos.conf.delay_between_our_scan = 90;
+ strat_infos.conf.scan_opp_min_time = 90;
+ strat_infos.conf.delay_between_opp_scan = 90;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
+ strat_infos.conf.flags = 0;
+ strat_infos.conf.scan_our_min_time = 35;
+ strat_infos.conf.delay_between_our_scan = 90;
+ strat_infos.conf.scan_opp_min_time = 90;
+ strat_infos.conf.delay_between_opp_scan = 90;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
+ strat_infos.conf.flags =
+ STRAT_CONF_STORE_STATIC2 |
+ STRAT_CONF_BIG_3_TEMPLE;
+ strat_infos.conf.scan_our_min_time = 35;
+ strat_infos.conf.delay_between_our_scan = 90;
+ strat_infos.conf.scan_opp_min_time = 90;
+ strat_infos.conf.delay_between_opp_scan = 90;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
+ strat_infos.conf.flags =
+ STRAT_CONF_TAKE_ONE_LINTEL |
+ STRAT_CONF_STORE_STATIC2 |
+ STRAT_CONF_EARLY_SCAN |
+ STRAT_CONF_PUSH_OPP_COLS;
+ strat_infos.conf.scan_our_min_time = 50;
+ strat_infos.conf.delay_between_our_scan = 90;
+ strat_infos.conf.scan_opp_min_time = 15;
+ strat_infos.conf.delay_between_opp_scan = 90;
+ strat_infos.conf.wait_opponent = 5;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
+ strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
+ strat_infos.conf.scan_our_min_time = 90;
+ strat_infos.conf.delay_between_our_scan = 90;
+ strat_infos.conf.scan_opp_min_time = 30;
+ strat_infos.conf.delay_between_opp_scan = 90;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
+ strat_infos.conf.flags =
+ STRAT_CONF_ONLY_ONE_ON_DISC;
+ strat_infos.conf.scan_our_min_time = 90;
+ strat_infos.conf.delay_between_our_scan = 90;
+ strat_infos.conf.scan_opp_min_time = 90;
+ strat_infos.conf.delay_between_opp_scan = 90;
+ }
+ strat_infos.dump_enabled = 1;
+ strat_dump_conf();
+}
+
+prog_char str_strat_conf_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
+prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
+parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
+
+prog_char help_strat_conf[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf = {
+ .f = cmd_strat_conf_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_conf,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_conf_arg0,
+ (prog_void *)&cmd_strat_conf_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf2 is parsed successfully */
+struct cmd_strat_conf2_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_strat_conf2 is parsed successfully */
+static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
+{
+ struct cmd_strat_conf2_result *res = parsed_result;
+ uint8_t on, bit = 0;
+
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ on = 1;
+ else
+ on = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
+ bit = STRAT_CONF_ONLY_ONE_ON_DISC;
+ else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
+ bit = STRAT_CONF_BYPASS_STATIC2;
+ else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
+ bit = STRAT_CONF_TAKE_ONE_LINTEL;
+ else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
+ bit = STRAT_CONF_TAKE_ONE_LINTEL;
+ else if (!strcmp_P(res->arg1, PSTR("store_static2")))
+ bit = STRAT_CONF_STORE_STATIC2;
+ else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
+ bit = STRAT_CONF_BIG_3_TEMPLE;
+ else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
+ bit = STRAT_CONF_EARLY_SCAN;
+ else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
+ bit = STRAT_CONF_PUSH_OPP_COLS;
+
+ if (on)
+ strat_infos.conf.flags |= bit;
+ else
+ strat_infos.conf.flags &= (~bit);
+
+ strat_infos.dump_enabled = 1;
+ strat_dump_conf();
+}
+
+prog_char str_strat_conf2_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
+prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan";
+parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
+prog_char str_strat_conf2_arg2[] = "on#off";
+parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
+
+
+prog_char help_strat_conf2[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf2 = {
+ .f = cmd_strat_conf2_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_conf2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_conf2_arg0,
+ (prog_void *)&cmd_strat_conf2_arg1,
+ (prog_void *)&cmd_strat_conf2_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf3 is parsed successfully */
+struct cmd_strat_conf3_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+};
+
+/* function called when cmd_strat_conf3 is parsed successfully */
+static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
+{
+ struct cmd_strat_conf3_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
+ if (res->arg2 > 90)
+ res->arg2 = 90;
+ strat_infos.conf.scan_opp_min_time = res->arg2;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
+ if (res->arg2 > 90)
+ res->arg2 = 90;
+ strat_infos.conf.delay_between_opp_scan = res->arg2;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
+ if (res->arg2 > 90)
+ res->arg2 = 90;
+ strat_infos.conf.scan_our_min_time = res->arg2;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
+ if (res->arg2 > 90)
+ res->arg2 = 90;
+ strat_infos.conf.delay_between_our_scan = res->arg2;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
+ strat_infos.conf.wait_opponent = res->arg2;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
+ strat_infos.conf.lintel_min_time = res->arg2;
+ }
+ strat_infos.dump_enabled = 1;
+ strat_dump_conf();
+}
+
+prog_char str_strat_conf3_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
+prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent";
+parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
+parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
+
+prog_char help_strat_conf3[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf3 = {
+ .f = cmd_strat_conf3_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_conf3,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_conf3_arg0,
+ (prog_void *)&cmd_strat_conf3_arg1,
+ (prog_void *)&cmd_strat_conf3_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* strat configuration */
+
+/* this structure is filled when cmd_strat_conf4 is parsed successfully */
+struct cmd_strat_conf4_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t arg2;
+};
+
+/* function called when cmd_strat_conf4 is parsed successfully */
+static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
+{
+ struct cmd_strat_conf4_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
+ strat_infos.conf.scan_opp_angle = res->arg2;
+ }
+ strat_infos.dump_enabled = 1;
+ strat_dump_conf();
+}
+
+prog_char str_strat_conf4_arg0[] = "strat_conf";
+parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
+prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
+parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
+parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
+
+prog_char help_strat_conf4[] = "configure strat options";
+parse_pgm_inst_t cmd_strat_conf4 = {
+ .f = cmd_strat_conf4_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_conf4,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_conf4_arg0,
+ (prog_void *)&cmd_strat_conf4_arg1,
+ (prog_void *)&cmd_strat_conf4_arg2,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Subtraj */
+
+/* this structure is filled when cmd_subtraj is parsed successfully */
+struct cmd_subtraj_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+ int32_t arg4;
+ int32_t arg5;
+};
+
+/* function called when cmd_subtraj is parsed successfully */
+static void cmd_subtraj_parsed(void *parsed_result, void *data)
+{
+ struct cmd_subtraj_result *res = parsed_result;
+ uint8_t err = 0;
+ struct column_dispenser *disp;
+
+ if (strcmp_P(res->arg1, PSTR("static")) == 0) {
+ err = strat_static_columns(res->arg2);
+ }
+ else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
+ strat_infos.s_cols.configuration = res->arg2;
+ switch (res->arg2) {
+ case 1:
+ position_set(&mainboard.pos, 1398,
+ COLOR_Y(1297), COLOR_A(-66));
+ break;
+ case 2:
+ position_set(&mainboard.pos, 1232,
+ COLOR_Y(1051), COLOR_A(4));
+ break;
+ case 3:
+ position_set(&mainboard.pos, 1232,
+ COLOR_Y(1043), COLOR_A(5));
+ break;
+ case 4:
+ position_set(&mainboard.pos, 1346,
+ COLOR_Y(852), COLOR_A(57));
+ break;
+ default:
+ return;
+ }
+ if (res->arg2 == 1 && res->arg3 == 1) {
+ strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
+ }
+ if (res->arg2 == 1 && res->arg3 == 2) {
+ strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
+ }
+ err = strat_static_columns_pass2();
+ }
+ else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
+ err = strat_goto_lintel_disp(&strat_infos.l1);
+ }
+ else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
+ err = strat_goto_lintel_disp(&strat_infos.l2);
+ }
+ else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
+ disp = &strat_infos.c1;
+ err = strat_goto_col_disp(&disp);
+ }
+ else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
+ disp = &strat_infos.c2;
+ err = strat_goto_col_disp(&disp);
+ }
+ else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
+ disp = &strat_infos.c3;
+ err = strat_goto_col_disp(&disp);
+ }
+ else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
+ if (res->arg2 == 0) {
+ printf_P(PSTR("bad level\r\n"));
+ return;
+ }
+ err = strat_goto_disc(res->arg2);
+ }
+
+ printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
+ trajectory_hardstop(&mainboard.traj);
+}
+
+prog_char str_subtraj_arg0[] = "subtraj";
+parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
+prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
+parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
+parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
+parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
+parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
+parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
+
+prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
+parse_pgm_inst_t cmd_subtraj = {
+ .f = cmd_subtraj_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_subtraj,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_subtraj_arg0,
+ (prog_void *)&cmd_subtraj_arg1,
+ (prog_void *)&cmd_subtraj_arg2,
+ (prog_void *)&cmd_subtraj_arg3,
+ (prog_void *)&cmd_subtraj_arg4,
+ (prog_void *)&cmd_subtraj_arg5,
+ NULL,
+ },
+};
--- /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.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "strat.h"
+#include "actuator.h"
+
+/* called every 5 ms */
+static void do_cs(void *dummy)
+{
+ static uint16_t cpt = 0;
+ static int32_t old_a = 0, old_d = 0;
+
+ /* read encoders */
+ if (mainboard.flags & DO_ENCODERS) {
+ encoders_spi_manage(NULL);
+ }
+
+ /* XXX there is an issue which is probably related to avr-libc
+ * 1.6.2 (debian): this code using fixed_point lib does not
+ * work with it */
+ /* robot system, conversion to angle,distance */
+ if (mainboard.flags & DO_RS) {
+ int16_t a,d;
+ rs_update(&mainboard.rs); /* takes about 0.5 ms */
+ /* process and store current speed */
+ a = rs_get_angle(&mainboard.rs);
+ d = rs_get_distance(&mainboard.rs);
+ mainboard.speed_a = a - old_a;
+ mainboard.speed_d = d - old_d;
+ old_a = a;
+ old_d = d;
+ }
+
+ /* control system */
+ if (mainboard.flags & DO_CS) {
+ if (mainboard.angle.on)
+ cs_manage(&mainboard.angle.cs);
+ if (mainboard.distance.on)
+ cs_manage(&mainboard.distance.cs);
+ }
+ if ((cpt & 1) && (mainboard.flags & DO_POS)) {
+ /* about 1.5ms (worst case without centrifugal force
+ * compensation) */
+ position_manage(&mainboard.pos);
+ }
+ if (mainboard.flags & DO_BD) {
+ bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
+ bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
+ }
+ if (mainboard.flags & DO_TIMER) {
+ uint8_t second;
+ /* the robot should stop correctly in the strat, but
+ * in some cases, we must force the stop from an
+ * interrupt */
+ second = time_get_s();
+ if (second >= MATCH_TIME + 2) {
+ pwm_ng_set(LEFT_PWM, 0);
+ pwm_ng_set(RIGHT_PWM, 0);
+ printf_P(PSTR("END OF TIME\r\n"));
+ while(1);
+ }
+ }
+ /* brakes */
+ if (mainboard.flags & DO_POWER)
+ BRAKE_OFF();
+ else
+ BRAKE_ON();
+ cpt++;
+}
+
+void dump_cs_debug(const char *name, struct cs *cs)
+{
+ DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "
+ "in=% .5ld out=% .5ld",
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_cs(const char *name, struct cs *cs)
+{
+ printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
+ "in=% .5ld out=% .5ld\r\n"),
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_pid(const char *name, struct pid_filter *pid)
+{
+ printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ name,
+ pid_get_value_in(pid) * pid_get_gain_P(pid),
+ pid_get_value_I(pid) * pid_get_gain_I(pid),
+ pid_get_value_D(pid) * pid_get_gain_D(pid),
+ pid_get_value_out(pid));
+}
+
+void microb_cs_init(void)
+{
+ /* ROBOT_SYSTEM */
+ rs_init(&mainboard.rs);
+ rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
+ rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM);
+ /* increase gain to decrease dist, increase left and it will turn more left */
+ rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ LEFT_ENCODER, IMP_COEF * 1.0015);
+ rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ RIGHT_ENCODER, IMP_COEF * -1.006);
+ /* rs will use external encoders */
+ rs_set_flags(&mainboard.rs, RS_USE_EXT);
+
+ /* POSITION MANAGER */
+ position_init(&mainboard.pos);
+ position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
+ position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
+ position_set_centrifugal_coef(&mainboard.pos, 0.000016);
+ position_use_ext(&mainboard.pos);
+
+ /* TRAJECTORY MANAGER */
+ trajectory_init(&mainboard.traj);
+ trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
+ &mainboard.angle.cs);
+ trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
+ trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */
+ /* distance window, angle window, angle start */
+ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
+
+ /* ---- CS angle */
+ /* PID */
+ pid_init(&mainboard.angle.pid);
+ pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
+ pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095);
+ pid_set_out_shift(&mainboard.angle.pid, 10);
+ pid_set_derivate_filter(&mainboard.angle.pid, 4);
+
+ /* QUADRAMP */
+ quadramp_init(&mainboard.angle.qr);
+ quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */
+ quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */
+
+ /* CS */
+ cs_init(&mainboard.angle.cs);
+ cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr);
+ cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid);
+ cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs);
+ cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs);
+ cs_set_consign(&mainboard.angle.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&mainboard.angle.bd);
+ bd_set_speed_threshold(&mainboard.angle.bd, 80);
+ bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50);
+
+ /* ---- CS distance */
+ /* PID */
+ pid_init(&mainboard.distance.pid);
+ pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
+ pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
+ pid_set_out_shift(&mainboard.distance.pid, 10);
+ pid_set_derivate_filter(&mainboard.distance.pid, 6);
+
+ /* QUADRAMP */
+ quadramp_init(&mainboard.distance.qr);
+ quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */
+ quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */
+
+ /* CS */
+ cs_init(&mainboard.distance.cs);
+ cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr);
+ cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid);
+ cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs);
+ cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs);
+ cs_set_consign(&mainboard.distance.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&mainboard.distance.bd);
+ bd_set_speed_threshold(&mainboard.distance.bd, 60);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
+
+ /* set them on !! */
+ mainboard.angle.on = 1;
+ mainboard.distance.on = 1;
+
+
+ scheduler_add_periodical_event_priority(do_cs, NULL,
+ 5000L / SCHEDULER_UNIT,
+ CS_PRIO);
+}
--- /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.3 2009-03-29 18:42:41 zer0 Exp $
+ *
+ */
+
+void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_cs_debug(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
--- /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-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _DEBUG_CONFIG_
+#define _DEBUG_CONFIG_ 1.0 // version
+
+
+/** port line definition for the show_int_loop() function */
+/* undefine it to disable this functionnality */
+#define INTERRUPT_SHOW_PORT PORTA
+#define INTERRUPT_SHOW_BIT 3
+
+
+
+/** memory mark for the min_stack_space_available() function
+ the ram is filled with this value after a reset ! */
+#define MARK 0x55
+
+/** the mark is inserted in whole RAM if this is enabled
+ (could lead to problems if you need to hold values through a reset...)
+ so it's better to disable it.
+ stack counting is not affected */
+//#define DIAG_FILL_ENTIRE_RAM
+
+
+#endif //_DEBUG_CONFIG_
--- /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-02-20 21:10:01 zer0 Exp $
+ *
+ */
+#ifndef _ENCODERS_SPI_CONFIG_H_
+#define _ENCODERS_SPI_CONFIG_H_
+
+#define ENCODERS_SPI_NUMBER 4
+#define ENCODERS_SPI_SS_PORT SS_PORT /* PORTB on atmega2560 */
+#define ENCODERS_SPI_SS_BIT SS_BIT /* 0 on atmega2560 */
+
+/* see spi configuration */
+#define ENCODERS_SPI_CLK_RATE SPI_CLK_RATE_16
+#define ENCODERS_SPI_FORMAT SPI_FORMAT_3
+#define ENCODERS_SPI_DATA_ORDER SPI_LSB_FIRST
+
+#endif
--- /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-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _ERROR_CONFIG_
+#define _ERROR_CONFIG_
+
+/** enable the dump of the comment */
+#define ERROR_DUMP_TEXTLOG
+
+/** enable the dump of filename and line number */
+#define ERROR_DUMP_FILE_LINE
+
+#endif
--- /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.2 2009-03-05 23:01:32 zer0 Exp $
+ *
+ */
+
+
+#define I2C_BITRATE 1 // divider dor i2c baudrate, see TWBR in doc
+#define I2C_PRESCALER 3 // prescaler config, rate = 2^(n*2)
+
+/* Size of transmission buffer */
+#define I2C_SEND_BUFFER_SIZE 32
+
+/* Size of reception buffer */
+#define I2C_RECV_BUFFER_SIZE 32
--- /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.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <i2c.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "sensor.h"
+#include "i2c_protocol.h"
+
+#define I2C_STATE_MAX 4
+
+#define I2C_TIMEOUT 100 /* ms */
+#define I2C_MAX_ERRORS 40
+
+static volatile uint8_t i2c_poll_num = 0;
+static volatile uint8_t i2c_state = 0;
+static volatile uint16_t i2c_errors = 0;
+
+#define OP_READY 0 /* no i2c op running */
+#define OP_POLL 1 /* a user command is running */
+#define OP_CMD 2 /* a polling (req / ans) is running */
+
+static volatile uint8_t running_op = OP_READY;
+
+#define I2C_MAX_LOG 3
+static uint8_t error_log = 0;
+
+/* used for commands */
+uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
+volatile int8_t command_dest=-1;
+volatile uint8_t command_size=0;
+
+static int8_t i2c_req_mechboard_status(void);
+static int8_t i2c_req_sensorboard_status(void);
+
+#define I2C_ERROR(args...) do { \
+ if (error_log < I2C_MAX_LOG) { \
+ ERROR(E_USER_I2C_PROTO, args); \
+ error_log ++; \
+ if (error_log == I2C_MAX_LOG) { \
+ ERROR(E_USER_I2C_PROTO, \
+ "i2c logs are now warnings"); \
+ } \
+ } \
+ else \
+ WARNING(E_USER_I2C_PROTO, args); \
+ } while(0)
+
+void i2c_protocol_init(void)
+{
+}
+
+void i2c_protocol_debug(void)
+{
+ printf_P(PSTR("I2C protocol debug infos:\r\n"));
+ printf_P(PSTR(" i2c_state=%d\r\n"), i2c_state);
+ printf_P(PSTR(" i2c_errors=%d\r\n"), i2c_errors);
+ printf_P(PSTR(" running_op=%d\r\n"), running_op);
+ printf_P(PSTR(" command_size=%d\r\n"), command_size);
+ printf_P(PSTR(" command_dest=%d\r\n"), command_dest);
+ printf_P(PSTR(" i2c_status=%x\r\n"), i2c_status());
+}
+
+static void i2cproto_next_state(uint8_t inc)
+{
+ i2c_state += inc;
+ if (i2c_state >= I2C_STATE_MAX) {
+ i2c_state = 0;
+ i2c_poll_num ++;
+ }
+}
+
+void i2cproto_wait_update(void)
+{
+ uint8_t poll_num;
+ poll_num = i2c_poll_num;
+ WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150);
+}
+
+/* called periodically : the goal of this 'thread' is to send requests
+ * and read answers on i2c slaves in the correct order. */
+void i2c_poll_slaves(void *dummy)
+{
+ uint8_t flags;
+ int8_t err;
+ static uint8_t a = 0;
+
+ a++;
+ if (a & 0x4)
+ LED2_TOGGLE();
+
+ /* already running */
+ IRQ_LOCK(flags);
+ if (running_op != OP_READY) {
+ IRQ_UNLOCK(flags);
+ return;
+ }
+
+ /* if a command is ready to be sent, so send it */
+ if (command_size) {
+ running_op = OP_CMD;
+ err = i2c_send(command_dest, command_buf, command_size,
+ I2C_CTRL_GENERIC);
+ if (err < 0)
+ goto error;
+ IRQ_UNLOCK(flags);
+ return;
+ }
+
+ /* no command, so do the polling */
+ running_op = OP_POLL;
+
+ switch(i2c_state) {
+
+ /* poll status of mechboard */
+#define I2C_REQ_MECHBOARD 0
+ case I2C_REQ_MECHBOARD:
+ if ((err = i2c_req_mechboard_status()))
+ goto error;
+ break;
+
+#define I2C_ANS_MECHBOARD 1
+ case I2C_ANS_MECHBOARD:
+ if ((err = i2c_recv(I2C_MECHBOARD_ADDR,
+ sizeof(struct i2c_ans_mechboard_status),
+ I2C_CTRL_GENERIC)))
+ goto error;
+ break;
+
+ /* poll status of sensorboard */
+#define I2C_REQ_SENSORBOARD 2
+ case I2C_REQ_SENSORBOARD:
+ if ((err = i2c_req_sensorboard_status()))
+ goto error;
+ break;
+
+#define I2C_ANS_SENSORBOARD 3
+ case I2C_ANS_SENSORBOARD:
+ if ((err = i2c_recv(I2C_SENSORBOARD_ADDR,
+ sizeof(struct i2c_ans_sensorboard_status),
+ I2C_CTRL_GENERIC)))
+ goto error;
+ break;
+
+ /* nothing, go to the first request */
+ default:
+ i2c_state = 0;
+ running_op = OP_READY;
+ }
+ IRQ_UNLOCK(flags);
+
+ return;
+
+ error:
+ running_op = OP_READY;
+ IRQ_UNLOCK(flags);
+ i2c_errors++;
+ if (i2c_errors > I2C_MAX_ERRORS) {
+ I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
+ "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
+ i2c_reset();
+ i2c_errors = 0;
+ }
+}
+
+/* called when the xmit is finished */
+void i2c_sendevent(int8_t size)
+{
+ if (size > 0) {
+ if (running_op == OP_POLL) {
+ i2cproto_next_state(1);
+ }
+ else
+ command_size = 0;
+ }
+ else {
+ i2c_errors++;
+ NOTICE(E_USER_I2C_PROTO, "send error state=%d size=%d "
+ "op=%d", i2c_state, size, running_op);
+ if (i2c_errors > I2C_MAX_ERRORS) {
+ I2C_ERROR("I2C error, slave not ready");
+ i2c_reset();
+ i2c_errors = 0;
+ }
+
+ if (running_op == OP_POLL) {
+ /* skip associated answer */
+ i2cproto_next_state(2);
+ }
+ }
+ running_op = OP_READY;
+}
+
+/* called rx event */
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+ if (running_op == OP_POLL)
+ i2cproto_next_state(1);
+
+ /* recv is only trigged after a poll */
+ running_op = OP_READY;
+
+ if (size < 0) {
+ goto error;
+ }
+
+ switch (buf[0]) {
+
+ case I2C_ANS_MECHBOARD_STATUS: {
+ struct i2c_ans_mechboard_status * ans =
+ (struct i2c_ans_mechboard_status *)buf;
+
+ if (size != sizeof (*ans))
+ goto error;
+
+ /* status */
+ mechboard.mode = ans->mode;
+ mechboard.status = ans->status;
+ mechboard.lintel_count = ans->lintel_count;
+ mechboard.column_flags = ans->column_flags;
+ /* pumps pwm */
+ mechboard.pump_left1 = ans->pump_left1;
+ mechboard.pump_left2 = ans->pump_left2;
+ mechboard.pump_right1 = ans->pump_right1;
+ mechboard.pump_right2 = ans->pump_right2;
+ pwm_ng_set(LEFT_PUMP1_PWM, mechboard.pump_left1);
+ pwm_ng_set(LEFT_PUMP2_PWM, mechboard.pump_left2);
+ /* pumps current */
+ mechboard.pump_right1_current = ans->pump_right1_current;
+ mechboard.pump_right2_current = ans->pump_right2_current;
+ /* servos */
+ mechboard.servo_lintel_left = ans->servo_lintel_left;
+ mechboard.servo_lintel_right = ans->servo_lintel_right;
+ pwm_ng_set(&gen.servo2, mechboard.servo_lintel_right);
+ pwm_ng_set(&gen.servo3, mechboard.servo_lintel_left);
+
+ break;
+ }
+
+ case I2C_ANS_SENSORBOARD_STATUS: {
+ struct i2c_ans_sensorboard_status * ans =
+ (struct i2c_ans_sensorboard_status *)buf;
+
+ if (size != sizeof (*ans))
+ goto error;
+ sensorboard.status = ans->status;
+ sensorboard.opponent_x = ans->opponent_x;
+ sensorboard.opponent_y = ans->opponent_y;
+ sensorboard.opponent_a = ans->opponent_a;
+ sensorboard.opponent_d = ans->opponent_d;
+
+ sensorboard.scan_status = ans->scan_status;
+ sensorboard.dropzone_h = ans->dropzone_h;
+ sensorboard.dropzone_x = ans->dropzone_x;
+ sensorboard.dropzone_y = ans->dropzone_y;
+ break;
+ }
+
+ default:
+ break;
+ }
+
+ return;
+ error:
+ i2c_errors++;
+ NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
+ i2c_state, running_op);
+ if (i2c_errors > I2C_MAX_ERRORS) {
+ I2C_ERROR("I2C error, slave not ready");
+ i2c_reset();
+ i2c_errors = 0;
+ }
+}
+
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
+{
+}
+
+
+
+/* ******** ******** ******** ******** */
+/* commands */
+/* ******** ******** ******** ******** */
+
+
+static int8_t
+i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
+{
+ uint8_t flags;
+ microseconds us = time_get_us2();
+
+ while ((time_get_us2() - us) < (I2C_TIMEOUT)*1000L) {
+ IRQ_LOCK(flags);
+ if (command_size == 0) {
+ memcpy(command_buf, buf, size);
+ command_size = size;
+ command_dest = addr;
+ IRQ_UNLOCK(flags);
+ return 0;
+ }
+ IRQ_UNLOCK(flags);
+ }
+ /* this should not happen... except if we are called from an
+ * interrupt context, but it's forbidden */
+ I2C_ERROR("I2C command send failed");
+ return -EBUSY;
+}
+
+static int8_t i2c_req_mechboard_status(void)
+{
+ struct i2c_req_mechboard_status buf;
+ int8_t err;
+
+ buf.hdr.cmd = I2C_REQ_MECHBOARD_STATUS;
+ buf.pump_left1_current = sensor_get_adc(ADC_CSENSE3);
+ buf.pump_left2_current = sensor_get_adc(ADC_CSENSE4);
+ err = i2c_send(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,
+ sizeof(buf), I2C_CTRL_GENERIC);
+
+ return err;
+}
+
+static int8_t i2c_req_sensorboard_status(void)
+{
+ struct i2c_req_sensorboard_status buf;
+
+ buf.hdr.cmd = I2C_REQ_SENSORBOARD_STATUS;
+ /* robot position */
+ buf.x = position_get_x_s16(&mainboard.pos);
+ buf.y = position_get_y_s16(&mainboard.pos);
+ buf.a = position_get_a_deg_s16(&mainboard.pos);
+ /* pickup wheels */
+ buf.enable_pickup_wheels = mainboard.enable_pickup_wheels;
+
+ return i2c_send(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf,
+ sizeof(buf), I2C_CTRL_GENERIC);
+}
+
+int8_t i2c_set_color(uint8_t addr, uint8_t color)
+{
+ struct i2c_cmd_generic_color buf;
+
+ if (addr == I2C_SENSORBOARD_ADDR)
+ return 0; /* XXX disabled for now */
+ buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
+ buf.color = color;
+ return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
+{
+ struct i2c_cmd_led_control buf;
+ buf.hdr.cmd = I2C_CMD_GENERIC_LED_CONTROL;
+ buf.led_num = led;
+ buf.state = state;
+ return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_manual(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_MANUAL;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_harvest(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_HARVEST;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_lazy_harvest(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
+ buf.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
+ buf.prep_pickup.side = side;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC;
+ buf.prep_pickup.side = side;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
+ buf.prep_pickup.next_mode = next_mode;
+ buf.prep_pickup.side = side;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_pickup(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PICKUP;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_eject(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_EJECT;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_manivelle(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_MANIVELLE;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_push_temple(uint8_t level)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE;
+ buf.push_temple.level = level;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
+ buf.prep_build.level_l = level;
+ buf.prep_build.level_r = level;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
+ buf.prep_build.level_l = level_l;
+ buf.prep_build.level_r = level_r;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
+ buf.prep_inside.level_l = level;
+ buf.prep_inside.level_r = level;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
+ buf.prep_inside.level_l = level_l;
+ buf.prep_inside.level_r = level_r;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
+ buf.autobuild.level_left = level;
+ buf.autobuild.level_right = level;
+ buf.autobuild.count_left = 2;
+ buf.autobuild.count_right = 2;
+ buf.autobuild.do_lintel = 1;
+ buf.autobuild.distance_left = 210;
+ buf.autobuild.distance_right = 210;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l,
+ uint8_t dist_l,
+ uint8_t level_r, uint8_t count_r,
+ uint8_t dist_r,
+ uint8_t do_lintel)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
+ buf.autobuild.level_left = level_l;
+ buf.autobuild.level_right = level_r;
+ buf.autobuild.count_left = count_l;
+ buf.autobuild.count_right = count_r;
+ buf.autobuild.distance_left = dist_l;
+ buf.autobuild.distance_right = dist_r;
+ buf.autobuild.do_lintel = do_lintel;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_init(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_INIT;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_prepare_get_lintel(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_get_lintel(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_GET_LINTEL;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_put_lintel(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_PUT_LINTEL;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_clear(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_CLEAR;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_loaded(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_LOADED;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_store(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_STORE;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_mechboard_mode_lazy_pickup(void)
+{
+ struct i2c_cmd_mechboard_set_mode buf;
+ buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
+ buf.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
+ return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_sensorboard_set_beacon(uint8_t enable)
+{
+ struct i2c_cmd_sensorboard_start_beacon buf;
+ buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_BEACON;
+ buf.enable = enable;
+ return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_sensorboard_scanner_set(uint8_t mode)
+{
+ struct i2c_cmd_sensorboard_scanner buf;
+ buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_SCANNER;
+ buf.mode = mode;
+ return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_sensorboard_scanner_calib(void)
+{
+ struct i2c_cmd_sensorboard_calib_scanner buf;
+ buf.hdr.cmd = I2C_CMD_SENSORBOARD_CALIB_SCANNER;
+ return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone,
+ int16_t x, int16_t y)
+{
+ struct i2c_cmd_sensorboard_scanner_algo buf;
+ buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
+ buf.algo = I2C_SCANNER_ALGO_COLUMN_DROPZONE;
+ buf.drop_zone.working_zone = zone;
+ buf.drop_zone.center_x = x;
+ buf.drop_zone.center_y = y;
+ return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone,
+ int16_t x, int16_t y)
+{
+ struct i2c_cmd_sensorboard_scanner_algo buf;
+ buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
+ buf.algo = I2C_SCANNER_ALGO_TEMPLE_DROPZONE;
+ buf.drop_zone.working_zone = zone;
+ buf.drop_zone.center_x = x;
+ buf.drop_zone.center_y = y;
+ return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_sensorboard_scanner_algo_check(uint8_t level,
+ int16_t x, int16_t y)
+{
+ struct i2c_cmd_sensorboard_scanner_algo buf;
+ buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
+ buf.algo = I2C_SCANNER_ALGO_CHECK_TEMPLE;
+ buf.check_temple.level = level;
+ buf.check_temple.temple_x = x;
+ buf.check_temple.temple_y = y;
+ return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
--- /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.h,v 1.6 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#ifndef _I2C_PROTOCOL_H_
+#define _I2C_PROTOCOL_H_
+
+void i2c_protocol_init(void);
+void i2c_protocol_debug(void);
+
+void i2cproto_wait_update(void);
+void i2c_poll_slaves(void *dummy);
+
+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);
+
+int8_t i2c_set_color(uint8_t addr, uint8_t color);
+int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state);
+
+int8_t i2c_mechboard_mode_manual(void);
+int8_t i2c_mechboard_mode_harvest(void);
+int8_t i2c_mechboard_mode_lazy_harvest(void);
+int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side);
+int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode);
+int8_t i2c_mechboard_mode_pickup(void);
+int8_t i2c_mechboard_mode_eject(void);
+int8_t i2c_mechboard_mode_lazy_pickup(void);
+
+int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level);
+int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r);
+int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level);
+int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r);
+int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level);
+int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l,
+ uint8_t dist_l,
+ uint8_t level_r, uint8_t count_r,
+ uint8_t dist_r,
+ uint8_t do_lintel);
+int8_t i2c_mechboard_mode_init(void);
+int8_t i2c_mechboard_mode_eject(void);
+int8_t i2c_mechboard_mode_prepare_get_lintel(void);
+int8_t i2c_mechboard_mode_get_lintel(void);
+int8_t i2c_mechboard_mode_put_lintel(void);
+int8_t i2c_mechboard_mode_clear(void);
+int8_t i2c_mechboard_mode_loaded(void);
+int8_t i2c_mechboard_mode_store(void);
+int8_t i2c_mechboard_mode_manivelle(void);
+int8_t i2c_mechboard_mode_push_temple(uint8_t level);
+int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side);
+
+int8_t i2c_sensorboard_set_beacon(uint8_t enable);
+
+int8_t i2c_sensorboard_scanner_set(uint8_t mode);
+static inline int8_t i2c_sensorboard_scanner_stop(void) {
+ return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_STOP);
+}
+static inline int8_t i2c_sensorboard_scanner_start(void) {
+ return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_START);
+}
+static inline int8_t i2c_sensorboard_scanner_prepare(void) {
+ return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_PREPARE);
+}
+
+int8_t i2c_sensorboard_scanner_calib(void);
+
+int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone,
+ int16_t x, int16_t y);
+int8_t i2c_sensorboard_scanner_algo_check(uint8_t level,
+ int16_t x, int16_t y);
+int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone,
+ int16_t x, int16_t y);
+
+
+#endif
--- /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.10 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <avr/eeprom.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 <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "../common/eeprom_mapping.h"
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+#include "ax12_user.h"
+#include "strat.h"
+#include "cmdline.h"
+#include "sensor.h"
+#include "actuator.h"
+#include "cs.h"
+#include "i2c_protocol.h"
+
+#if __AVR_LIBC_VERSION__ == 10602UL
+#error "won't work with this version"
+#endif
+
+/* 0 means "programmed"
+ * ---- with 16 Mhz quartz
+ * CKSEL 3-0 : 0111
+ * 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 mainboard mainboard;
+struct mechboard mechboard;
+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_time_monitor(void *dummy)
+{
+ uint16_t seconds;
+ seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
+ seconds ++;
+ eeprom_write_word(EEPROM_TIME_ADDRESS, seconds);
+}
+
+void do_led_blink(void *dummy)
+{
+#if 1 /* simple blink */
+ LED1_TOGGLE();
+#endif
+}
+
+static void main_timer_interrupt(void)
+{
+ static uint8_t cpt = 0;
+ cpt++;
+ sei();
+ if ((cpt & 0x3) == 0)
+ scheduler_interrupt();
+}
+
+int main(void)
+{
+ uint16_t seconds;
+
+ /* brake */
+ BRAKE_DDR();
+ BRAKE_OFF();
+
+ /* CPLD reset on PG3 */
+ DDRG |= 1<<3;
+ PORTG &= ~(1<<3); /* implicit */
+
+ /* LEDS */
+ DDRJ |= 0x0c;
+ DDRL = 0xc0;
+ LED1_OFF();
+ LED2_OFF();
+ LED3_OFF();
+ LED4_OFF();
+
+ memset(&gen, 0, sizeof(gen));
+ memset(&mainboard, 0, sizeof(mainboard));
+ mainboard.flags = DO_ENCODERS | DO_RS |
+ DO_POS | DO_POWER | DO_BD;
+ sensorboard.opponent_x = I2C_OPPONENT_NOT_THERE;
+
+ /* UART */
+ uart_init();
+#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_MAINBOARD);
+ /* check eeprom to avoid to run the bad program */
+ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
+ EEPROM_MAGIC_MAINBOARD) {
+ 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_init(I2C_MODE_MASTER, I2C_MAINBOARD_ADDR);
+ i2c_protocol_init();
+ i2c_register_recv_event(i2c_recvevent);
+ i2c_register_send_event(i2c_sendevent);
+
+ /* 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 |
+ PWM_NG_MODE_SIGN_INVERTED, &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);
+ pwm_ng_set(&gen.servo2, 290); /* right */
+ pwm_ng_set(&gen.servo3, 400); /* left */
+ /* 2 lintels 180, 485 */
+ /* 1 lintel 155, 520 */
+
+ /* SCHEDULER */
+ scheduler_init();
+
+ 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);
+
+ /* start i2c slave polling */
+ scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
+ 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO);
+
+ /* strat */
+ gen.logs[0] = E_USER_STRAT;
+ gen.log_level = 5;
+ strat_reset_infos();
+
+ /* strat-related event */
+ scheduler_add_periodical_event_priority(strat_event, NULL,
+ 25000L / SCHEDULER_UNIT,
+ STRAT_PRIO);
+
+ /* eeprom time monitor */
+ scheduler_add_periodical_event_priority(do_time_monitor, NULL,
+ 1000000L / SCHEDULER_UNIT,
+ EEPROM_TIME_PRIO);
+
+ sei();
+
+ printf_P(PSTR("\r\n"));
+ printf_P(PSTR("Respect et robustesse.\r\n"));
+ seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
+ printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
+ 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.10 2009-11-08 17:24:33 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)
+
+/* only 90 seconds, don't forget it :) */
+#define MATCH_TIME 89
+
+/* decrease track to decrease angle */
+#define EXT_TRACK_MM 302.0188
+#define VIRTUAL_TRACK_MM EXT_TRACK_MM
+
+#define ROBOT_LENGTH 320
+#define ROBOT_WIDTH 320
+
+/* it is a 2048 imps -> 8192 because we see 1/4 period
+ * and diameter: 55mm -> perimeter 173mm
+ * 8192/173 -> 473 */
+/* increase it to go further */
+#define IMP_ENCODERS 2048
+#define WHEEL_DIAMETER_MM 55.0
+#define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI)
+#define IMP_COEF 10.
+#define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF)
+
+#define LEFT_ENCODER ((void *)1)
+#define RIGHT_ENCODER ((void *)0)
+
+#define LEFT_PWM ((void *)&gen.pwm1_4A)
+#define RIGHT_PWM ((void *)&gen.pwm2_4B)
+
+#define LEFT_PUMP1_PWM ((void *)&gen.pwm3_1A)
+#define LEFT_PUMP2_PWM ((void *)&gen.pwm4_1B)
+
+/** ERROR NUMS */
+#define E_USER_STRAT 194
+#define E_USER_I2C_PROTO 195
+#define E_USER_SENSOR 196
+#define E_USER_CS 197
+
+#define LED_PRIO 170
+#define TIME_PRIO 160
+#define ADC_PRIO 120
+#define CS_PRIO 100
+#define STRAT_PRIO 30
+#define I2C_POLL_PRIO 20
+#define EEPROM_TIME_PRIO 10
+
+#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;
+};
+
+/* mainboard specific */
+struct mainboard {
+#define DO_ENCODERS 1
+#define DO_CS 2
+#define DO_RS 4
+#define DO_POS 8
+#define DO_BD 16
+#define DO_TIMER 32
+#define DO_POWER 64
+ uint8_t flags; /* misc flags */
+
+ /* control systems */
+ struct cs_block angle;
+ struct cs_block distance;
+
+ /* x,y positionning */
+ struct robot_system rs;
+ struct robot_position pos;
+ struct trajectory traj;
+
+ /* robot status */
+ uint8_t our_color;
+ volatile int16_t speed_a; /* current angle speed */
+ volatile int16_t speed_d; /* current dist speed */
+ int32_t pwm_l; /* current left pwm */
+ int32_t pwm_r; /* current right pwm */
+ uint8_t enable_pickup_wheels; /* these PWM are on sensorboard */
+
+};
+
+/* state of mechboard, synchronized through i2c */
+struct mechboard {
+ uint8_t mode;
+ uint8_t status;
+ int8_t lintel_count;
+ uint8_t column_flags;
+
+ /* pwm */
+ int16_t pump_left1;
+ int16_t pump_right1;
+ int16_t pump_left2;
+ int16_t pump_right2;
+
+ /* currents (for left arm, we can just read it on adc) */
+ int16_t pump_right1_current;
+ int16_t pump_right2_current;
+
+ /* pwm for lintel servos */
+ uint16_t servo_lintel_left;
+ uint16_t servo_lintel_right;
+};
+
+/* state of sensorboard, synchronized through i2c */
+struct sensorboard {
+ uint8_t status;
+ /* opponent pos */
+ int16_t opponent_x;
+ int16_t opponent_y;
+ int16_t opponent_a;
+ int16_t opponent_d;
+
+ /* scanner */
+#define I2C_SCAN_DONE 1
+ uint8_t scan_status;
+#define I2C_COLUMN_NO_DROPZONE -1
+ int8_t dropzone_h;
+ int16_t dropzone_x;
+ int16_t dropzone_y;
+};
+
+extern struct genboard gen;
+extern struct mainboard mainboard;
+extern struct mechboard mechboard;
+extern struct sensorboard sensorboard;
+
+/* 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; \
+ } \
+ } \
+ if (__ret) \
+ DEBUG(E_USER_STRAT, "cond is true at line %d",\
+ __LINE__); \
+ else \
+ DEBUG(E_USER_STRAT, "timeout at line %d", \
+ __LINE__); \
+ \
+ __ret; \
+})
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: obstacle_avoidance_config.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#define MAX_POLY 3
+#define MAX_PTS 12
+#define MAX_RAYS 150
+#define MAX_CHKPOINTS 7
--- /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-03-15 20:18:33 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 10
+
+
+#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.8 2009-11-08 17:24:33 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 <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "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 },
+ [ADC_BATTERY1] = { .config = ADC_CONF(8), .filter = rii_strong },
+ [ADC_BATTERY2] = { .config = ADC_CONF(9), .filter = rii_strong },
+
+ /* 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 1,5,6,7,8
+ */
+static struct sensor_filter sensor_filter[SENSOR_MAX] = {
+ [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
+ [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
+ [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */
+ [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */
+ [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 0 */
+ [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */
+ [S_DISP_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 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);
+}
+
+/* virtual obstacle */
+
+#define DISABLE_CPT_MAX 200
+static uint8_t disable = 0; /* used to disable obstacle detection
+ * during some time */
+
+/* called every 10 ms */
+void
+sensor_obstacle_update(void)
+{
+ if (disable > 0) {
+ disable --;
+ if (disable == 0)
+ DEBUG(E_USER_STRAT, "re-enable sensor");
+ }
+}
+
+void sensor_obstacle_disable(void)
+{
+ DEBUG(E_USER_STRAT, "disable sensor");
+ disable = DISABLE_CPT_MAX;
+}
+
+void sensor_obstacle_enable(void)
+{
+ disable = 0;
+}
+
+uint8_t sensor_obstacle_is_disabled(void)
+{
+ return disable;
+}
+
+
+/************ global sensor init */
+
+/* called every 10 ms, see init below */
+static void do_sensors(void *dummy)
+{
+ do_adc(NULL);
+ do_boolean_sensors(NULL);
+ sensor_obstacle_update();
+}
+
+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.6 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+/* synchronize with sensor.c */
+#define ADC_CSENSE1 0
+#define ADC_CSENSE2 1
+#define ADC_CSENSE3 2
+#define ADC_CSENSE4 3
+#define ADC_BATTERY1 4
+#define ADC_BATTERY2 5
+#define ADC_MAX 6
+
+/* synchronize with sensor.c */
+#define S_CAP1 0
+#define S_CAP2 1
+#define S_COLUMN_RIGHT 2
+#define S_COLUMN_LEFT 3
+#define S_START_SWITCH 4
+#define S_DISP_LEFT 5
+#define S_DISP_RIGHT 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);
+
+void sensor_obstacle_disable(void);
+void sensor_obstacle_enable(void);
+uint8_t sensor_obstacle_is_disabled(void);
--- /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, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <diagnostic.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
+#include "main.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "sensor.h"
+#include "actuator.h"
+
+#define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
+#define COL_SCAN_PRE_MARGIN 250
+
+
+#ifdef TEST_BEACON
+
+#define BEACON_MAX_SAMPLES 100
+struct beacon_sample {
+ int16_t posx;
+ int16_t posy;
+ int16_t posa;
+ int16_t oppx;
+ int16_t oppy;
+ uint8_t time;
+};
+
+static struct beacon_sample beacon_sample[BEACON_MAX_SAMPLES];
+static uint8_t beacon_prev_time = 0;
+static uint8_t beacon_cur_idx = 0;
+
+static void beacon_update_samples(void)
+{
+ int16_t opp_a, opp_d, opp_x, opp_y;
+ int8_t err;
+ uint8_t time;
+
+ time = time_get_s();
+
+ /* one sample per second max */
+ if (time <= beacon_prev_time)
+ return;
+ /* limit max number of samples */
+ if (beacon_cur_idx >= BEACON_MAX_SAMPLES)
+ return;
+
+ memset(&beacon_sample[beacon_cur_idx], 0, sizeof(beacon_sample[beacon_cur_idx]));
+ beacon_prev_time = time;
+ beacon_sample[beacon_cur_idx].time = time;
+
+ /* get opponent pos; if not found, just set struct to 0 */
+ err = get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a);
+ if (err == -1)
+ return;
+
+ beacon_sample[beacon_cur_idx].posx = position_get_x_s16(&mainboard.pos);
+ beacon_sample[beacon_cur_idx].posy = position_get_y_s16(&mainboard.pos);
+ beacon_sample[beacon_cur_idx].posa = position_get_a_deg_s16(&mainboard.pos);
+ beacon_sample[beacon_cur_idx].oppx = opp_x;
+ beacon_sample[beacon_cur_idx].oppy = opp_y;
+ beacon_cur_idx++;
+}
+
+void beacon_dump_samples(void)
+{
+ uint16_t i;
+
+ for (i=0; i<BEACON_MAX_SAMPLES; i++) {
+ printf_P(PSTR("%d: pos=(%d,%d,%d) opp=(%d,%d) time=%d\r\n"),
+ i,
+ beacon_sample[i].posx,
+ beacon_sample[i].posy,
+ beacon_sample[i].posa,
+ beacon_sample[i].oppx,
+ beacon_sample[i].oppy,
+ beacon_sample[i].time);
+ }
+}
+#endif
+
+struct strat_infos strat_infos = {
+ /* conf */
+ .conf = {
+ .flags = 0,
+ /* scanner disabled by default */
+ .scan_opp_min_time = 90,
+ .delay_between_opp_scan = 90,
+ .scan_our_min_time = 90,
+ .delay_between_our_scan = 90,
+ .wait_opponent = 0,
+ .lintel_min_time = 0,
+ .scan_opp_angle = -1,
+ },
+
+ /* static columns */
+ .s_cols = {
+ .flags = 0,
+ .configuration = 0,
+ },
+
+ /* column dispensers ; be carreful, positions are
+ * color-dependent, so COLOR_Y() and COLOR_A() should be
+ * used. All angles here are _absolute_ */
+ .c1 = {
+ .checkpoint_x = 2711 - COL_SCAN_PRE_MARGIN,
+ .checkpoint_y = AREA_Y - COL_DISP_MARGIN,
+ .scan_left = 0,
+ .scan_a = 180,
+ .eject_a = 180,
+ .recalib_x = 2711,
+ .recalib_y = AREA_Y - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
+ .pickup_a = 90,
+ .name = "col_disp1",
+ },
+ .c2 = {
+ .checkpoint_x = AREA_X - COL_DISP_MARGIN,
+ .checkpoint_y = 800 - COL_SCAN_PRE_MARGIN,
+ .scan_left = 1,
+ .scan_a = -90,
+ .eject_a = -90,
+ .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
+ .recalib_y = 800,
+ .pickup_a = 0,
+ .name = "col_disp2",
+ },
+ .c3 = {
+ .checkpoint_x = AREA_X-COL_DISP_MARGIN,
+ .checkpoint_y = 1300 + COL_SCAN_PRE_MARGIN,
+ .scan_a = 90,
+ .scan_left = 0,
+ .eject_a = -90,
+ .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
+ .recalib_y = 1300,
+ .pickup_a = 0,
+ .name = "col_disp3",
+ },
+
+ /* lintel dispensers */
+ .l1 = {
+ .x = 912, /* XXX for red only */
+ .name = "lin_disp1",
+ },
+ .l2 = {
+ .x = 1312, /* XXX for red only */
+ .name = "lin_disp2",
+ },
+
+ /* build zones */
+ .zone_list = {
+#define ZONE_DISC_NUM 0
+ {
+ .flags = ZONE_F_VALID | ZONE_F_DISC,
+ .level = 2,
+ .checkpoint_x = 0,
+ .checkpoint_x = 0,
+ .name = "disc",
+ },
+#define ZONE_1A_NUM 1
+ {
+ .flags = ZONE_F_VALID,
+ .level = 1,
+ .checkpoint_x = 1385,
+ .checkpoint_y = 1700,
+ .name = "z1a",
+ },
+#define ZONE_1B_NUM 2
+ {
+ .flags = ZONE_F_VALID,
+ .level = 1,
+ .checkpoint_x = 1615,
+ .checkpoint_y = 1700,
+ .name = "z1b",
+ },
+#define ZONE_0B_NUM 3
+ {
+ .flags = ZONE_F_VALID,
+ .level = 0,
+ .checkpoint_x = 2100,
+ .checkpoint_y = 1700,
+ .name = "z0b",
+ },
+#define ZONE_0A_NUM 4
+ {
+ .flags = ZONE_F_VALID,
+ .level = 0,
+ .checkpoint_x = 900,
+ .checkpoint_y = 1700,
+ .name = "z0a",
+ },
+ }
+};
+
+/*************************************************************/
+
+/* INIT */
+
+/*************************************************************/
+
+void strat_set_bounding_box(void)
+{
+ if (get_color() == I2C_COLOR_RED) {
+ strat_infos.area_bbox.x1 = 300;
+ strat_infos.area_bbox.y1 = 200;
+ strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
+ strat_infos.area_bbox.y2 = 1800;
+ }
+ else {
+ strat_infos.area_bbox.x1 = 200;
+ strat_infos.area_bbox.y1 = 300;
+ strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
+ strat_infos.area_bbox.y2 = 1900;
+ }
+
+ polygon_set_boundingbox(strat_infos.area_bbox.x1,
+ strat_infos.area_bbox.y1,
+ strat_infos.area_bbox.x2,
+ strat_infos.area_bbox.y2);
+}
+
+/* called before each strat, and before the start switch */
+void strat_preinit(void)
+{
+ time_reset();
+ interrupt_traj_reset();
+ mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
+ DO_POS | DO_BD | DO_POWER;
+
+ i2c_mechboard_mode_init();
+ if (get_color() == I2C_COLOR_RED)
+ i2c_mechboard_mode_prepare_pickup(I2C_LEFT_SIDE);
+ else
+ i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
+
+ strat_dump_conf();
+ strat_dump_infos(__FUNCTION__);
+}
+
+void strat_dump_conf(void)
+{
+ if (!strat_infos.dump_enabled)
+ return;
+
+ printf_P(PSTR("-- conf --\r\n"));
+
+ printf_P(PSTR(" one build on disc: "));
+ if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" bypass static2: "));
+ if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" take one lintel: "));
+ if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" skip this temple when temple check fails: "));
+ if (strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" store static2: "));
+ if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" (big3) try to build a temple with 3 lintels: "));
+ if (strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" early opponent scan: "));
+ if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" push opponent columns: "));
+ if (strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS)
+ printf_P(PSTR("on\r\n"));
+ else
+ printf_P(PSTR("off\r\n"));
+
+ printf_P(PSTR(" scan opponent min time: %d\r\n"),
+ strat_infos.conf.scan_opp_min_time);
+ printf_P(PSTR(" delay between oppnent scan: %d\r\n"),
+ strat_infos.conf.delay_between_opp_scan);
+ printf_P(PSTR(" scan our min time: %d\r\n"),
+ strat_infos.conf.scan_our_min_time);
+ printf_P(PSTR(" delay between our scan: %d\r\n"),
+ strat_infos.conf.delay_between_our_scan);
+ printf_P(PSTR(" wait opponent gone before scan: %d\r\n"),
+ strat_infos.conf.wait_opponent);
+ printf_P(PSTR(" lintel min time: %d\r\n"),
+ strat_infos.conf.lintel_min_time);
+ printf_P(PSTR(" scan_opp_angle: %d\r\n"),
+ strat_infos.conf.scan_opp_angle);
+}
+
+void strat_dump_temple(struct temple *temple)
+{
+ if (!strat_infos.dump_enabled)
+ return;
+
+ printf_P(PSTR(" temple %p (%s): "), temple, temple->zone->name);
+
+ if (temple->flags & TEMPLE_F_MONOCOL)
+ printf_P(PSTR("MONOCOL "));
+ else
+ printf_P(PSTR("BICOL "));
+
+ if (temple->flags & TEMPLE_F_ON_DISC)
+ printf_P(PSTR("ON_DISC "));
+ else
+ printf_P(PSTR("ON_ZONE_0_1 "));
+
+ if (temple->flags & TEMPLE_F_OPPONENT)
+ printf_P(PSTR("OPPONENT "));
+ else
+ printf_P(PSTR("OURS "));
+
+ if (temple->flags & TEMPLE_F_LINTEL)
+ printf_P(PSTR("LIN_ON_TOP "));
+ else
+ printf_P(PSTR("COL_ON_TOP "));
+
+ printf_P(PSTR("\r\n"));
+
+ printf_P(PSTR(" pos=(%d,%d,%d) ckpt=(%d,%d) ltime=%d\r\n"),
+ temple->x, temple->y, temple->a,
+ temple->checkpoint_x, temple->checkpoint_y,
+ temple->last_try_time);
+ printf_P(PSTR(" L: lev=%d da=%d,%d\r\n"),
+ temple->level_l, temple->dist_l, temple->angle_l);
+ printf_P(PSTR(" R: lev=%d da=%d,%d\r\n"),
+ temple->level_l, temple->dist_l, temple->angle_l);
+}
+
+void strat_dump_zone(struct build_zone *zone)
+{
+ if (!strat_infos.dump_enabled)
+ return;
+
+ printf_P(PSTR(" zone %s: "), zone->name);
+
+ if (zone->flags & ZONE_F_DISC)
+ printf_P(PSTR("DISC "));
+ else if (zone->flags & ZONE_F_ZONE1)
+ printf_P(PSTR("ZONE1 "));
+ else if (zone->flags & ZONE_F_ZONE0)
+ printf_P(PSTR("ZONE0 "));
+
+ if (zone->flags & ZONE_F_BUSY)
+ printf_P(PSTR("BUSY "));
+ else
+ printf_P(PSTR("FREE "));
+
+ printf_P(PSTR("\r\n"));
+
+ printf_P(PSTR(" lev=%d ckpt=(%d,%d) ltime=%d\r\n"),
+ zone->level,
+ zone->checkpoint_x, zone->checkpoint_y,
+ zone->last_try_time);
+}
+
+void strat_dump_static_cols(void)
+{
+ if (!strat_infos.dump_enabled)
+ return;
+
+ printf_P(PSTR(" static cols: l0=%d l1=%d l2=%d\r\n"),
+ strat_infos.s_cols.flags & STATIC_COL_LINE0_DONE,
+ strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE,
+ strat_infos.s_cols.flags & STATIC_COL_LINE2_DONE);
+}
+
+void strat_dump_col_disp(void)
+{
+ if (!strat_infos.dump_enabled)
+ return;
+
+ printf_P(PSTR(" c1 cnt=%d ltt=%d\r\n"),
+ strat_infos.c1.count, strat_infos.c1.last_try_time);
+ printf_P(PSTR(" c2 cnt=%d ltt=%d\r\n"),
+ strat_infos.c2.count, strat_infos.c2.last_try_time);
+ printf_P(PSTR(" c3 cnt=%d ltt=%d\r\n"),
+ strat_infos.c3.count, strat_infos.c3.last_try_time);
+}
+
+void strat_dump_lin_disp(void)
+{
+ if (!strat_infos.dump_enabled)
+ return;
+ printf_P(PSTR(" l1 cnt=%d ltt=%d\r\n"),
+ strat_infos.l1.count, strat_infos.l1.last_try_time);
+ printf_P(PSTR(" l2 cnt=%d ltt=%d\r\n"),
+ strat_infos.l2.count, strat_infos.l2.last_try_time);
+
+}
+
+void strat_dump_all_temples(void)
+{
+ struct temple *temple;
+ uint8_t i;
+
+ if (!strat_infos.dump_enabled)
+ return;
+
+ for (i=0; i<MAX_TEMPLE; i++) {
+ temple = &strat_infos.temple_list[i];
+ if (!(temple->flags & TEMPLE_F_VALID))
+ continue;
+ strat_dump_temple(temple);
+ }
+}
+
+void strat_dump_all_zones(void)
+{
+ struct build_zone *zone;
+ uint8_t i;
+
+ if (!strat_infos.dump_enabled)
+ return;
+
+ for (i=0; i<MAX_ZONE; i++) {
+ zone = &strat_infos.zone_list[i];
+ if (!(zone->flags & ZONE_F_VALID))
+ continue;
+ strat_dump_zone(zone);
+ }
+}
+
+/* display current information about the state of the game */
+void strat_dump_infos(const char *caller)
+{
+ if (!strat_infos.dump_enabled)
+ return;
+
+ printf_P(PSTR("%s() dump strat infos:\r\n"), caller);
+ strat_dump_static_cols();
+ strat_dump_col_disp();
+ strat_dump_lin_disp();
+ strat_dump_all_temples();
+ strat_dump_all_zones();
+}
+
+/* init current area state before a match. Dump update user conf
+ * here */
+void strat_reset_infos(void)
+{
+ uint8_t i;
+
+ /* /!\ don't do a big memset() as there is static data */
+ strat_infos.s_cols.flags = 0;
+ strat_infos.c1.count = 5;
+ strat_infos.c1.last_try_time = 0;
+ strat_infos.c2.count = 5;
+ strat_infos.c2.last_try_time = 0;
+ strat_infos.c3.count = 5;
+ strat_infos.c3.last_try_time = 0;
+ strat_infos.l1.count = 1;
+ strat_infos.l1.last_try_time = 0;
+ strat_infos.l2.count = 1;
+ strat_infos.l2.last_try_time = 0;
+
+ strat_infos.taken_lintel = 0;
+ strat_infos.col_in_boobs = 0;
+ strat_infos.lazy_pickup_done = 0;
+ strat_infos.i2c_loaded_skipped = 0;
+
+ memset(strat_infos.temple_list, 0, sizeof(strat_infos.temple_list));
+
+ for (i=0; i<MAX_ZONE; i++)
+ strat_infos.zone_list[i].flags = ZONE_F_VALID;
+ strat_infos.zone_list[ZONE_DISC_NUM].flags |= ZONE_F_DISC;
+ strat_infos.zone_list[ZONE_1A_NUM].flags |= ZONE_F_ZONE1;
+ strat_infos.zone_list[ZONE_1B_NUM].flags |= ZONE_F_ZONE1;
+ strat_infos.zone_list[ZONE_0A_NUM].flags |= ZONE_F_ZONE0;
+ strat_infos.zone_list[ZONE_0B_NUM].flags |= ZONE_F_ZONE0;
+
+ strat_set_bounding_box();
+
+ /* set lintel position, depending on color */
+ if (mainboard.our_color == I2C_COLOR_RED) {
+ strat_infos.l1.x = 912;
+ strat_infos.l2.x = 1312;
+ }
+ else {
+ strat_infos.l1.x = 888;
+ strat_infos.l2.x = 1288;
+ }
+}
+
+/* call it just before launching the strat */
+void strat_init(void)
+{
+ pickup_wheels_on();
+ strat_reset_infos();
+
+ /* we consider that the color is correctly set */
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ time_reset();
+ interrupt_traj_reset();
+
+ /* used in strat_base for END_TIMER */
+ mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
+ DO_POS | DO_BD | DO_TIMER | DO_POWER;
+
+#ifdef TEST_BEACON
+ beacon_prev_time = 0;
+ beacon_cur_idx = 0;
+#endif
+}
+
+
+/* call it after each strat */
+void strat_exit(void)
+{
+ uint8_t flags;
+
+ pickup_wheels_off();
+ mainboard.flags &= ~(DO_TIMER);
+ strat_hardstop();
+ time_reset();
+ wait_ms(1000);
+ IRQ_LOCK(flags);
+ mainboard.flags &= ~(DO_CS);
+ pwm_ng_set(LEFT_PWM, 0);
+ pwm_ng_set(RIGHT_PWM, 0);
+ IRQ_UNLOCK(flags);
+}
+
+/* called periodically */
+void strat_event(void *dummy)
+{
+ /* limit speed when opponent is close */
+ strat_limit_speed();
+
+#ifdef TEST_BEACON
+ beacon_update_samples();
+#endif
+}
+
+/* do static cols + first temples */
+static uint8_t strat_beginning(void)
+{
+ uint8_t err;
+
+ /* don't limit the speed when opponent is near: it can change
+ * the radius of the curves */
+ strat_limit_speed_disable();
+
+ err = strat_static_columns(0);
+
+ strat_limit_speed_enable();
+
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ /* go to disc to build the first temple */
+
+ /* XXX if opponent is near disc, go to zone1 */
+ err = strat_goto_disc(2);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+ DEBUG(E_USER_STRAT, "disc reached");
+
+ /* can return END_ERROR or END_TIMER, should not happen
+ * here */
+ err = strat_build_new_temple(&strat_infos.zone_list[0]);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ /* bypass static2 if specified */
+ if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2) {
+ err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
+ return err;
+ }
+
+ /* get the last 2 columns, and build them on previous temple */
+ err = strat_static_columns_pass2();
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ /* early opponent scan, for offensive strategy */
+ if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) {
+ err = strat_pickup_lintels();
+ /* ignore code */
+
+ /* try to build on opponent (scan must be enabled) */
+ err = strat_build_on_opponent_temple();
+ /* ignore code */
+ }
+
+ return err;
+}
+
+/* return true if we need to grab some more elements (lintel/cols) */
+uint8_t need_more_elements(void)
+{
+ if (time_get_s() <= 75) {
+ /* we have at least one col on each arm, build now */
+ if ((get_column_count_left() >= 1) &&
+ (get_column_count_right() >= 1))
+ return 0;
+ }
+ else {
+ if (get_column_count())
+ return 0;
+ }
+ return 1;
+}
+
+/* dump state (every 5 s max) */
+#define DUMP_RATE_LIMIT(dump, last_print) \
+ do { \
+ if (time_get_s() - last_print > 5) { \
+ dump(); \
+ last_print = time_get_s(); \
+ } \
+ } while (0)
+
+
+uint8_t strat_main(void)
+{
+ uint8_t err;
+ struct temple *temple = NULL;
+ struct build_zone *zone = NULL;
+
+ uint8_t last_print_cols = 0;
+ uint8_t last_print_lin = 0;
+ uint8_t last_print_temple = 0;
+ uint8_t last_print_zone = 0;
+
+ /* do static cols + first temple */
+ err = strat_beginning();
+
+ /* skip error code */
+
+ while (1) {
+
+ if (err == END_TIMER) {
+ DEBUG(E_USER_STRAT, "End of time");
+ strat_exit();
+ break;
+ }
+
+ /* we have at least one col on each arm, build now */
+ if (need_more_elements() == 0) {
+
+ /* try to build on opponent, will return
+ * END_TRAJ without doing anything if
+ * disabled */
+ err = strat_build_on_opponent_temple();
+ if (!TRAJ_SUCCESS(err))
+ continue;
+ if (need_more_elements())
+ continue;
+
+ /* try to scan and build on our temple, will
+ * return END_TRAJ without doing anything if
+ * disabled */
+ err = strat_check_temple_and_build();
+ if (!TRAJ_SUCCESS(err))
+ continue;
+ if (need_more_elements())
+ continue;
+
+ /* Else, do a simple build, as before */
+
+ temple = strat_get_best_temple();
+
+ /* one valid temple found */
+ if (temple) {
+ DUMP_RATE_LIMIT(strat_dump_all_temples, last_print_temple);
+
+ err = strat_goto_temple(temple);
+ if (!TRAJ_SUCCESS(err))
+ continue;
+
+ /* can return END_ERROR or END_TIMER,
+ * should not happen here */
+ err = strat_grow_temple(temple);
+ if (!TRAJ_SUCCESS(err))
+ continue;
+
+ err = strat_escape(temple->zone, TRAJ_FLAGS_STD);
+ if (!TRAJ_SUCCESS(err))
+ continue;
+
+ continue;
+ }
+
+ zone = strat_get_best_zone();
+ if (zone) {
+ DUMP_RATE_LIMIT(strat_dump_all_zones, last_print_zone);
+
+ DEBUG(E_USER_STRAT, "goto zone %s", zone->name);
+ err = strat_goto_build_zone(zone, zone->level);
+ if (!TRAJ_SUCCESS(err))
+ continue;
+ DEBUG(E_USER_STRAT, "zone reached");
+
+ /* no error code except END_ERROR, should not happen */
+ err = strat_build_new_temple(zone);
+
+ err = strat_escape(zone, TRAJ_FLAGS_STD);
+ if (!TRAJ_SUCCESS(err))
+ continue;
+
+ continue;
+ }
+
+ /* XXX hey what can we do here... :'( */
+ DEBUG(E_USER_STRAT, "panic :)");
+ time_wait_ms(1000);
+ continue;
+ }
+
+ /* else we need some elements (lintels, then columns) */
+ else {
+ if (strat_infos.l1.count != 0 && strat_infos.l2.count != 0)
+ DUMP_RATE_LIMIT(strat_dump_lin_disp, last_print_lin);
+
+ err = strat_pickup_lintels();
+ /* can return an error code, but we have
+ * nothing to do because pickup_column()
+ * starts with a goto_and_avoid() */
+ if (!TRAJ_SUCCESS(err))
+ nop();
+
+ DUMP_RATE_LIMIT(strat_dump_col_disp, last_print_cols);
+
+ err = strat_pickup_columns();
+ if (!TRAJ_SUCCESS(err))
+ nop(); /* nothing to do */
+
+ /* XXX check here that we have elements, or do
+ * something else */
+ /* if we cannot take elements, try to build */
+ }
+ }
+ return END_TRAJ;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#ifndef _STRAT_H_
+#define _STRAT_H_
+
+/* convert coords according to our color */
+#define COLOR_Y(y) ((mainboard.our_color==I2C_COLOR_RED)? (y) : (AREA_Y-(y)))
+#define COLOR_A(a) ((mainboard.our_color==I2C_COLOR_RED)? (a) : (-a))
+#define COLOR_SIGN(x) ((mainboard.our_color==I2C_COLOR_RED)? (x) : (-x))
+#define COLOR_INVERT(x) ((mainboard.our_color==I2C_COLOR_RED)? (x) : (!x))
+
+/* area */
+#define AREA_X 3000
+#define AREA_Y 2100
+
+#define START_X 200
+#define START_Y COLOR_Y(200)
+#define START_A COLOR_A(45)
+
+#define CENTER_X 1500
+#define CENTER_Y 1050
+
+#define CORNER_X 3000
+#define CORNER_Y COLOR_Y(2100)
+
+/*
+ * /- line 0
+ * | /- line 1
+ * | | /- line 2
+ * | | |
+ * +---C1--------------------------C1---+
+ * | z0a z1 z0b |
+ * | . |
+ * | . |
+ * C3 0 1 2 ^ C3
+ * y | 3 4 5 < > |
+ * C2 6 7 8 v C2
+ * |------ 9 10 11 . ------|
+ * | | . | |
+ * | red | . |green|
+ * +-----+--L1--L2-------L2--L1---+-----+
+ * x
+ */
+
+/* static columns */
+#define LINE0_X 600
+#define LINE1_X 850
+#define LINE2_X 1100
+
+#define COL0_X 600
+#define COL0_Y COLOR_Y(1175)
+#define COL1_X 850
+#define COL1_Y COLOR_Y(1175)
+#define COL2_X 1100
+#define COL2_Y COLOR_Y(1175)
+
+#define COL3_X 600
+#define COL3_Y COLOR_Y(975)
+#define COL4_X 850
+#define COL4_Y COLOR_Y(975)
+#define COL5_X 1100
+#define COL5_Y COLOR_Y(975)
+
+#define COL6_X 600
+#define COL6_Y COLOR_Y(775)
+#define COL7_X 850
+#define COL7_Y COLOR_Y(775)
+#define COL8_X 1100
+#define COL8_Y COLOR_Y(775)
+
+#define COL9_X 600
+#define COL9_Y COLOR_Y(575)
+#define COL10_X 850
+#define COL10_Y COLOR_Y(575)
+#define COL11_X 1100
+#define COL11_Y COLOR_Y(575)
+
+/* distance to go backward before pickup in dispenser */
+#define DIST_BACK_DISPENSER 35
+
+/* diag of the pentagon (pentacle ?) */
+#define DISC_PENTA_DIAG 530
+
+#define COL_DISP_MAX_TRIES 5
+#define LIN_DISP_MAX_TRIES 3
+
+/* useful traj flags */
+#define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR))
+#define TRAJ_FLAGS_STD (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR|END_TIMER)
+#define TRAJ_FLAGS_NO_TIMER (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR)
+#define TRAJ_FLAGS_NO_NEAR (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR|END_TIMER)
+#define TRAJ_FLAGS_NO_NEAR_NO_TIMER (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR)
+#define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR)
+
+/* default speeds */
+#define SPEED_DIST_FAST 2500
+#define SPEED_ANGLE_FAST 2000
+#define SPEED_DIST_SLOW 1000
+#define SPEED_ANGLE_SLOW 1000
+#define SPEED_DIST_VERY_SLOW 400
+#define SPEED_ANGLE_VERY_SLOW 400
+
+/* strat infos structures */
+
+struct bbox {
+ int32_t x1;
+ int32_t y1;
+ int32_t x2;
+ int32_t y2;
+};
+
+struct conf {
+#define STRAT_CONF_ONLY_ONE_ON_DISC 0x01
+#define STRAT_CONF_BYPASS_STATIC2 0x02
+#define STRAT_CONF_TAKE_ONE_LINTEL 0x04
+#define STRAT_CONF_SKIP_WHEN_CHECK_FAILS 0x08
+#define STRAT_CONF_STORE_STATIC2 0x10
+#define STRAT_CONF_BIG_3_TEMPLE 0x20
+#define STRAT_CONF_EARLY_SCAN 0x40
+#define STRAT_CONF_PUSH_OPP_COLS 0x80
+ uint8_t flags;
+ uint8_t scan_opp_min_time;
+ uint8_t delay_between_opp_scan;
+ uint8_t scan_our_min_time;
+ uint8_t delay_between_our_scan;
+ uint8_t wait_opponent;
+ uint8_t lintel_min_time;
+ int16_t scan_opp_angle;
+};
+
+struct static_columns {
+#define STATIC_COL_LINE0_DONE 0x01
+#define STATIC_COL_LINE1_DONE 0x02
+#define STATIC_COL_LINE2_DONE 0x04
+ uint8_t flags;
+ uint8_t configuration;
+};
+
+struct column_dispenser {
+ int8_t count;
+ uint8_t last_try_time;
+ uint8_t scan_left;
+ int16_t checkpoint_x;
+ int16_t checkpoint_y;
+ int16_t scan_a;
+ int16_t eject_a;
+ int16_t pickup_a;
+ int16_t recalib_x;
+ int16_t recalib_y;
+ char *name;
+};
+
+struct lintel_dispenser {
+ int8_t count;
+ uint8_t last_try_time;
+ int16_t x;
+ char *name;
+};
+
+struct temple {
+#define TEMPLE_F_VALID 0x01 /* structure is valid */
+#define TEMPLE_F_MONOCOL 0x02 /* temple has only one col */
+#define TEMPLE_F_ON_DISC 0x04 /* temple is on disc (else it's on other zone) */
+#define TEMPLE_F_OPPONENT 0x08 /* temple was originally built by opponent */
+#define TEMPLE_F_LINTEL 0x10 /* lintel on top (don't put another lintel) */
+
+ uint8_t flags;
+ /* position of the robot when we built it */
+ int16_t x;
+ int16_t y;
+ int16_t a;
+
+ /* position of the robot checkpoint */
+ int16_t checkpoint_x;
+ int16_t checkpoint_y;
+
+ /* position and level of each col */
+ uint8_t level_l;
+ uint8_t dist_l;
+ uint8_t angle_l;
+
+ uint8_t level_r;
+ uint8_t dist_r;
+ uint8_t angle_r;
+
+#define TEMPLE_DISABLE_TIME 5
+ uint8_t last_try_time;
+
+ struct build_zone *zone;
+};
+
+struct build_zone {
+#define ZONE_F_VALID 0x01 /* zone is valid */
+#define ZONE_F_DISC 0x02 /* specific disc zone */
+#define ZONE_F_ZONE1 0x04 /* specific zone 1 */
+#define ZONE_F_ZONE0 0x08 /* specific zone 0 */
+#define ZONE_F_BUSY 0x10 /* this zone is busy */
+ uint8_t flags;
+ uint8_t level;
+ int16_t checkpoint_x;
+ int16_t checkpoint_y;
+
+#define ZONE_DISABLE_TIME 5
+ uint8_t last_try_time;
+ char *name;
+};
+
+#define MAX_TEMPLE 5
+#define MAX_ZONE 5
+
+/* all infos related to strat */
+struct strat_infos {
+ uint8_t dump_enabled;
+ struct conf conf;
+ struct bbox area_bbox;
+ uint8_t taken_lintel;
+ uint8_t col_in_boobs;
+ uint8_t lazy_pickup_done;
+ uint8_t i2c_loaded_skipped;
+ struct static_columns s_cols;
+ struct column_dispenser c1;
+ struct column_dispenser c2;
+ struct column_dispenser c3;
+ struct lintel_dispenser l1;
+ struct lintel_dispenser l2;
+ struct build_zone zone_list[MAX_ZONE];
+ struct temple temple_list[MAX_TEMPLE];
+};
+extern struct strat_infos strat_infos;
+
+/* in strat.c */
+void strat_dump_infos(const char *caller); /* show current known state
+ of area */
+void strat_dump_temple(struct temple *temple);
+void strat_dump_conf(void);
+void strat_reset_infos(void); /* reset current known state */
+void strat_preinit(void);
+void strat_init(void);
+void strat_exit(void);
+void strat_dump_flags(void);
+void strat_goto_near(int16_t x, int16_t y, uint16_t dist);
+uint8_t strat_main(void);
+void strat_event(void *dummy);
+
+/* in strat_static_columns.c */
+uint8_t strat_static_columns(uint8_t configuration);
+uint8_t strat_static_columns_pass2(void);
+
+/* in strat_lintel.c */
+uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp);
+uint8_t strat_pickup_lintels(void);
+
+/* in strat_column_disp.c */
+uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a);
+uint8_t strat_pickup_columns(void);
+uint8_t strat_goto_col_disp(struct column_dispenser **disp);
+
+/* in strat_building.c */
+uint8_t strat_goto_disc(int8_t level);
+uint8_t strat_goto_build_zone(struct build_zone *build_zone, uint8_t level);
+uint8_t strat_build_new_temple(struct build_zone *build_zone);
+uint8_t strat_goto_temple(struct temple *temple);
+uint8_t strat_grow_temple(struct temple *temple);
+uint8_t strat_grow_temple_column(struct temple *temple);
+struct temple *strat_get_best_temple(void);
+struct temple *strat_get_our_temple_on_disc(uint8_t valid);
+struct build_zone *strat_get_best_zone(void);
+struct temple *strat_get_free_temple(void);
+
+/* in strat_scan.c */
+struct scan_disc_result;
+void scanner_dump_state(void);
+int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x,
+ int16_t *ckpt_rel_y, int16_t *back_mm);
+uint8_t strat_scan_disc(int16_t angle, uint8_t mode,
+ struct scan_disc_result *result);
+uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level);
+int16_t strat_get_temple_angle(struct temple *temple);
+int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle);
+uint8_t strat_build_on_opponent_temple(void);
+uint8_t strat_check_temple_and_build(void);
+
+#endif
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "main.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "sensor.h"
+
+#define EDGE_NUMBER 5
+void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt,
+ int16_t radius, int16_t x, int16_t y)
+{
+
+ double c_a, s_a;
+ uint8_t i;
+ double px1, py1, px2, py2;
+ double a_rad;
+
+ a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
+
+ /* generate pentagon */
+ c_a = cos(-2*M_PI/EDGE_NUMBER);
+ s_a = sin(-2*M_PI/EDGE_NUMBER);
+
+ /*
+ px1 = radius;
+ py1 = 0;
+ */
+ px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER));
+ py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER));
+
+
+ for (i = 0; i < EDGE_NUMBER; i++){
+ oa_poly_set_point(pol, x + px1, y + py1, i);
+
+ px2 = px1*c_a + py1*s_a;
+ py2 = -px1*s_a + py1*c_a;
+
+ px1 = px2;
+ py1 = py2;
+ }
+}
+
+void set_rotated_poly(poly_t *pol, const point_t *robot_pt,
+ int16_t w, int16_t l, int16_t x, int16_t y)
+{
+ double tmp_x, tmp_y;
+ double a_rad;
+
+ a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
+
+ DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f",
+ __FUNCTION__, x, y, a_rad);
+
+ /* point 1 */
+ tmp_x = w;
+ tmp_y = l;
+ rotate(&tmp_x, &tmp_y, a_rad);
+ tmp_x += x;
+ tmp_y += y;
+ oa_poly_set_point(pol, tmp_x, tmp_y, 0);
+
+ /* point 2 */
+ tmp_x = -w;
+ tmp_y = l;
+ rotate(&tmp_x, &tmp_y, a_rad);
+ tmp_x += x;
+ tmp_y += y;
+ oa_poly_set_point(pol, tmp_x, tmp_y, 1);
+
+ /* point 3 */
+ tmp_x = -w;
+ tmp_y = -l;
+ rotate(&tmp_x, &tmp_y, a_rad);
+ tmp_x += x;
+ tmp_y += y;
+ oa_poly_set_point(pol, tmp_x, tmp_y, 2);
+
+ /* point 4 */
+ tmp_x = w;
+ tmp_y = -l;
+ rotate(&tmp_x, &tmp_y, a_rad);
+ tmp_x += x;
+ tmp_y += y;
+ oa_poly_set_point(pol, tmp_x, tmp_y, 3);
+}
+
+#define DISC_X CENTER_X
+#define DISC_Y CENTER_Y
+
+void set_central_disc_poly(poly_t *pol, const point_t *robot_pt)
+{
+ set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG,
+ DISC_X, DISC_Y);
+}
+
+#ifdef HOMOLOGATION
+/* /!\ half size */
+#define O_WIDTH 400
+#define O_LENGTH 550
+#else
+/* /!\ half size */
+#define O_WIDTH 360
+#define O_LENGTH 500
+#endif
+
+void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l)
+{
+ int16_t x, y;
+ get_opponent_xy(&x, &y);
+ DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y);
+
+ /* place poly even if invalid, because it's -100 */
+ set_rotated_poly(pol, robot_pt, w, l, x, y);
+}
+
+/* don't care about polygons further than this distance for escape */
+#define ESCAPE_POLY_THRES 1000
+
+/* don't reduce opp if opp is too far */
+#define REDUCE_POLY_THRES 600
+
+/* has to be longer than any poly */
+#define ESCAPE_VECT_LEN 3000
+
+/*
+ * Go in playground, loop until out of poly. The argument robot_pt is
+ * the pointer to the current position of the robot.
+ * Return 0 if there was nothing to do.
+ * Return 1 if we had to move. In this case, update the theorical
+ * position of the robot in robot_pt.
+ */
+static int8_t go_in_area(point_t *robot_pt)
+{
+ point_t poly_pts_area[4];
+ poly_t poly_area;
+ point_t disc_pt, dst_pt;
+
+ disc_pt.x = DISC_X;
+ disc_pt.y = DISC_Y;
+
+ /* Go in playground */
+ if (!is_in_boundingbox(robot_pt)){
+ NOTICE(E_USER_STRAT, "not in playground %"PRIi32", %"PRIi32"",
+ robot_pt->x, robot_pt->y);
+
+ poly_area.l = 4;
+ poly_area.pts = poly_pts_area;
+ poly_pts_area[0].x = strat_infos.area_bbox.x1;
+ poly_pts_area[0].y = strat_infos.area_bbox.y1;
+
+ poly_pts_area[1].x = strat_infos.area_bbox.x2;
+ poly_pts_area[1].y = strat_infos.area_bbox.y1;
+
+ poly_pts_area[2].x = strat_infos.area_bbox.x2;
+ poly_pts_area[2].y = strat_infos.area_bbox.y2;
+
+ poly_pts_area[3].x = strat_infos.area_bbox.x1;
+ poly_pts_area[3].y = strat_infos.area_bbox.y2;
+
+ is_crossing_poly(*robot_pt, disc_pt, &dst_pt, &poly_area);
+ NOTICE(E_USER_STRAT, "pt dst %"PRIi32", %"PRIi32"", dst_pt.x, dst_pt.y);
+
+ strat_goto_xy_force(dst_pt.x, dst_pt.y);
+
+ robot_pt->x = dst_pt.x;
+ robot_pt->y = dst_pt.y;
+
+ NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
+ dst_pt.x, dst_pt.y);
+
+ return 1;
+ }
+
+ return 0;
+}
+
+
+/*
+ * Escape from polygons if needed.
+ * robot_pt is the current position of the robot, it will be
+ * updated.
+ */
+static int8_t escape_from_poly(point_t *robot_pt,
+ poly_t *pol_disc,
+ int16_t opp_x, int16_t opp_y,
+ int16_t opp_w, int16_t opp_l,
+ poly_t *pol_opp)
+{
+ uint8_t in_disc = 0, in_opp = 0;
+ double escape_dx = 0, escape_dy = 0;
+ double disc_dx = 0, disc_dy = 0;
+ double opp_dx = 0, opp_dy = 0;
+ double len;
+ point_t opp_pt, disc_pt, dst_pt;
+ point_t intersect_disc_pt, intersect_opp_pt;
+
+ opp_pt.x = opp_x;
+ opp_pt.y = opp_y;
+ disc_pt.x = DISC_X;
+ disc_pt.y = DISC_Y;
+
+ /* escape from other poly if necessary */
+ if (is_in_poly(robot_pt, pol_disc) == 1)
+ in_disc = 1;
+ if (is_in_poly(robot_pt, pol_opp) == 1)
+ in_opp = 1;
+
+ if (in_disc == 0 && in_opp == 0) {
+ NOTICE(E_USER_STRAT, "no need to escape");
+ return 0;
+ }
+
+ NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp);
+
+ /* process escape vector */
+
+ if (distance_between(robot_pt->x, robot_pt->y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) {
+ disc_dx = robot_pt->x - DISC_X;
+ disc_dy = robot_pt->y - DISC_Y;
+ NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f",
+ disc_dx, disc_dy);
+ len = norm(disc_dx, disc_dy);
+ if (len != 0) {
+ disc_dx /= len;
+ disc_dy /= len;
+ }
+ else {
+ disc_dx = 1.0;
+ disc_dy = 0.0;
+ }
+ escape_dx += disc_dx;
+ escape_dy += disc_dy;
+ }
+
+ if (distance_between(robot_pt->x, robot_pt->y, opp_x, opp_y) < ESCAPE_POLY_THRES) {
+ opp_dx = robot_pt->x - opp_x;
+ opp_dy = robot_pt->y - opp_y;
+ NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f",
+ opp_dx, opp_dy);
+ len = norm(opp_dx, opp_dy);
+ if (len != 0) {
+ opp_dx /= len;
+ opp_dy /= len;
+ }
+ else {
+ opp_dx = 1.0;
+ opp_dy = 0.0;
+ }
+ escape_dx += opp_dx;
+ escape_dy += opp_dy;
+ }
+
+ /* normalize escape vector */
+ len = norm(escape_dx, escape_dy);
+ if (len != 0) {
+ escape_dx /= len;
+ escape_dy /= len;
+ }
+ else {
+ if (pol_disc != NULL) {
+ /* rotate 90° */
+ escape_dx = disc_dy;
+ escape_dy = disc_dx;
+ }
+ else if (pol_opp != NULL) {
+ /* rotate 90° */
+ escape_dx = opp_dy;
+ escape_dy = opp_dx;
+ }
+ else { /* should not happen */
+ opp_dx = 1.0;
+ opp_dy = 0.0;
+ }
+ }
+
+ NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f",
+ escape_dx, escape_dy);
+
+ /* process the correct len of escape vector */
+
+ dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN;
+ dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN;
+
+ NOTICE(E_USER_STRAT, "robot pt %"PRIi32" %"PRIi32,
+ robot_pt->x, robot_pt->y);
+ NOTICE(E_USER_STRAT, "dst point %"PRIi32",%"PRIi32,
+ dst_pt.x, dst_pt.y);
+
+ if (in_disc) {
+ if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt,
+ pol_disc) == 1) {
+ /* we add 2 mm to be sure we are out of th polygon */
+ dst_pt.x = intersect_disc_pt.x + escape_dx * 2;
+ dst_pt.y = intersect_disc_pt.y + escape_dy * 2;
+ if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) {
+
+ if (!is_in_boundingbox(&dst_pt))
+ return -1;
+
+ NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
+ dst_pt.x, dst_pt.y);
+
+ strat_goto_xy_force(dst_pt.x, dst_pt.y);
+
+ robot_pt->x = dst_pt.x;
+ robot_pt->y = dst_pt.y;
+
+ return 0;
+ }
+ }
+ }
+
+ if (in_opp) {
+ if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt,
+ pol_opp) == 1) {
+ /* we add 2 cm to be sure we are out of th polygon */
+ dst_pt.x = intersect_opp_pt.x + escape_dx * 2;
+ dst_pt.y = intersect_opp_pt.y + escape_dy * 2;
+
+ if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) {
+
+ if (!is_in_boundingbox(&dst_pt))
+ return -1;
+
+ NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
+ dst_pt.x, dst_pt.y);
+
+ strat_goto_xy_force(dst_pt.x, dst_pt.y);
+
+ robot_pt->x = dst_pt.x;
+ robot_pt->y = dst_pt.y;
+
+ return 0;
+ }
+ }
+ }
+
+ /* should not happen */
+ return -1;
+}
+
+
+static int8_t __goto_and_avoid(int16_t x, int16_t y,
+ uint8_t flags_intermediate,
+ uint8_t flags_final,
+ uint8_t forward)
+{
+ int8_t len = -1, i;
+ point_t *p;
+ poly_t *pol_disc, *pol_opp;
+ int8_t ret;
+ int16_t opp_w, opp_l, opp_x, opp_y;
+ point_t p_dst, robot_pt;
+
+ DEBUG(E_USER_STRAT, "%s(%d,%d) flags_i=%x flags_f=%x forw=%d",
+ __FUNCTION__, x, y, flags_intermediate, flags_final, forward);
+
+ retry:
+ get_opponent_xy(&opp_x, &opp_y);
+ opp_w = O_WIDTH;
+ opp_l = O_LENGTH;
+
+ robot_pt.x = position_get_x_s16(&mainboard.pos);
+ robot_pt.y = position_get_y_s16(&mainboard.pos);
+
+ oa_init();
+ pol_disc = oa_new_poly(5);
+ set_central_disc_poly(pol_disc, &robot_pt);
+ pol_opp = oa_new_poly(4);
+ set_opponent_poly(pol_opp, &robot_pt, O_WIDTH, O_LENGTH);
+
+ /* If we are not in the limited area, try to go in it. */
+ ret = go_in_area(&robot_pt);
+
+ /* check that destination is valid */
+ p_dst.x = x;
+ p_dst.y = y;
+ if (!is_in_boundingbox(&p_dst)) {
+ NOTICE(E_USER_STRAT, " dst is not in playground");
+ return END_ERROR;
+ }
+ if (is_point_in_poly(pol_disc, x, y)) {
+ NOTICE(E_USER_STRAT, " dst is in disc");
+ return END_ERROR;
+ }
+ if (is_point_in_poly(pol_opp, x, y)) {
+ NOTICE(E_USER_STRAT, " dst is in opp");
+ return END_ERROR;
+ }
+
+ /* now start to avoid */
+ while (opp_w && opp_l) {
+
+ /* robot_pt is not updated if it fails */
+ ret = escape_from_poly(&robot_pt,
+ pol_disc, opp_x, opp_y,
+ opp_w, opp_l, pol_opp);
+ if (ret == 0) {
+ oa_reset();
+ oa_start_end_points(robot_pt.x, robot_pt.y, x, y);
+ /* oa_dump(); */
+
+ len = oa_process();
+ if (len >= 0)
+ break;
+ }
+ if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) {
+ if (opp_w == 0)
+ opp_l /= 2;
+ opp_w /= 2;
+ }
+ else {
+ NOTICE(E_USER_STRAT, "oa_process() returned %d", len);
+ return END_ERROR;
+ }
+
+ NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l);
+ set_opponent_poly(pol_opp, &robot_pt, opp_w, opp_l);
+ }
+
+ p = oa_get_path();
+ for (i=0 ; i<len ; i++) {
+ DEBUG(E_USER_STRAT, "With avoidance %d: x=%"PRIi32" y=%"PRIi32"", i, p->x, p->y);
+
+ if (forward)
+ trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y);
+ else
+ trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y);
+
+ /* no END_NEAR for the last point */
+ if (i == len - 1)
+ ret = wait_traj_end(flags_final);
+ else
+ ret = wait_traj_end(flags_intermediate);
+
+ if (ret == END_BLOCKING) {
+ DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)",
+ __FUNCTION__, x, y);
+ goto retry;
+ }
+ else if (ret == END_OBSTACLE) {
+ /* brake and wait the speed to be slow */
+ DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)",
+ __FUNCTION__, x, y);
+ goto retry;
+ }
+ /* else if it is not END_TRAJ or END_NEAR, return */
+ else if (!TRAJ_SUCCESS(ret)) {
+ return ret;
+ }
+ p++;
+ }
+
+ return END_TRAJ;
+}
+
+/* go forward to a x,y point. use current speed for that */
+uint8_t goto_and_avoid_forward(int16_t x, int16_t y, uint8_t flags_intermediate,
+ uint8_t flags_final)
+{
+ return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1);
+}
+
+/* go backward to a x,y point. use current speed for that */
+uint8_t goto_and_avoid_backward(int16_t x, int16_t y, uint8_t flags_intermediate,
+ uint8_t flags_final)
+{
+ return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0);
+}
+
+/* go to a x,y point. prefer backward but go forward if the point is
+ * near and in front of us */
+uint8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate,
+ uint8_t flags_final)
+{
+ double d,a;
+ abs_xy_to_rel_da(x, y, &d, &a);
+
+ if (d < 300 && a < RAD(90) && a > RAD(-90))
+ return __goto_and_avoid(x, y, flags_intermediate,
+ flags_final, 1);
+ else
+ return __goto_and_avoid(x, y, flags_intermediate,
+ flags_final, 0);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_avoid.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+void set_opponent_poly(poly_t *pol, int16_t w, int16_t l);
+int8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate,
+ uint8_t flags_final);
+int8_t goto_and_avoid_backward(int16_t x, int16_t y,
+ uint8_t flags_intermediate,
+ uint8_t flags_final);
+int8_t goto_and_avoid_forward(int16_t x, int16_t y,
+ uint8_t flags_intermediate,
+ uint8_t flags_final);
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+#include "cmdline.h"
+#include "strat_utils.h"
+#include "strat_base.h"
+#include "strat.h"
+#include "sensor.h"
+#include "i2c_protocol.h"
+
+/* true if we want to interrupt a trajectory */
+static uint8_t traj_intr=0;
+
+/* filled when a END_OBSTACLE is returned */
+struct opponent_obstacle opponent_obstacle;
+
+/* asked speed */
+static volatile int16_t strat_speed_a = SPEED_DIST_FAST;
+static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST;
+static volatile uint16_t strat_limit_speed_a = 0; /* no limit */
+static volatile uint16_t strat_limit_speed_d = 0;
+
+static volatile uint8_t strat_limit_speed_enabled = 1;
+
+
+/* Strings that match the end traj cause */
+/* /!\ keep it sync with stat_base.h */
+const char *err_tab []= {
+ "END_TRAJ",
+ "END_BLOCKING",
+ "END_NEAR",
+ "END_OBSTACLE",
+ "END_ERROR",
+ "END_INTR",
+ "END_TIMER",
+ "END_RESERVED",
+};
+
+/* return string from end traj type num */
+const char *get_err(uint8_t err)
+{
+ uint8_t i;
+ if (err == 0)
+ return "SUCCESS";
+ for (i=0 ; i<8; i++) {
+ if (err & (1 <<i))
+ return err_tab[i];
+ }
+ return "END_UNKNOWN";
+}
+
+void strat_hardstop(void)
+{
+ trajectory_hardstop(&mainboard.traj);
+ pid_reset(&mainboard.angle.pid);
+ pid_reset(&mainboard.distance.pid);
+ bd_reset(&mainboard.angle.bd);
+ bd_reset(&mainboard.distance.bd);
+
+ while ((ABS(mainboard.speed_d) > 200) ||
+ (ABS(mainboard.speed_a) > 200))
+
+ trajectory_hardstop(&mainboard.traj);
+ pid_reset(&mainboard.angle.pid);
+ pid_reset(&mainboard.distance.pid);
+ bd_reset(&mainboard.angle.bd);
+ bd_reset(&mainboard.distance.bd);
+}
+
+/* go to an x,y point without checking for obstacle or blocking. It
+ * should be used for very small dist only. Return END_TRAJ if we
+ * reach destination, or END_BLOCKING if the robot blocked more than 3
+ * times. */
+uint8_t strat_goto_xy_force(int16_t x, int16_t y)
+{
+ uint8_t i, err;
+
+#ifdef HOMOLOGATION
+ int8_t serr;
+ uint8_t hardstop = 0;
+ microseconds us = time_get_us2();
+ int16_t opp_a, opp_d, opp_x, opp_y;
+
+ while (1) {
+ serr = get_opponent_xyda(&opp_x, &opp_y,
+ &opp_d, &opp_a);
+ if (serr == -1)
+ break;
+ if (opp_d < 600)
+ break;
+ if (hardstop == 0) {
+ strat_hardstop();
+ hardstop = 1;
+ }
+ if ((time_get_us2() - us) > 3000000L)
+ break;
+ }
+#endif
+ for (i=0; i<3; i++) {
+ trajectory_goto_xy_abs(&mainboard.traj, x, y);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (TRAJ_SUCCESS(err))
+ return END_TRAJ;
+ if (err == END_BLOCKING) {
+ time_wait_ms(500);
+ strat_hardstop();
+ }
+ }
+ return END_BLOCKING;
+}
+
+/* reset position */
+void strat_reset_pos(int16_t x, int16_t y, int16_t a)
+{
+ int16_t posx = position_get_x_s16(&mainboard.pos);
+ int16_t posy = position_get_y_s16(&mainboard.pos);
+ int16_t posa = position_get_a_deg_s16(&mainboard.pos);
+
+ if (x == DO_NOT_SET_POS)
+ x = posx;
+ if (y == DO_NOT_SET_POS)
+ y = posy;
+ if (a == DO_NOT_SET_POS)
+ a = posa;
+
+ DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
+ x == DO_NOT_SET_POS ? "" : "x",
+ y == DO_NOT_SET_POS ? "" : "y",
+ a == DO_NOT_SET_POS ? "" : "a");
+ position_set(&mainboard.pos, x, y, a);
+ DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
+}
+
+/*
+ * decrease gain on angle PID, and go forward until we reach the
+ * border.
+ */
+uint8_t strat_calib(int16_t dist, uint8_t flags)
+{
+ int32_t p = pid_get_gain_P(&mainboard.angle.pid);
+ int32_t i = pid_get_gain_I(&mainboard.angle.pid);
+ int32_t d = pid_get_gain_D(&mainboard.angle.pid);
+ uint8_t err;
+
+ pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
+ trajectory_d_rel(&mainboard.traj, dist);
+ err = wait_traj_end(flags);
+ pid_set_gains(&mainboard.angle.pid, p, i, d);
+ return err;
+}
+
+/* escape from zone, and don't brake, so we can continue with another
+ * traj */
+uint8_t strat_escape(struct build_zone *zone, uint8_t flags)
+{
+ uint8_t err;
+ uint16_t old_spdd, old_spda;
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000);
+ if (err == 0) {
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -150);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+ }
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ if (zone->flags & ZONE_F_DISC) {
+ trajectory_d_rel(&mainboard.traj, -350);
+ err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags);
+ }
+ else {
+ trajectory_d_rel(&mainboard.traj, -300);
+ err = wait_traj_end(flags);
+ }
+
+ strat_set_speed(old_spdd, old_spda);
+ if (err == 0)
+ return END_NEAR;
+ return err;
+}
+
+static void strat_update_traj_speed(void)
+{
+ uint16_t d, a;
+
+ d = strat_speed_d;
+ if (strat_limit_speed_d && d > strat_limit_speed_d)
+ d = strat_limit_speed_d;
+ a = strat_speed_a;
+ if (strat_limit_speed_a && a > strat_limit_speed_a)
+ a = strat_limit_speed_a;
+
+ trajectory_set_speed(&mainboard.traj, d, a);
+}
+
+void strat_set_speed(uint16_t d, uint16_t a)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ strat_speed_d = d;
+ strat_speed_a = a;
+ strat_update_traj_speed();
+ IRQ_UNLOCK(flags);
+}
+
+void strat_get_speed(uint16_t *d, uint16_t *a)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ *d = strat_speed_d;
+ *a = strat_speed_a;
+ IRQ_UNLOCK(flags);
+}
+
+void strat_limit_speed_enable(void)
+{
+ strat_limit_speed_enabled = 1;
+}
+
+void strat_limit_speed_disable(void)
+{
+ strat_limit_speed_enabled = 0;
+}
+
+/* called periodically */
+void strat_limit_speed(void)
+{
+ uint16_t lim_d = 0, lim_a = 0;
+ int16_t opp_d, opp_a;
+
+ if (strat_limit_speed_enabled == 0)
+ goto update;
+
+ if (get_opponent_da(&opp_d, &opp_a) != 0)
+ goto update;
+
+#ifdef HOMOLOGATION
+ if (opp_d < 600) {
+ lim_d = 150;
+ lim_a = 200;
+ }
+#else
+ if (opp_d < 500) {
+ if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
+ lim_d = SPEED_DIST_VERY_SLOW;
+ lim_a = SPEED_ANGLE_VERY_SLOW;
+ }
+ else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
+ lim_d = SPEED_DIST_VERY_SLOW;
+ lim_a = SPEED_ANGLE_VERY_SLOW;
+ }
+ else {
+ lim_d = SPEED_DIST_SLOW;
+ lim_a = SPEED_ANGLE_VERY_SLOW;
+ }
+ }
+#endif
+ else if (opp_d < 800) {
+ if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
+ lim_d = SPEED_DIST_SLOW;
+ lim_a = SPEED_ANGLE_SLOW;
+ }
+ else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
+ lim_d = SPEED_DIST_SLOW;
+ lim_a = SPEED_ANGLE_SLOW;
+ }
+ }
+
+ update:
+ if (lim_d != strat_limit_speed_d ||
+ lim_a != strat_limit_speed_a) {
+ strat_limit_speed_d = lim_d;
+ strat_limit_speed_a = lim_a;
+ DEBUG(E_USER_STRAT, "new speed limit da=%d,%d", lim_d, lim_a);
+ strat_update_traj_speed();
+ }
+}
+
+/* start the strat */
+void strat_start(void)
+{
+ int8_t i;
+ uint8_t err;
+
+ strat_preinit();
+
+ /* if start sw not plugged */
+ if (sensor_get(S_START_SWITCH)) {
+ printf_P(PSTR("No start switch, press a key or plug it\r\n"));
+
+ /* while start sw not plugged */
+ while (sensor_get(S_START_SWITCH)) {
+ if (! cmdline_keypressed())
+ continue;
+
+ for (i=3; i>0; i--) {
+ printf_P(PSTR("%d\r\n"), i);
+ time_wait_ms(1000);
+ }
+ break;
+ }
+ }
+
+ /* if start sw plugged */
+ if (!sensor_get(S_START_SWITCH)) {
+ printf_P(PSTR("Ready, unplug start switch to start\r\n"));
+ /* while start sw plugged */
+ while (!sensor_get(S_START_SWITCH));
+ }
+
+ strat_init();
+ err = strat_main();
+ NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err));
+ strat_exit();
+}
+
+/* return true if we have to brake due to an obstacle */
+uint8_t strat_obstacle(void)
+{
+ int16_t x_rel, y_rel;
+ int16_t opp_x, opp_y, opp_d, opp_a;
+
+ /* too slow */
+ if (ABS(mainboard.speed_d) < 150)
+ return 0;
+
+ /* no opponent detected */
+ if (get_opponent_xyda(&opp_x, &opp_y,
+ &opp_d, &opp_a))
+ return 0;
+
+ /* save obstacle position */
+ opponent_obstacle.x = opp_x;
+ opponent_obstacle.y = opp_y;
+ opponent_obstacle.d = opp_d;
+ opponent_obstacle.a = opp_a;
+
+ /* sensor are temporarily disabled */
+ if (sensor_obstacle_is_disabled())
+ return 0;
+
+ /* relative position */
+ x_rel = cos(RAD(opp_a)) * (double)opp_d;
+ y_rel = sin(RAD(opp_a)) * (double)opp_d;
+
+ /* opponent too far */
+ if (opp_d > 600)
+ return 0;
+
+ /* opponent is in front of us */
+ if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
+ DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
+ "xrel=%d yrel=%d (speed_d=%d)",
+ opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
+ sensor_obstacle_disable();
+ return 1;
+ }
+ /* opponent is behind us */
+ if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
+ DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
+ opp_d, opp_a, x_rel, y_rel);
+ sensor_obstacle_disable();
+ return 1;
+ }
+
+ return 0;
+}
+
+void interrupt_traj(void)
+{
+ traj_intr = 1;
+}
+
+void interrupt_traj_reset(void)
+{
+ traj_intr = 0;
+}
+
+uint8_t test_traj_end(uint8_t why)
+{
+ uint16_t cur_timer;
+ point_t robot_pt;
+
+ robot_pt.x = position_get_x_s16(&mainboard.pos);
+ robot_pt.y = position_get_y_s16(&mainboard.pos);
+
+ /* trigger an event at 3 sec before the end of the match if we
+ * have balls in the barrel */
+ cur_timer = time_get_s();
+
+ if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) {
+ /* end of match */
+ if (cur_timer >= MATCH_TIME)
+ return END_TIMER;
+ }
+
+ if ((why & END_INTR) && traj_intr) {
+ interrupt_traj_reset();
+ return END_INTR;
+ }
+
+ if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
+ return END_TRAJ;
+
+ /* we are near the destination point (depends on current
+ * speed) AND the robot is in the area bounding box. */
+ if (why & END_NEAR) {
+ int16_t d_near = 100;
+
+ if (mainboard.speed_d > 2000)
+ d_near = 150;
+
+ if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
+ is_in_boundingbox(&robot_pt))
+ return END_NEAR;
+ }
+
+ if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
+ strat_hardstop();
+ return END_BLOCKING;
+ }
+
+ if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) {
+ strat_hardstop();
+ return END_BLOCKING;
+ }
+
+ if ((why & END_OBSTACLE) && strat_obstacle()) {
+ strat_hardstop();
+ return END_OBSTACLE;
+ }
+
+ return 0;
+}
+
+uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
+{
+ uint8_t ret = 0;
+ int16_t opp_x, opp_y, opp_d, opp_a;
+
+ while (ret == 0)
+ ret = test_traj_end(why);
+
+ if (ret == END_OBSTACLE) {
+ if (get_opponent_xyda(&opp_x, &opp_y,
+ &opp_d, &opp_a) == 0)
+ DEBUG(E_USER_STRAT, "Got %s at line %d"
+ " xy=(%d,%d) da=(%d,%d)", get_err(ret),
+ line, opp_x, opp_y, opp_d, opp_a);
+ }
+ else {
+ DEBUG(E_USER_STRAT, "Got %s at line %d",
+ get_err(ret), line);
+ }
+ return ret;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_base.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+/* return values for strats and sub trajs */
+#define END_TRAJ 1 /* traj successful */
+#define END_BLOCKING 2 /* blocking during traj */
+#define END_NEAR 4 /* we are near destination */
+#define END_OBSTACLE 8 /* There is an obstacle in front of us */
+#define END_ERROR 16 /* Cannot do the command */
+#define END_INTR 32 /* interrupted by user */
+#define END_TIMER 64 /* we don't a lot of time */
+#define END_RESERVED 128 /* reserved */
+
+/* only valid after a END_OBSTACLE */
+struct opponent_obstacle {
+ int16_t x;
+ int16_t y;
+ int16_t a;
+ int16_t d;
+};
+extern struct opponent_obstacle opponent_obstacle;
+
+/* stop as fast as possible, without ramp */
+void strat_hardstop(void);
+
+#define DO_NOT_SET_POS -1000
+/* Reset position. If arg == DO_NOT_SET_POS, don't update value for
+ * it. */
+void strat_reset_pos(int16_t x, int16_t y, int16_t a);
+
+/* decrease gain on angle PID, and go forward until we reach the
+ * border. */
+uint8_t strat_calib(int16_t d, uint8_t flags);
+
+/* launch start procedure */
+void strat_start(void);
+
+/* go to an x,y point without checking for obstacle or blocking. It
+ * should be used for very small dist only. Return END_TRAJ if we
+ * reach destination, or END_BLOCKING if the robot blocked more than 3
+ * times. */
+uint8_t strat_goto_xy_force(int16_t x, int16_t y);
+
+/* escape from disc polygon or another zone */
+struct build_zone;
+uint8_t strat_escape(struct build_zone *zone, uint8_t flags);
+
+/* return true if we have to brake due to an obstacle */
+uint8_t strat_obstacle(void);
+
+/* set/get user strat speed */
+void strat_set_speed(uint16_t d, uint16_t a);
+void strat_get_speed(uint16_t *d, uint16_t *a);
+
+/* when user type ctrl-c we can interrupt traj */
+void interrupt_traj(void);
+void interrupt_traj_reset(void);
+
+/* limit speed when we are close of opponent */
+void strat_limit_speed_enable(void);
+void strat_limit_speed_disable(void);
+void strat_limit_speed(void);
+
+/* get name of traj error with its number */
+const char *get_err(uint8_t err);
+
+/* test trajectory end condition */
+uint8_t test_traj_end(uint8_t why);
+
+/* loop until test_traj_end() is true */
+#define wait_traj_end(why) __wait_traj_end_debug(why, __LINE__)
+uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line);
--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_building.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "cmdline.h"
+#include "i2c_protocol.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "strat_avoid.h"
+#include "sensor.h"
+
+
+#define DISC_DIST_NEED_GOTO_AVOID 1000
+#define DISC_DIST_PREPARE_BUILD 700
+#define DISC_DIST_SLOW 500
+
+#define ERROUT(e) do { \
+ err = e; \
+ goto end; \
+ } while(0)
+
+static uint8_t is_ready_for_prepare_build(void)
+{
+ double d, a;
+ if (distance_from_robot(CENTER_X, CENTER_Y) >
+ DISC_DIST_PREPARE_BUILD)
+ return 0;
+ abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
+ if (a < RAD(-30))
+ return 0;
+ if (a > RAD(30))
+ return 0;
+ return 1;
+}
+
+/* go to the nearest place on the disc. Also prepare the arms for
+ * building at the correct level. If level==-1, don't move the
+ * arms. */
+uint8_t strat_goto_disc(int8_t level)
+{
+ uint8_t err;
+ uint16_t old_spdd, old_spda;
+ double d, a, x, y;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ /* workaround for some static cols configurations */
+ if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) {
+ if (time_get_s() > 15)
+ i2c_mechboard_mode_loaded();
+ }
+
+ /* if we are far from the disc, goto backward faster */
+ abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
+ if (d > DISC_DIST_NEED_GOTO_AVOID) {
+ rel_da_to_abs_xy(d - DISC_DIST_PREPARE_BUILD, a, &x, &y);
+ err = goto_and_avoid(x, y, TRAJ_FLAGS_STD,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+ }
+
+#ifdef HOMOLOGATION
+ {
+ int16_t opp_d, opp_a;
+ trajectory_turnto_xy(&mainboard.traj,
+ CENTER_X, CENTER_Y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ time_wait_ms(500);
+
+ err = get_opponent_da(&opp_d, &opp_a);
+ if (err == 0 && opp_d < 600 &&
+ (opp_a > 325 || opp_a < 35))
+ return END_ERROR;
+ }
+#endif
+
+ trajectory_goto_forward_xy_abs(&mainboard.traj,
+ CENTER_X, CENTER_Y);
+ err = WAIT_COND_OR_TRAJ_END(is_ready_for_prepare_build(),
+ TRAJ_FLAGS_NO_NEAR);
+
+ if (err == END_BLOCKING)
+ ERROUT(END_BLOCKING);
+ if (TRAJ_SUCCESS(err)) /* should not reach dest */
+ ERROUT(END_ERROR);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ if (level != -1)
+ i2c_mechboard_mode_prepare_build_both(level);
+
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(CENTER_X,
+ CENTER_Y) < DISC_DIST_SLOW,
+ TRAJ_FLAGS_NO_NEAR);
+
+ if (err == END_BLOCKING)
+ ERROUT(END_BLOCKING);
+ if (TRAJ_SUCCESS(err)) /* should not reach dest */
+ ERROUT(END_ERROR);
+
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ if (err == END_BLOCKING)
+ ERROUT(END_TRAJ);
+ if (TRAJ_SUCCESS(err)) /* should not reach dest */
+ ERROUT(END_ERROR);
+
+ ERROUT(err);
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/* must be called from the checkpoint before zone 1. */
+static uint8_t strat_goto_build_zone1_near(uint8_t level)
+{
+ uint8_t err;
+
+ /* turn to build zone */
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ trajectory_a_abs(&mainboard.traj, COLOR_A(90));
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ /* move forward to reach the build zone */
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
+ i2c_mechboard_mode_prepare_build_both(level);
+ err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
+ if (err == END_BLOCKING) {
+ err = END_TRAJ;
+ }
+
+ DEBUG(E_USER_STRAT, "build zone reached");
+ return err;
+}
+
+/* must be called from the checkpoint before zone 0 */
+static uint8_t strat_goto_build_zone0_near(uint8_t level)
+{
+ uint8_t err;
+#ifdef OLD_STYLE
+ int16_t cur_y, diff_y, dst_y;
+#endif
+
+ /* turn to build zone */
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ trajectory_a_abs(&mainboard.traj, COLOR_A(90));
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+#ifdef OLD_STYLE
+ cur_y = position_get_y_s16(&mainboard.pos);
+ dst_y = COLOR_Y(AREA_Y - (ROBOT_LENGTH/2) - 100);
+ diff_y = ABS(cur_y - dst_y);
+
+ /* move forward to reach the build zone */
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
+ i2c_mechboard_mode_prepare_build_both(level);
+ trajectory_d_rel(&mainboard.traj, diff_y);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (err == END_BLOCKING) { /* not very good for z0 but... */
+ err = END_TRAJ;
+ }
+#else
+ /* move forward to reach the build zone */
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
+ i2c_mechboard_mode_prepare_build_both(level);
+ err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
+ if (err == END_BLOCKING) {
+ err = END_TRAJ;
+ }
+#endif
+
+ DEBUG(E_USER_STRAT, "build zone reached");
+ return err;
+}
+
+/* Go to any build zone: disc, 1a or 1b. Doesn't work with zone 0 for
+ * now... */
+uint8_t strat_goto_build_zone(struct build_zone *zone, uint8_t level)
+{
+ uint8_t err = END_TRAJ;
+ uint16_t old_spdd, old_spda;
+ int16_t checkpoint_x, checkpoint_y;
+ int16_t errx;
+
+ zone->last_try_time = time_get_s();
+
+ if (zone->flags & ZONE_F_DISC)
+ return strat_goto_disc(level);
+
+ DEBUG(E_USER_STRAT, "goto build zone x=%d", zone->checkpoint_x);
+
+ /* workaround for some static cols configurations */
+ if (time_get_s() > 15)
+ i2c_mechboard_mode_loaded();
+
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ checkpoint_x = zone->checkpoint_x;
+ checkpoint_y = COLOR_Y(zone->checkpoint_y);
+ errx = position_get_x_s16(&mainboard.pos) - checkpoint_x;
+
+ /* goto checkpoint if we are too far from it, or if error on x
+ * is too big. */
+ if (distance_from_robot(checkpoint_x, checkpoint_y) > 300 ||
+ ABS(errx) > 15) {
+ err = goto_and_avoid(checkpoint_x, checkpoint_y,
+ TRAJ_FLAGS_STD,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+ }
+
+ if (zone->flags & ZONE_F_ZONE1)
+ err = strat_goto_build_zone1_near(level);
+ else if (zone->flags & ZONE_F_ZONE0)
+ err = strat_goto_build_zone0_near(level);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/* return a free temple structure */
+struct temple *strat_get_free_temple(void)
+{
+ uint8_t i;
+
+ for (i=0; i<MAX_TEMPLE; i++) {
+ if (!(strat_infos.temple_list[i].flags & TEMPLE_F_VALID))
+ return &strat_infos.temple_list[i];
+ }
+ return NULL;
+}
+
+uint8_t strat_can_build_on_temple(struct temple *temple)
+{
+ uint8_t col_l, col_r, max_col, lintel;
+
+ col_l = get_column_count_left();
+ col_r = get_column_count_right();
+ max_col = (col_l > col_r ? col_l : col_r);
+ lintel = (get_lintel_count() > 0);
+
+ if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
+ if ((temple->level_r > 5) &&
+ (temple->flags & TEMPLE_F_ON_DISC))
+ return 0;
+ }
+
+ /* return symetric temples only */
+ if (temple->level_l != temple->level_r)
+ return 0;
+
+ if ((time_get_s() - temple->last_try_time) < TEMPLE_DISABLE_TIME)
+ return 0;
+
+ /* we could do better to work on non-symetric temples */
+ if (temple->level_l + max_col + lintel > 9)
+ return 0;
+
+ if (temple->flags & TEMPLE_F_MONOCOL)
+ return 0;
+
+ /* XXX don't allow to build on opponent temple. For that we
+ * must support the little back_mm. */
+ if (temple->flags & TEMPLE_F_OPPONENT)
+ return 0;
+
+ return 1;
+}
+
+
+/* return the best existing temple for building */
+struct temple *strat_get_best_temple(void)
+{
+ uint8_t i;
+ struct temple *best = NULL;
+ struct temple *temple = NULL;
+
+ for (i=0; i<MAX_TEMPLE; i++) {
+ temple = &strat_infos.temple_list[i];
+
+ if (!(temple->flags & TEMPLE_F_VALID))
+ continue;
+
+ if (strat_can_build_on_temple(temple) == 0)
+ continue;
+
+ if (best == NULL) {
+ best = temple;
+ continue;
+ }
+
+ /* take the higher temple between 'best' and 'temple' */
+ if (best->level_l < temple->level_l)
+ best = temple;
+ }
+
+ DEBUG(E_USER_STRAT, "%s() return %p", __FUNCTION__, best);
+ return best;
+}
+
+/* return the temple we built on the disc if any. If valid == 1, the
+ * temple must be buildable. */
+struct temple *strat_get_our_temple_on_disc(uint8_t valid)
+{
+ uint8_t i;
+ struct temple *temple = NULL;
+
+ if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
+ return NULL;
+ }
+
+ for (i=0; i<MAX_TEMPLE; i++) {
+ temple = &strat_infos.temple_list[i];
+
+ if (!(temple->flags & TEMPLE_F_VALID))
+ continue;
+
+ if (valid == 1 && strat_can_build_on_temple(temple) == 0)
+ continue;
+
+ if (temple->flags & TEMPLE_F_ON_DISC)
+ return temple;
+ }
+ return NULL;
+}
+
+#define COL_MAX(x,y) (((x)>(y)) ? (x) : (y))
+
+#define TIME_FOR_LINTEL 3000L
+#define TIME_FOR_BUILD 0L
+#define TIME_FOR_COL 800L
+#define TIME_MARGIN 2000L
+
+#define CHECKPOINT_DISC_DIST 380
+#define CHECKPOINT_OTHER_DIST 200
+/* Grow a temple. It will update temple list. */
+uint8_t strat_grow_temple(struct temple *temple)
+{
+ double checkpoint_x, checkpoint_y;
+ uint8_t add_level = 0;
+ uint8_t do_lintel = 1;
+ uint8_t col_l, col_r, col_max;
+ uint8_t err;
+ uint16_t timeout;
+
+ /* XXX temple must be symetric */
+ uint8_t level = temple->level_l;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ if (temple->level_l >= 9)
+ return END_ERROR;
+
+ if ( (temple->zone->flags & ZONE_F_ZONE1) ||
+ (temple->zone->flags & ZONE_F_ZONE0) ) {
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, -17);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+
+ col_l = get_column_count_left();
+ col_r = get_column_count_right();
+
+ if (time_get_s() < 75) {
+ /* make temple symetric: if we have 1 col on left and 2 cols
+ * on right, only build one on both sides. */
+ if (col_l > col_r) {
+ col_r = col_l;
+ do_lintel = 0;
+ }
+ if (col_r > col_l) {
+ col_r = col_l;
+ do_lintel = 0;
+ }
+ if (get_lintel_count() == 0)
+ do_lintel = 0;
+ }
+ else if (col_l != col_r)
+ do_lintel = 0;
+
+ if (col_l == 0 || col_r == 0) {
+ if (temple->flags & TEMPLE_F_LINTEL)
+ do_lintel = 0;
+ }
+
+ if (col_l == 0 && col_r == 0 && do_lintel == 0) {
+ DEBUG(E_USER_STRAT, "nothing to do");
+ return END_ERROR;
+ }
+
+ add_level = do_lintel + col_l;
+ while (level + add_level > 9) {
+ if (do_lintel) {
+ do_lintel = 0;
+ add_level = do_lintel + col_l;
+ continue;
+ }
+ /* we know col_r and col_l are > 0 */
+ col_r--;
+ col_l--;
+ }
+
+ col_max = COL_MAX(col_r, col_l);
+
+ /* Reduce nb elts if we don't have time */
+ timeout = (!!col_max) * TIME_FOR_BUILD;
+ timeout += col_max * TIME_FOR_COL;
+ timeout += do_lintel * TIME_FOR_LINTEL;
+ if ((timeout / 1000L) + time_get_s() > 89 && do_lintel) {
+ do_lintel = 0;
+ timeout -= TIME_FOR_LINTEL;
+ }
+ if ((timeout / 1000L) + time_get_s() > 89 && col_max) {
+ if (col_r > 0)
+ col_r--;
+ if (col_l > 0)
+ col_l--;
+ col_max--;
+ timeout -= TIME_FOR_COL;
+ }
+
+ /* take a margin for timeout */
+ timeout += (!!col_max) * TIME_MARGIN;
+
+ if (col_l == 0 && col_r == 0 && do_lintel == 0) {
+ DEBUG(E_USER_STRAT, "nothing to do (2)");
+ return END_ERROR;
+ }
+
+ DEBUG(E_USER_STRAT, "Autobuild: left=%d,%d right=%d,%d lintel=%d",
+ level, col_l, level, col_r, do_lintel);
+
+ i2c_mechboard_mode_autobuild(level, col_l, I2C_AUTOBUILD_DEFAULT_DIST,
+ level, col_r, I2C_AUTOBUILD_DEFAULT_DIST,
+ do_lintel);
+ WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
+ I2C_MECHBOARD_MODE_AUTOBUILD, 100);
+ err = WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
+ I2C_MECHBOARD_MODE_AUTOBUILD, timeout);
+ if (err == 0) {
+ DEBUG(E_USER_STRAT, "timeout building temple (timeout was %d)", timeout);
+ temple->flags = 0; /* remove temple from list */
+ return END_TRAJ;
+ }
+ else
+ DEBUG(E_USER_STRAT, "temple built");
+
+ /* position of the robot when build the new temple */
+ temple->x = position_get_x_s16(&mainboard.pos);
+ temple->y = position_get_y_s16(&mainboard.pos);
+ temple->a = position_get_a_deg_s16(&mainboard.pos);
+
+ /* checkpoint is a bit behind us */
+ if (temple->zone->flags & ZONE_F_DISC) {
+ rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
+ &checkpoint_x, &checkpoint_y);
+ }
+ else {
+ rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
+ &checkpoint_x, &checkpoint_y);
+ }
+ temple->checkpoint_x = checkpoint_x;
+ temple->checkpoint_y = checkpoint_y;
+
+ temple->level_l = level + add_level;
+ temple->dist_l = 0;
+ temple->angle_l = 0;
+
+ temple->level_r = level + add_level;
+ temple->dist_r = 0;
+ temple->angle_r = 0;
+
+ temple->flags = TEMPLE_F_VALID;
+
+ if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
+ temple->flags |= TEMPLE_F_ON_DISC;
+
+ if (do_lintel)
+ temple->flags |= TEMPLE_F_LINTEL;
+
+ /* we must push the temple */
+ if ( ((temple->zone->flags & ZONE_F_ZONE1) ||
+ (temple->zone->flags & ZONE_F_ZONE0)) &&
+ level <= 1) {
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, -100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple(level);
+ time_wait_ms(400);
+ strat_set_speed(200, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+
+ /* Special case for big 3 */
+ if (strat_infos.col_in_boobs) {
+ uint16_t old_spdd, old_spda;
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_SLOW);
+ DEBUG(E_USER_STRAT, "%s() big 3", __FUNCTION__);
+ strat_infos.col_in_boobs = 0;
+ trajectory_d_rel(&mainboard.traj, -120);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
+ I2C_MECHBOARD_MODE_CLEAR);
+ WAIT_COND_OR_TIMEOUT(get_column_count() >= 2, 4000L);
+ i2c_mechboard_mode_prepare_build_both(level + add_level);
+ time_wait_ms(800);
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 120);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ err = strat_grow_temple(temple);
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+ }
+
+ return END_TRAJ;
+}
+
+#define COL_BACK_DIST 70
+#define COL_ANGLE 20
+#define COL_ARM_DIST 220
+
+#define COL_BACK_DIST_ZONE1 35
+#define COL_ARM_DIST_ZONE1 230
+#define COL_ANGLE_ZONE1 19
+
+static uint8_t try_build_col(uint8_t l, uint8_t r,
+ uint8_t lp, uint8_t rp,
+ uint8_t lvl)
+{
+ uint8_t max_lvl = lvl + r + l;
+
+ if (l == 0 && r == 0)
+ return 0;
+ if (lp - l == 2 && rp - r == 0)
+ return 0;
+ if (lp - l == 0 && rp - r == 2)
+ return 0;
+ if (max_lvl > 9)
+ return 0;
+ if (max_lvl == 9 && rp == 2 && r == 1)
+ return 0;
+ return max_lvl;
+}
+
+/* Grow a temple by building a column on it. It will update temple
+ * list. */
+uint8_t strat_grow_temple_column(struct temple *temple)
+{
+ uint16_t old_spdd, old_spda;
+ double checkpoint_x, checkpoint_y;
+ uint8_t add_level = 0;
+ uint8_t col_l, col_r;
+ uint8_t col_l_before, col_r_before;
+ uint8_t err;
+ int16_t a_abs, a;
+ uint8_t level = temple->level_l;
+ uint8_t lvl_ok = 0, col_l_ok = 0, col_r_ok = 0;
+ uint8_t tmp_lvl;
+ int16_t col_arm_dist = COL_ARM_DIST;
+ int16_t col_back_dist = COL_BACK_DIST;
+ int16_t col_angle = COL_ANGLE;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ if (level >= 9)
+ return END_ERROR;
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ if ( (temple->zone->flags & ZONE_F_ZONE1) ||
+ (temple->zone->flags & ZONE_F_ZONE0) ) {
+ if (level == 1)
+ col_arm_dist = COL_ARM_DIST_ZONE1;
+ col_back_dist = COL_BACK_DIST_ZONE1;
+ col_angle = COL_ANGLE_ZONE1;
+ }
+
+ a_abs = position_get_a_deg_s16(&mainboard.pos);
+
+ col_l_before = get_column_count_left();
+ col_r_before = get_column_count_right();
+ col_l = col_l_before;
+ col_r = col_r_before;
+
+ /* check number of cols */
+ for (col_l = 0; col_l < col_l_before + 1; col_l++) {
+ for (col_r = 0; col_r < col_r_before + 1; col_r++) {
+ tmp_lvl = try_build_col(col_l, col_r,
+ col_l_before,
+ col_r_before, level);
+ if (tmp_lvl > lvl_ok) {
+ lvl_ok = tmp_lvl;
+ col_l_ok = col_l;
+ col_r_ok = col_r;
+ }
+ }
+ }
+
+ col_l = col_l_ok;
+ col_r = col_r_ok;
+ add_level = col_l + col_r;
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+
+ if (col_l == 0 && col_r == 0)
+ ERROUT(END_ERROR);
+
+ DEBUG(E_USER_STRAT, "Build col: left=%d right=%d",
+ col_l, col_r);
+
+ i2c_mechboard_mode_prepare_inside_both(level);
+ trajectory_d_rel(&mainboard.traj, -col_back_dist);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* build with left arm */
+ if (col_l) {
+ a = a_abs - col_angle;
+ if (a < -180)
+ a += 360;
+ trajectory_a_abs(&mainboard.traj, a);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ if (time_get_s() > 88)
+ return END_TIMER;
+
+ if (level >= 7 && get_column_count_left() == 2)
+ i2c_mechboard_mode_prepare_build_select(level+1, -1);
+ else
+ i2c_mechboard_mode_prepare_build_select(level, -1);
+ time_wait_ms(200);
+ i2c_mechboard_mode_autobuild(level, col_l, col_arm_dist,
+ 0, 0, col_arm_dist, 0);
+ while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
+
+ if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
+ (col_r == 0) &&
+ (temple->flags & TEMPLE_F_OPPONENT)) {
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple_disc(I2C_LEFT_SIDE);
+ time_wait_ms(500);
+ trajectory_d_rel(&mainboard.traj, 100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+ else if ((level == 1 || level == 0) && (col_r == 0)) {
+ trajectory_d_rel(&mainboard.traj, -100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple(level);
+ time_wait_ms(400);
+ strat_set_speed(200, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 120);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+
+ i2c_mechboard_mode_prepare_inside_select(level+col_l, -1);
+ }
+
+ /* build with right arm */
+ if (col_r) {
+ a = a_abs + col_angle;
+ if (a > 180)
+ a -= 360;
+ trajectory_a_abs(&mainboard.traj, a);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ if (time_get_s() > 88)
+ return END_TIMER;
+
+ if ((level+col_l) >= 7 && get_column_count_right() == 2)
+ i2c_mechboard_mode_prepare_build_select(-1, level + col_l + 1);
+ else
+ i2c_mechboard_mode_prepare_build_select(-1, level + col_l);
+ time_wait_ms(200);
+ i2c_mechboard_mode_autobuild(0, 0, col_arm_dist,
+ level + col_l, col_r,
+ col_arm_dist, 0);
+ while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
+ while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
+
+ if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
+ (temple->flags & TEMPLE_F_OPPONENT)) {
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
+ time_wait_ms(500);
+ trajectory_d_rel(&mainboard.traj, 100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+ else if (level == 1 || level == 0) {
+ trajectory_d_rel(&mainboard.traj, -100);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_mechboard_mode_push_temple(level);
+ time_wait_ms(400);
+ strat_set_speed(200, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 120);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+
+ i2c_mechboard_mode_prepare_inside_select(-1, level + col_l + col_r);
+
+ }
+
+ trajectory_a_abs(&mainboard.traj, a_abs);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* position of the robot when build the new temple */
+ temple->x = position_get_x_s16(&mainboard.pos);
+ temple->y = position_get_y_s16(&mainboard.pos);
+ temple->a = position_get_a_deg_s16(&mainboard.pos);
+
+ /* checkpoint is a bit behind us */
+ if (temple->zone->flags | ZONE_F_DISC) {
+ rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
+ &checkpoint_x, &checkpoint_y);
+ }
+ else {
+ rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
+ &checkpoint_x, &checkpoint_y);
+ }
+ temple->checkpoint_x = checkpoint_x;
+ temple->checkpoint_y = checkpoint_y;
+
+ temple->level_l = level + add_level;
+ temple->dist_l = 0; /* XXX */
+ temple->angle_l = 0;
+
+ temple->level_r = level + add_level;
+ temple->dist_r = 0;
+ temple->angle_r = 0;
+
+ temple->flags = TEMPLE_F_VALID | TEMPLE_F_MONOCOL;
+
+ if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
+ temple->flags |= TEMPLE_F_ON_DISC;
+
+ if ( (temple->zone->flags & ZONE_F_ZONE1) ||
+ (temple->zone->flags & ZONE_F_ZONE0) ) {
+
+ }
+ return END_TRAJ;
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+uint8_t strat_build_new_temple(struct build_zone *zone)
+{
+ struct temple *temple;
+ uint8_t level = zone->level;
+ uint8_t err;
+
+ /* create a dummy temple */
+ temple = strat_get_free_temple();
+ if (!temple)
+ return END_ERROR;
+
+ memset(temple, 0, sizeof(*temple));
+ temple->level_l = level;
+ temple->level_r = level;
+ temple->flags = TEMPLE_F_VALID | TEMPLE_F_LINTEL;
+ temple->zone = zone;
+
+ zone->flags |= ZONE_F_BUSY;
+
+ if (time_get_s() > 50 && time_get_s() < 85 &&
+ get_lintel_count() == 0)
+ err = strat_grow_temple_column(temple);
+ else
+ err = strat_grow_temple(temple);
+
+ if (!TRAJ_SUCCESS(err))
+ temple->flags = 0;
+ return err;
+}
+
+uint8_t strat_goto_temple(struct temple *temple)
+{
+ uint16_t old_spdd, old_spda;
+ uint8_t err;
+
+ DEBUG(E_USER_STRAT, "goto temple %p checkpoint=%d,%d",
+ temple, temple->checkpoint_x, temple->checkpoint_y);
+
+ temple->last_try_time = time_get_s();
+
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ i2c_mechboard_mode_loaded();
+
+ err = goto_and_avoid(temple->checkpoint_x,
+ temple->checkpoint_y,
+ TRAJ_FLAGS_STD,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ err = strat_goto_build_zone(temple->zone, temple->level_r);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ DEBUG(E_USER_STRAT, "zone reached");
+ ERROUT(END_TRAJ);
+
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/* return the best existing temple for building */
+struct build_zone *strat_get_best_zone(void)
+{
+ uint8_t i;
+ struct build_zone *zone = NULL;
+
+ for (i=0; i<MAX_ZONE; i++) {
+ zone = &strat_infos.zone_list[i];
+
+ if (zone->flags & ZONE_F_BUSY)
+ continue;
+ if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME)
+ continue;
+
+ return zone;
+ }
+ return NULL;
+}
--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_column_disp.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "actuator.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_avoid.h"
+#include "strat_utils.h"
+#include "sensor.h"
+#include "i2c_protocol.h"
+
+#define ERROUT(e) do { \
+ err = e; \
+ goto end; \
+ } while(0)
+
+/* distance between the wheel axis and the IR sensor */
+#define IR_SHIFT_DISTANCE_RIGHT 85
+#define IR_SHIFT_DISTANCE_LEFT 95
+
+/* return red or green sensor */
+#define COLOR_IR_SENSOR(left) \
+ ({ \
+ uint8_t __ret = 0; \
+ if (left) \
+ __ret = sensor_get(S_DISP_LEFT); \
+ else \
+ __ret = sensor_get(S_DISP_RIGHT); \
+ \
+ __ret; \
+ }) \
+
+/* eject one col, some error codes are ignored here: we want to be
+ * sure that the column is correctly ejected. */
+uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a)
+{
+ uint8_t err;
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -300);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ i2c_mechboard_mode_eject();
+ time_wait_ms(600);
+ trajectory_a_abs(&mainboard.traj, eject_a);
+
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
+ i2c_mechboard_mode_clear();
+ time_wait_ms(1000);
+ trajectory_a_abs(&mainboard.traj, pickup_a);
+
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ return err;
+}
+
+/* get columns from dispenser. Must be called when the robot is in
+ * front of the dispenser. */
+static uint8_t strat_pickup_col_disp(struct column_dispenser *disp)
+{
+ uint16_t old_spdd, old_spda;
+ int16_t recalib_x, recalib_y;
+ int16_t eject_a, pickup_a;
+ uint8_t err, timeout = 0;
+ int8_t cols_count_before, cols_count_after, cols;
+ microseconds us;
+ uint8_t first_try = 1;
+ uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
+
+ /* XXX set lazy pickup mode */
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ cols_count_before = get_column_count();
+ pickup_a = COLOR_A(disp->pickup_a);
+ eject_a = COLOR_A(disp->eject_a);
+ recalib_x = disp->recalib_x;
+ recalib_y = COLOR_Y(disp->recalib_y);
+
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+
+ /* turn to dispenser */
+ i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
+ trajectory_a_abs(&mainboard.traj, pickup_a);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* go forward until blocking, then go back ~30mm */
+
+ pickup_wheels_on();
+ retry:
+ if (time_get_s() > 86) {
+ DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
+ return END_TIMER;
+ }
+
+ if ((strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE) &&
+ strat_infos.col_in_boobs == 0 &&
+ strat_infos.lazy_pickup_done == 0) {
+ DEBUG(E_USER_STRAT, "%s() mode lazy", __FUNCTION__);
+ pickup_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
+ strat_infos.col_in_boobs = 1;
+ strat_infos.lazy_pickup_done = 1;
+ }
+ else {
+ pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
+ strat_infos.col_in_boobs = 0;
+ }
+
+ if (first_try)
+ i2c_mechboard_mode_lazy_harvest();
+ else
+ i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
+ first_try = 0;
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, 120);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+
+ err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
+
+ trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
+ strat_eject_col(eject_a, pickup_a);
+ goto retry;
+ }
+
+ /* start to pickup with finger / arms */
+
+ DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__);
+
+ if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
+ i2c_mechboard_mode_pickup();
+ else
+ i2c_mechboard_mode_lazy_pickup();
+ WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100);
+ us = time_get_us2();
+ cols = get_column_count();
+ while (get_mechboard_mode() == pickup_mode) {
+ if (get_column_count() != cols) {
+ cols = get_column_count();
+ us = time_get_us2();
+ }
+ if ((get_column_count() - cols_count_before) >= disp->count) {
+ DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__);
+ break;
+ }
+ /* 1 second timeout */
+ if (time_get_us2() - us > 1000000L) {
+ DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__);
+ timeout = 1;
+ break;
+ }
+ }
+
+ /* eject if we found a bad color column */
+
+ if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
+ strat_eject_col(eject_a, pickup_a);
+ goto retry;
+ }
+
+ /* only recalib if it was not a timeout or if we got at least
+ * 2 cols. */
+ if (timeout == 0 || (get_column_count() - cols_count_before >= 2))
+ strat_reset_pos(recalib_x, recalib_y, pickup_a);
+ else {
+ /* else just update x or y depending on disp */
+ if (disp == &strat_infos.c1)
+ strat_reset_pos(recalib_x, DO_NOT_SET_POS,
+ DO_NOT_SET_POS);
+ else
+ strat_reset_pos(recalib_x, DO_NOT_SET_POS,
+ DO_NOT_SET_POS);
+ }
+
+ /* go back */
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -300);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
+
+ /* update dispenser count */
+
+ cols_count_after = get_column_count();
+ cols = cols_count_after - cols_count_before;
+ if (cols > 0) {
+ DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
+ disp->count -= cols;
+ if (disp->count < 0)
+ disp->count = 0;
+ }
+
+ pickup_wheels_off();
+ if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
+ i2c_mechboard_mode_clear();
+ else
+ disp->count -= 2;
+
+ ERROUT(END_TRAJ);
+
+end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/*
+ * Go in front of a dispenser. It will update the dispenser if it is
+ * c2 or c3 if we detect that this dispenser does not exist.
+ */
+uint8_t strat_goto_col_disp(struct column_dispenser **pdisp)
+{
+ uint8_t err;
+ int16_t checkpoint_x, checkpoint_y;
+ int16_t scan_a;
+ uint16_t old_spdd, old_spda, scan_left;
+ int16_t pos1x, pos1y, pos2x, pos2y, pos, dist;
+ int16_t margin_col2, margin_col3;
+ struct column_dispenser *disp = *pdisp;
+
+ if (disp->count <= 0)
+ return END_ERROR;
+
+ if (disp->last_try_time >= time_get_s())
+ return END_ERROR;
+
+ disp->last_try_time = time_get_s();
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
+ I2C_MECHBOARD_MODE_CLEAR);
+
+ /* set some useful variables */
+ checkpoint_x = disp->checkpoint_x;
+ checkpoint_y = COLOR_Y(disp->checkpoint_y);
+ scan_a = COLOR_A(disp->scan_a);
+ scan_left = COLOR_INVERT(disp->scan_left);
+
+ /* goto checkpoint */
+ DEBUG(E_USER_STRAT, "%s(): goto %s (%d,%d) scan_left=%d",
+ __FUNCTION__, disp->name, checkpoint_x,
+ checkpoint_y, scan_left);
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+#if 0
+ /* we have an intermediate checkpoint if we are on our
+ * side. If goto_and_avoid() returns END_ERROR, skip
+ * this checkpoint. */
+ if (position_get_x_s16(&mainboard.pos) < 1500) {
+ err = goto_and_avoid(1000, COLOR_Y(1500),
+ TRAJ_FLAGS_STD,
+ TRAJ_FLAGS_STD);
+ if (!TRAJ_SUCCESS(err) && err != END_ERROR)
+ ERROUT(err);
+ }
+#endif
+ /* go to checkpoint near the dispenser */
+ err = goto_and_avoid(checkpoint_x, checkpoint_y,
+ TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* turn to correct angle to prepare scanning */
+
+ trajectory_a_abs(&mainboard.traj, scan_a);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* scan now */
+
+ DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -1000);
+ err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(scan_left),
+ TRAJ_FLAGS_NO_NEAR);
+ if (err) /* we should not reach end */
+ ERROUT(END_ERROR);
+ pos1x = position_get_x_s16(&mainboard.pos);
+ pos1y = position_get_y_s16(&mainboard.pos);
+
+ err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left),
+ TRAJ_FLAGS_NO_NEAR);
+ if (err)
+ ERROUT(END_ERROR);
+ pos2x = position_get_x_s16(&mainboard.pos);
+ pos2y = position_get_y_s16(&mainboard.pos);
+
+ dist = distance_between(pos1x, pos1y, pos2x, pos2y);
+ DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist);
+
+ if (scan_left)
+ trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2);
+ else
+ trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ if (disp == &strat_infos.c1)
+ ERROUT(END_TRAJ);
+
+ /* mark c2 or c3 as empty... */
+ if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0)
+ ERROUT(END_TRAJ);
+
+ pos = (pos2y + pos1y) / 2;
+ if (scan_a == 90) /* y is decreasing when scanning */
+ pos -= 80;
+ else if (scan_a == -90) /* y is increasing when scanning */
+ pos += 80;
+
+ margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y));
+ margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y));
+
+ if (margin_col3 > margin_col2) {
+ DEBUG(E_USER_STRAT, "%s(): delete disp c3 (scan_pos=%d)", __FUNCTION__, pos);
+ strat_infos.c3.count = 0;
+ *pdisp = &strat_infos.c2;
+ if (strat_infos.c3.last_try_time > strat_infos.c2.last_try_time)
+ strat_infos.c2.last_try_time = strat_infos.c3.last_try_time;
+ }
+ else {
+ DEBUG(E_USER_STRAT, "%s(): delete disp c2 (scan_pos=%d)", __FUNCTION__, pos);
+ strat_infos.c2.count = 0;
+ *pdisp = &strat_infos.c3;
+ if (strat_infos.c2.last_try_time > strat_infos.c3.last_try_time)
+ strat_infos.c3.last_try_time = strat_infos.c2.last_try_time;
+ }
+ ERROUT(END_TRAJ);
+
+end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/* return the best dispenser between the 2 */
+static struct column_dispenser *
+strat_disp_compare(struct column_dispenser *a,
+ struct column_dispenser *b)
+{
+ uint8_t want_cols = 4 - get_column_count();
+
+ DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols);
+
+ /* an empty dispenser is not valid */
+ if (a->count == 0)
+ return b;
+ if (b->count == 0)
+ return a;
+
+ /* try to do a round robin: this is not optimal, but at least
+ * we will try another dispenser when one fails. */
+ if (a->last_try_time < b->last_try_time) {
+ return a;
+ }
+ if (b->last_try_time < a->last_try_time) {
+ return b;
+ }
+
+ /* take the one with the most columns */
+ if (a->count >= want_cols && b->count < want_cols)
+ return a;
+
+ /* take the one with the most columns */
+ if (b->count >= want_cols && a->count < want_cols)
+ return b;
+
+ /* the closer is the better */
+ if (distance_from_robot(a->recalib_x, COLOR_Y(a->recalib_y)) <
+ distance_from_robot(b->recalib_x, COLOR_Y(b->recalib_y))) {
+ return a;
+ }
+ return b;
+}
+
+/* choose the best dispenser */
+static struct column_dispenser *strat_get_best_col_disp(void)
+{
+ struct column_dispenser *disp;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ /* for the first call, use c3 */
+ if (strat_infos.c1.last_try_time == 0 &&
+ strat_infos.c2.last_try_time == 0 &&
+ strat_infos.c3.last_try_time == 0)
+ return &strat_infos.c2; // XXX c3
+
+ DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__);
+
+ /* else compare with standard conditions */
+ disp = strat_disp_compare(&strat_infos.c1, &strat_infos.c2);
+ disp = strat_disp_compare(disp, &strat_infos.c3);
+
+ if (disp->count == 0)
+ return NULL;
+
+ return disp;
+}
+
+/* choose the best dispenser, depending on disp count, distance,
+ * tries, ... and go pickup on it. */
+uint8_t strat_pickup_columns(void)
+{
+ struct column_dispenser *disp;
+ uint8_t err;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+ disp = strat_get_best_col_disp();
+
+ if (disp == NULL) {
+ DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__);
+ return END_ERROR;
+ }
+
+ err = strat_goto_col_disp(&disp);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ err = strat_pickup_col_disp(disp);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ return END_TRAJ;
+}
--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_lintel.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_avoid.h"
+#include "strat_utils.h"
+#include "sensor.h"
+#include "i2c_protocol.h"
+
+#define ERROUT(e) do { \
+ err = e; \
+ goto end; \
+ } while(0)
+
+#define X_PRE_MARGIN 20
+#define X_POST_MARGIN 10
+
+/*
+ * goto lintel disp. Return END_TRAJ if success or if there is nothing
+ * to do. Return END_ERROR if dest cannot be reached, else, it may
+ * return END_OBSTACLE or END_BLOCKING.
+ */
+uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp)
+{
+ uint8_t err, first_try = 1, right_ok, left_ok;
+ uint16_t old_spdd, old_spda;
+ int16_t left_cur, right_cur, a;
+
+ if (disp->count == 0)
+ return END_ERROR;
+
+ if (get_lintel_count() == 2)
+ return END_ERROR;
+
+ if (disp->last_try_time >= time_get_s())
+ return END_ERROR;
+
+ disp->last_try_time = time_get_s();
+
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ DEBUG(E_USER_STRAT, "%s(): goto %s", __FUNCTION__, disp->name);
+ i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
+
+ err = goto_and_avoid_backward(disp->x, COLOR_Y(400),
+ TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ if (time_get_s() > 86) {
+ DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
+ return END_TIMER;
+ }
+
+ i2c_mechboard_mode_prepare_get_lintel();
+ retry:
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
+ if (err == END_BLOCKING) {
+ a = position_get_a_deg_s16(&mainboard.pos);
+ /* only reset pos if angle is not too different */
+ if (ABS(a - COLOR_A(-90)) < 5)
+ strat_reset_pos(DO_NOT_SET_POS,
+ COLOR_Y(ROBOT_LENGTH/2),
+ COLOR_A(-90));
+ }
+ else if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_get_lintel();
+ time_wait_ms(500);
+
+ left_cur = sensor_get_adc(ADC_CSENSE3);
+ left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
+ right_cur = mechboard.pump_right1_current;
+ right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
+
+ DEBUG(E_USER_STRAT, "%s left_ok=%d (%d), right_ok=%d (%d)", __FUNCTION__,
+ left_ok, left_cur, right_ok, right_cur);
+ if (first_try) {
+ if (!right_ok && !left_ok) {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ }
+ /* XXX recalib x ? */
+ else if (right_ok && !left_ok) {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ strat_set_speed(500, 500);
+ trajectory_d_a_rel(&mainboard.traj, -200, 30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ trajectory_d_a_rel(&mainboard.traj, 190, -30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ first_try = 0;
+ goto retry;
+ }
+ else if (!right_ok && left_ok) {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ strat_set_speed(500, 500);
+ trajectory_d_a_rel(&mainboard.traj, -200, -30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ trajectory_d_a_rel(&mainboard.traj, 190, 30);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ first_try = 0;
+ goto retry;
+ }
+ /* else, lintel is ok */
+ else {
+ strat_infos.taken_lintel ++;
+ i2c_mechboard_mode_put_lintel();
+ }
+ }
+ else {
+ if (right_ok && left_ok) {
+ /* lintel is ok */
+ strat_infos.taken_lintel ++;
+ i2c_mechboard_mode_put_lintel();
+ }
+ else {
+ i2c_mechboard_mode_prepare_get_lintel();
+ time_wait_ms(300);
+ }
+ }
+ disp->count--;
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -250);
+ err = wait_traj_end(TRAJ_FLAGS_STD);
+
+ ERROUT(err);
+
+end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/* go pickup lintels on dispensers. Return END_TRAJ on success or if
+
+ * there is nothing to do, else return the error status. */
+uint8_t strat_pickup_lintels(void)
+{
+ uint8_t err;
+
+ if (get_column_count() != 0)
+ return END_ERROR;
+
+ if (strat_infos.l1.count == 0 && strat_infos.l2.count == 0)
+ return END_TRAJ;
+
+ /* skip if it's too early */
+ if (time_get_s() < strat_infos.conf.lintel_min_time)
+ return END_TRAJ;
+
+ /* skip next lintel if we want only one */
+ if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
+ if (strat_infos.taken_lintel)
+ return END_TRAJ;
+ }
+
+ /* don't take lintel now if we already have one and if there
+ * is not much time */
+ if (get_lintel_count() && time_get_s() > 75)
+ return END_TRAJ;
+
+ /* take lintel 1 */
+ err = strat_goto_lintel_disp(&strat_infos.l1);
+ if (!TRAJ_SUCCESS(err) && err != END_ERROR)
+ return err;
+
+ /* skip next lintel if we want only one */
+ if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
+ if (strat_infos.taken_lintel)
+ return END_TRAJ;
+ }
+
+ /* don't take lintel now if we already have one and if there
+ * is not much time */
+ if (get_lintel_count() && time_get_s() > 75)
+ return END_TRAJ;
+
+ /* take lintel 2 */
+ err = strat_goto_lintel_disp(&strat_infos.l2);
+ if (!TRAJ_SUCCESS(err) && err != END_ERROR)
+ return err;
+
+ return END_TRAJ;
+}
--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_scan.c,v 1.2 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "cmdline.h"
+#include "i2c_protocol.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "strat_avoid.h"
+#include "sensor.h"
+
+#define ERROUT(e) do { \
+ err = e; \
+ goto end; \
+ } while(0)
+
+
+void scanner_dump_state(void)
+{
+ uint8_t status;
+
+ printf_P(PSTR("scanner state:\r\n"));
+ status = sensorboard.scan_status;
+
+ printf_P(PSTR(" status=%x: "), sensorboard.scan_status);
+
+ if (status & I2C_SCAN_DONE)
+ printf_P(PSTR("DONE "));
+ else
+ printf_P(PSTR("RUNNING "));
+ if (status & I2C_SCAN_MAX_COLUMN)
+ printf_P(PSTR("OBSTACLE "));
+
+ printf_P(PSTR("\r\n"));
+
+ if (sensorboard.dropzone_h == -1) {
+ printf_P(PSTR("No zone found\r\n"));
+ return;
+ }
+
+ printf_P(PSTR(" column_h=%d\r\n"), sensorboard.dropzone_h);
+ printf_P(PSTR(" column_x=%d\r\n"), sensorboard.dropzone_x);
+ printf_P(PSTR(" column_y=%d\r\n"), sensorboard.dropzone_y);
+}
+
+/* must be larger than the disc poly */
+#define CHECKPOINT_DIST 600
+
+/* go to a specific angle on disc, if level == -1, don't move arms */
+uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level)
+{
+ uint8_t err;
+ uint16_t old_spdd, old_spda;
+ double x, y;
+ uint8_t need_clear = 0;
+
+ DEBUG(E_USER_STRAT, "%s(a_deg=%d, level=%d)", __FUNCTION__,
+ a_deg, level);
+
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ /* workaround for some static cols configurations */
+ if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) {
+ if (time_get_s() > 15)
+ i2c_mechboard_mode_loaded();
+ }
+ /* another workaround for offensive configuration */
+ else {
+ if (strat_infos.i2c_loaded_skipped == 0) {
+ DEBUG(E_USER_STRAT, "%s() need clear");
+ strat_infos.i2c_loaded_skipped = 1;
+ i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
+ I2C_MECHBOARD_MODE_CLEAR);
+ need_clear = 1;
+ }
+ else
+ i2c_mechboard_mode_loaded();
+ }
+
+
+ /* calculate the checkpoint */
+ x = CHECKPOINT_DIST;
+ y = 0;
+ rotate(&x, &y, RAD(a_deg));
+ x += CENTER_X;
+ y += CENTER_Y;
+
+ err = goto_and_avoid(x, y, TRAJ_FLAGS_STD,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* early offensive conf only */
+ if (need_clear) {
+ err = WAIT_COND_OR_TIMEOUT(get_column_count() == 2,
+ 3000L);
+ DEBUG(E_USER_STRAT, "%s() offensive: err=%d", err);
+ if (err == 0) /* timeout */
+ return END_ERROR;
+ }
+ err = strat_goto_disc(level);
+
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+
+}
+
+/* only valid for temple on disc */
+int16_t strat_get_temple_angle(struct temple *temple)
+{
+ int16_t x, y;
+ double a;
+
+ x = temple->x;
+ y = temple->y;
+ x -= CENTER_X;
+ y -= CENTER_Y;
+ a = atan2(y, x);
+ return DEG(a);
+}
+
+#define SCAN_ANGLE_OFFSET (-40)
+int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle)
+{
+ return temple_angle + SCAN_ANGLE_OFFSET;
+}
+
+/* start to scan after this distance */
+#define DIST_START_SCAN 50
+
+/* scan during this distance (includes DIST_START_SCAN) */
+#define DIST_SCAN 430
+
+/* speed of the scan */
+#define SPEED_SCAN 450
+
+/* from scanner point of view */
+#define DISC_CENTER_X 15
+#define DISC_CENTER_Y 13
+
+/* distance of the checkpoint */
+#define CKPT_DST 550.
+
+/* to convert in robot coordinates */
+#define SIDE_OFFSET (ROBOT_WIDTH/2)
+#define DIST_OFFSET (DIST_SCAN - DIST_START_SCAN)
+
+/* center of the disc in robot coordinates */
+#define CENTER_X_SCANNER 166
+#define CENTER_Y_SCANNER 174
+
+/* center of the disc in scanner millimeters coordinates */
+#define CENTER_X_SCANNER2 120
+#define CENTER_Y_SCANNER2 155
+
+/* structure filled by strat_scan_disc() */
+struct scan_disc_result {
+#define SCAN_FAILED 0
+#define SCAN_VALID 1
+ uint8_t status;
+
+#define SCAN_ACTION_BUILD_TEMPLE 0
+#define SCAN_ACTION_BUILD_COL 1
+ uint8_t action;
+
+ uint8_t level;
+};
+
+#define SCAN_MODE_CHECK_TEMPLE 0
+#define SCAN_MODE_SCAN_COL 1
+#define SCAN_MODE_SCAN_TEMPLE 2
+
+int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x,
+ int16_t *ckpt_rel_y, int16_t *back_mm)
+{
+ int16_t center_rel_x, center_rel_y;
+ int16_t col_rel_x, col_rel_y;
+ int16_t col_vect_x, col_vect_y;
+ double col_vect_norm;
+ int16_t ckpt_vect_x, ckpt_vect_y;
+
+ /* do some filtering */
+ if (mode == SCAN_MODE_SCAN_TEMPLE &&
+ sensorboard.dropzone_x > CENTER_X_SCANNER) {
+ DEBUG(E_USER_STRAT, "x too big");
+ return -1;
+ }
+
+ /* process relative pos from robot point of view */
+ center_rel_x = DIST_OFFSET - CENTER_Y_SCANNER;
+ center_rel_y = -(SIDE_OFFSET + CENTER_X_SCANNER);
+
+ col_rel_x = DIST_OFFSET - sensorboard.dropzone_y;
+ col_rel_y = -(SIDE_OFFSET + sensorboard.dropzone_x);
+ DEBUG(E_USER_STRAT, "col_rel = %d,%d", col_rel_x, col_rel_y);
+
+ /* vector from center to column */
+ col_vect_x = col_rel_x - center_rel_x;
+ col_vect_y = col_rel_y - center_rel_y;
+ col_vect_norm = norm(col_vect_x, col_vect_y);
+
+ /* vector from center to ckpt */
+ ckpt_vect_x = (double)(col_vect_x) * CKPT_DST / col_vect_norm;
+ ckpt_vect_y = (double)(col_vect_y) * CKPT_DST / col_vect_norm;
+
+ /* rel pos of ckpt */
+ *ckpt_rel_x = center_rel_x + ckpt_vect_x;
+ *ckpt_rel_y = center_rel_y + ckpt_vect_y;
+
+ /* do some filtering */
+ if (col_vect_norm > 150 || col_vect_norm < 30) {
+ DEBUG(E_USER_STRAT, "bad norm");
+ return -1;
+ }
+
+ if (mode == SCAN_MODE_SCAN_TEMPLE) {
+ if (col_vect_norm > 50) {
+ *back_mm = ABS(col_vect_norm-50);
+ }
+ }
+ return 0;
+}
+
+/*
+ * scan the disc: return END_TRAJ on success (and status in result is
+ * set to SCAN_VALID). In this case, all the scan_disc_result
+ * structure is filled with appropriate parameters. mode can be
+ * 'check' or 'scan_col'. Note that if we do a check_temple, the level
+ * field in structure must be filled first by the caller.
+ */
+uint8_t strat_scan_disc(int16_t angle, uint8_t mode,
+ struct scan_disc_result *result)
+{
+ uint16_t old_spdd, old_spda;
+ uint8_t err, stop_scanner = 0;
+ uint8_t original_mode = mode;
+ int16_t pos1x, pos1y, dist;
+ int16_t back_mm = 0;
+
+ int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
+
+ double center_abs_x, center_abs_y;
+ double ckpt_rel_d, ckpt_rel_a;
+ double ckpt_abs_x, ckpt_abs_y;
+
+ /* mark status as failed for now */
+ result->status = SCAN_FAILED;
+
+ DEBUG(E_USER_STRAT, "%s(angle=%d)", __FUNCTION__, angle);
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ /* go on disc */
+ err = strat_goto_disc_angle(angle, -1);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* wait opponent before scanning */
+ if (strat_infos.conf.wait_opponent > 0) {
+ int16_t opp_x, opp_y, opp_d, opp_a;
+ int8_t err;
+ microseconds us;
+
+ us = time_get_us2();
+ while ((err = get_opponent_xyda(&opp_x, &opp_y,
+ &opp_d, &opp_a)) == 0) {
+ if (opp_d > 600)
+ break;
+ if (opp_a < 180)
+ break;
+
+ if (time_get_us2() - us >= (uint32_t)strat_infos.conf.wait_opponent * 1000000L)
+ return END_ERROR;
+ }
+ }
+
+ /* save absolute position of disc */
+ rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y);
+
+ strat_limit_speed_disable();
+
+ /* go back and prepare to scan */
+ strat_set_speed(1000, 1000);
+ trajectory_d_a_rel(&mainboard.traj, -140, 130);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ /* XXX check that opp is not behind us */
+
+ /* prepare scanner */
+
+ stop_scanner = 1;
+ i2c_sensorboard_scanner_prepare();
+ time_wait_ms(250); /* XXX to remove ? */
+
+ strat_set_speed(SPEED_SCAN, 1000);
+
+ pos1x = position_get_x_s16(&mainboard.pos);
+ pos1y = position_get_y_s16(&mainboard.pos);
+ trajectory_d_rel(&mainboard.traj, -DIST_SCAN);
+
+ while (1) {
+ err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (err != 0)
+ break;
+
+ dist = distance_from_robot(pos1x, pos1y);
+
+ if (dist > DIST_START_SCAN)
+ break;
+
+ if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
+ err = END_ERROR;
+ break;
+ }
+ }
+
+ if (err) {
+ if (TRAJ_SUCCESS(err))
+ err = END_ERROR; /* should not reach end */
+ strat_hardstop();
+ trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ ERROUT(err);
+ }
+
+ /* start the scanner */
+
+ i2c_sensorboard_scanner_start();
+
+ err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
+ TRAJ_FLAGS_NO_NEAR);
+ if (err == 0)
+ err = END_ERROR;
+ if (!TRAJ_SUCCESS(err)) {
+ strat_hardstop();
+ trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
+ wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ ERROUT(err);
+ }
+
+ wait_scan_done(1000);
+
+ i2c_sensorboard_scanner_stop();
+ stop_scanner = 0;
+
+ if (mode == SCAN_MODE_CHECK_TEMPLE) {
+ i2c_sensorboard_scanner_algo_check(result->level,
+ CENTER_X_SCANNER2,
+ CENTER_Y_SCANNER2);
+ i2cproto_wait_update();
+ wait_scan_done(1000);
+ scanner_dump_state();
+
+ if (sensorboard.dropzone_h == -1 &&
+ !(strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)) {
+ DEBUG(E_USER_STRAT, "-- try to build a temple");
+ mode = SCAN_MODE_SCAN_TEMPLE;
+ }
+ else {
+ result->action = SCAN_ACTION_BUILD_TEMPLE;
+ /* level is already set by caller */
+ }
+ }
+
+ if (mode == SCAN_MODE_SCAN_TEMPLE) {
+ i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
+ DISC_CENTER_X,
+ DISC_CENTER_Y);
+ i2cproto_wait_update();
+ wait_scan_done(1000);
+ scanner_dump_state();
+
+ if (sensorboard.dropzone_h == -1 ||
+ strat_scan_get_checkpoint(mode, &ckpt_rel_x,
+ &ckpt_rel_y, &back_mm)) {
+ if (original_mode != SCAN_MODE_CHECK_TEMPLE) {
+ DEBUG(E_USER_STRAT, "-- try to build a column");
+ mode = SCAN_MODE_SCAN_COL;
+ }
+ else {
+ DEBUG(E_USER_STRAT, "-- check failed");
+ }
+ }
+ else {
+ result->action = SCAN_ACTION_BUILD_TEMPLE;
+ result->level = sensorboard.dropzone_h;
+ }
+ }
+
+ if (mode == SCAN_MODE_SCAN_COL) {
+ i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
+ DISC_CENTER_X,
+ DISC_CENTER_Y);
+ i2cproto_wait_update();
+ wait_scan_done(1000);
+ scanner_dump_state();
+
+ if (sensorboard.dropzone_h == -1 ||
+ strat_scan_get_checkpoint(mode, &ckpt_rel_x,
+ &ckpt_rel_y, &back_mm)) {
+ ERROUT(END_ERROR);
+ }
+ else {
+ result->action = SCAN_ACTION_BUILD_COL;
+ result->level = sensorboard.dropzone_h;
+ }
+ }
+
+ if (sensorboard.dropzone_h == -1) {
+ ERROUT(END_ERROR);
+ }
+
+ if (mode == SCAN_MODE_CHECK_TEMPLE) {
+ ckpt_rel_x = 220;
+ ckpt_rel_y = 100;
+ }
+
+ DEBUG(E_USER_STRAT, "rel xy for ckpt is %d,%d", ckpt_rel_x, ckpt_rel_y);
+
+ rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
+ abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
+
+ DEBUG(E_USER_STRAT, "abs ckpt is %2.2f,%2.2f", ckpt_abs_x, ckpt_abs_y);
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ /* intermediate checkpoint for some positions */
+ if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
+ trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+ }
+
+ trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ if (result->action == SCAN_ACTION_BUILD_TEMPLE) {
+ i2c_mechboard_mode_prepare_build_both(result->level);
+ }
+
+ trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ pos1x = position_get_x_s16(&mainboard.pos);
+ pos1y = position_get_y_s16(&mainboard.pos);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_d_rel(&mainboard.traj, 400);
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
+ TRAJ_FLAGS_SMALL_DIST);
+ if (err == 0) {
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
+ trajectory_d_rel(&mainboard.traj, 400);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+ if (TRAJ_SUCCESS(err))
+ err = END_ERROR; /* should not reach end */
+ if (err != END_BLOCKING && !TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ if (back_mm) {
+ trajectory_d_rel(&mainboard.traj, -back_mm);
+ wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ }
+
+ result->status = SCAN_VALID;
+
+ strat_limit_speed_enable();
+ return END_TRAJ;
+
+ end:
+ if (stop_scanner)
+ i2c_sensorboard_scanner_stop();
+ strat_limit_speed_enable();
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+
+}
+
+/* do action according to scanner result. temple argument can be NULL
+ * if it's a new one (from opponent) or it can be our previous
+ * temple. */
+uint8_t strat_scan_do_action(struct scan_disc_result *scan_result,
+ struct temple *temple, struct build_zone *zone)
+{
+ uint8_t err;
+
+ /* remove the temple from the list */
+ if (scan_result->status != SCAN_VALID)
+ return END_ERROR;
+
+ if (temple) {
+ /* we were scanning a temple, remove it */
+ if (scan_result->level != temple->level_l) {
+ temple->flags = 0;
+ temple = NULL;
+ }
+ }
+
+ if (temple == NULL) {
+ temple = strat_get_free_temple();
+ if (temple == NULL)
+ return END_ERROR;
+ memset(temple, 0, sizeof(*temple));
+ temple->level_l = scan_result->level;
+ temple->level_r = scan_result->level;
+ temple->flags = TEMPLE_F_OPPONENT |
+ TEMPLE_F_VALID | TEMPLE_F_LINTEL;
+ temple->zone = zone;
+ }
+ zone->flags |= ZONE_F_BUSY;
+
+ switch (scan_result->action) {
+
+ case SCAN_ACTION_BUILD_COL:
+ err = strat_grow_temple_column(temple);
+ break;
+
+ case SCAN_ACTION_BUILD_TEMPLE:
+ err = strat_grow_temple(temple);
+ break;
+ default:
+ err = END_TRAJ;
+ break;
+ }
+ if (!TRAJ_SUCCESS(err))
+ temple->flags = 0;
+ return err;
+}
+
+uint8_t strat_build_on_opponent_temple(void)
+{
+ struct temple *temple;
+ uint8_t err;
+ struct scan_disc_result scan_result;
+ int16_t temple_angle;
+
+ if (time_get_s() < strat_infos.conf.scan_opp_min_time)
+ return END_TRAJ;
+
+ strat_infos.conf.scan_opp_min_time =
+ time_get_s() + strat_infos.conf.delay_between_opp_scan;
+
+ /* scan on disc only */
+ if (strat_infos.conf.scan_opp_angle == -1) {
+ temple = strat_get_our_temple_on_disc(0);
+
+ /* scan the opposite of our temple if we found
+ * one on disc */
+ if (temple) {
+ temple_angle = strat_get_temple_angle(temple);
+ temple_angle += 180;
+ if (temple_angle > 180)
+ temple_angle -= 360;
+ }
+ /* else scan at 0 deg (opponent side) */
+ else {
+ temple_angle = 0;
+ }
+ }
+ else {
+ /* user specified scan position */
+ temple_angle = strat_infos.conf.delay_between_opp_scan;
+ if (temple_angle > 180)
+ temple_angle -= 360;
+ }
+ temple_angle = strat_temple_angle_to_scan_angle(temple_angle);
+
+
+ err = strat_scan_disc(temple_angle, SCAN_MODE_SCAN_TEMPLE,
+ &scan_result);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ /* XXX on disc only */
+ err = strat_scan_do_action(&scan_result, NULL,
+ &strat_infos.zone_list[0]);
+
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
+ return err;
+}
+
+uint8_t strat_check_temple_and_build(void)
+{
+ struct temple *temple;
+ uint8_t err;
+ struct scan_disc_result scan_result;
+ int16_t temple_angle;
+
+ if (time_get_s() < strat_infos.conf.scan_our_min_time)
+ return END_TRAJ;
+ strat_infos.conf.scan_our_min_time =
+ time_get_s() + strat_infos.conf.delay_between_our_scan;
+
+ /* on disc only, symetric only */
+ temple = strat_get_our_temple_on_disc(1);
+ if (temple == NULL)
+ return END_TRAJ;
+
+ temple_angle = strat_get_temple_angle(temple);
+ temple_angle = strat_temple_angle_to_scan_angle(temple_angle);
+
+ scan_result.level = temple->level_l;
+ err = strat_scan_disc(temple_angle, SCAN_MODE_CHECK_TEMPLE,
+ &scan_result);
+ if (scan_result.status != SCAN_VALID) {
+ temple->flags = 0;
+ temple = NULL;
+ }
+ /* no column after a temple check */
+ else if (scan_result.action == SCAN_ACTION_BUILD_COL &&
+ time_get_s() < 70)
+ err = END_ERROR;
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ err = strat_scan_do_action(&scan_result, temple,
+ temple->zone);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
+ return err;
+}
--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_static_columns.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "strat_avoid.h"
+#include "sensor.h"
+#include "i2c_protocol.h"
+
+#define ERROUT(e) do { \
+ err = e; \
+ goto end; \
+ } while(0)
+
+#define BIG_DIST 5000
+
+/*
+ * must be called from start area.
+ * get 4 static columns and build a temple on the disc
+ */
+uint8_t strat_static_columns(uint8_t configuration)
+{
+ uint8_t err;
+ uint8_t col1_present = 0, col4_present = 0;
+ uint16_t old_spdd, old_spda;
+
+ DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration);
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ /* calibrate scanner */
+ i2c_sensorboard_scanner_calib();
+
+ i2c_mechboard_mode_harvest();
+
+ /* go straight. total distance is less than 5 meters */
+ strat_set_speed(1000, 1000);
+ trajectory_d_rel(&mainboard.traj, BIG_DIST);
+
+ /* when y > 50, break */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ /* turn to 90° abs while going forward */
+ DEBUG(E_USER_STRAT, "turn now");
+ strat_set_speed(1000, 350);
+ trajectory_only_a_abs(&mainboard.traj, COLOR_A(90));
+
+ /* when y > 100, check the presence of column 4 */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
+ col4_present = 1;
+ if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
+ col4_present = 1;
+
+ /* when y > 120, check the presence of column 1 */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
+ col1_present = 1;
+ if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
+ col1_present = 1;
+
+ /* when y > 130, break */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE;
+
+ DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present);
+ DEBUG(E_USER_STRAT, "have %d cols", get_column_count());
+
+ if (configuration == 0) {
+ if (get_column_count() > 2) {
+ configuration = 1;
+ if (col4_present || col1_present) {
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE2_DONE;
+ }
+ else {
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE1_DONE;
+ }
+ }
+
+ /* only 2 colums on the first line */
+ else {
+ /* all other colums are on line 1 */
+ if (col4_present && col1_present) {
+ configuration = 2;
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE2_DONE;
+ }
+
+ /* only 2 columns on line 1, so there are also
+ * 2 on line 2 */
+ else if (col4_present || col1_present) {
+ configuration = 4;
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE2_DONE;
+ }
+
+ /* all other columns are on line 2 */
+ else {
+ configuration = 3;
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE1_DONE;
+ }
+ }
+ }
+
+ strat_infos.s_cols.configuration = configuration;
+ DEBUG(E_USER_STRAT, "use configuration %d", configuration);
+
+ if (configuration == 1) {
+ /* we already got 4 columns, go to the disc directly */
+
+ strat_set_speed(1500, 900);
+ trajectory_only_a_abs(&mainboard.traj, COLOR_A(0));
+ err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD);
+
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else if (configuration == 2 /* go from line 0 to line 1 */) {
+ strat_set_speed(800, 1000);
+ /* relative is needed here */
+ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ strat_set_speed(1000, 600);
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100),
+ TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns
+ on line 2*/) {
+ strat_set_speed(1000, 600);
+ /* relative is needed here */
+ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns
+ on line 2 */) {
+ strat_set_speed(1000, 600);
+ /* relative is needed here */
+ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else {
+ trajectory_stop(&mainboard.traj);
+ }
+
+ ERROUT(END_TRAJ);
+
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/*
+ * get last 2 columns
+ * must be called after the first temple building
+ */
+uint8_t strat_static_columns_pass2(void)
+{
+ uint16_t old_spdd, old_spda;
+ uint8_t side, err, next_mode;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ if (get_color() == I2C_COLOR_RED)
+ side = I2C_RIGHT_SIDE;
+ else
+ side = I2C_LEFT_SIDE;
+
+ if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
+ next_mode = I2C_MECHBOARD_MODE_STORE;
+ else
+ next_mode = I2C_MECHBOARD_MODE_HARVEST;
+
+ switch (strat_infos.s_cols.configuration) {
+
+ /* configuration 1: 4 cols on line 0 */
+ case 1:
+ if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) {
+ /* go on line 2 */
+
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ trajectory_goto_forward_xy_abs(&mainboard.traj,
+ LINE2_X,
+ COLOR_Y(400));
+ err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+ }
+ else {
+ /* go on line 1 */
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+
+ err = goto_and_avoid_forward(LINE1_X,
+ COLOR_Y(400),
+ TRAJ_FLAGS_NO_NEAR,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+ }
+
+ ERROUT(END_TRAJ);
+ break;
+
+ /* configuration 2: 2 cols on line 0,
+ all other colums are on line 1 */
+ case 2:
+ /* go on line 1 */
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+
+ err = goto_and_avoid_forward(COL10_X, COLOR_Y(400),
+ TRAJ_FLAGS_NO_NEAR,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ ERROUT(END_TRAJ);
+ break;
+
+ /* configuration 3: 2 cols on line 0,
+ all other colums are on line 2 */
+ case 3:
+ /* go on line 2 */
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+
+ trajectory_goto_forward_xy_abs(&mainboard.traj,
+ LINE2_X,
+ COLOR_Y(400));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ ERROUT(END_TRAJ);
+ break;
+
+ /* configuration 4: 2 cols on line 0,
+ 2 on line 1, 2 on line 2 */
+ case 4:
+ /* go on line 1 */
+ strat_set_speed(600, 2000);
+ trajectory_d_a_rel(&mainboard.traj, -BIG_DIST,
+ COLOR_A(-135));
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900),
+ TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(2000, 2000);
+ trajectory_d_rel(&mainboard.traj, -BIG_DIST);
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1100),
+ TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ err = goto_and_avoid_forward(LINE1_X,
+ COLOR_Y(400),
+ TRAJ_FLAGS_NO_NEAR,
+ TRAJ_FLAGS_NO_NEAR);
+ ERROUT(END_TRAJ);
+ break;
+
+ default:
+ break;
+ }
+
+ /* should not reach this point */
+ ERROUT(END_ERROR);
+
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_utils.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+#include "strat_utils.h"
+#include "strat.h"
+#include "sensor.h"
+#include "i2c_protocol.h"
+
+/* return the distance between two points */
+int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
+{
+ int32_t x,y;
+ x = (x2-x1);
+ x = x*x;
+ y = (y2-y1);
+ y = y*y;
+ return sqrt(x+y);
+}
+
+/* return the distance to a point in the area */
+int16_t distance_from_robot(int16_t x, int16_t y)
+{
+ return distance_between(position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos), x, y);
+}
+
+/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
+int16_t simple_modulo_360(int16_t a)
+{
+ if (a < -180) {
+ a += 360;
+ }
+ else if (a > 180) {
+ a -= 360;
+ }
+ return a;
+}
+
+/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
+double simple_modulo_2pi(double a)
+{
+ if (a < -M_PI) {
+ a += M_2PI;
+ }
+ else if (a > M_PI) {
+ a -= M_2PI;
+ }
+ return a;
+}
+
+/* return the distance to a point in the area */
+int16_t angle_abs_to_rel(int16_t a_abs)
+{
+ return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
+}
+
+void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
+ double *x_abs, double *y_abs)
+{
+ double x = position_get_x_double(&mainboard.pos);
+ double y = position_get_y_double(&mainboard.pos);
+ double a = position_get_a_rad_double(&mainboard.pos);
+
+ *x_abs = x + d_rel*cos(a+a_rel_rad);
+ *y_abs = y + d_rel*sin(a+a_rel_rad);
+}
+
+double norm(double x, double y)
+{
+ return sqrt(x*x + y*y);
+}
+
+void rel_xy_to_abs_xy(double x_rel, double y_rel,
+ double *x_abs, double *y_abs)
+{
+ double d_rel, a_rel;
+ d_rel = norm(x_rel, y_rel);
+ a_rel = atan2(y_rel, x_rel);
+ rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
+}
+
+/* return an angle between -pi and pi */
+void abs_xy_to_rel_da(double x_abs, double y_abs,
+ double *d_rel, double *a_rel_rad)
+{
+ double x = position_get_x_double(&mainboard.pos);
+ double y = position_get_y_double(&mainboard.pos);
+ double a = position_get_a_rad_double(&mainboard.pos);
+
+ *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
+ if (*a_rel_rad < -M_PI) {
+ *a_rel_rad += M_2PI;
+ }
+ else if (*a_rel_rad > M_PI) {
+ *a_rel_rad -= M_2PI;
+ }
+ *d_rel = norm(x_abs-x, y_abs-y);
+}
+
+void rotate(double *x, double *y, double rot)
+{
+ double l, a;
+
+ l = norm(*x, *y);
+ a = atan2(*y, *x);
+
+ a += rot;
+ *x = l * cos(a);
+ *y = l * sin(a);
+}
+
+/* return true if the point is in area */
+uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
+{
+ if (x < margin)
+ return 0;
+ if (x > (AREA_X - margin))
+ return 0;
+ if (y < margin)
+ return 0;
+ if (y > (AREA_Y - margin))
+ return 0;
+ return 1;
+}
+
+
+/* return true if the point is in area */
+uint8_t robot_is_in_area(int16_t margin)
+{
+ return is_in_area(position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ margin);
+}
+
+/* return true if we are near the disc */
+uint8_t robot_is_near_disc(void)
+{
+ if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG)
+ return 1;
+ return 0;
+}
+
+/* return 1 or 0 depending on which side of a line (y=cste) is the
+ * robot. works in red or green color. */
+uint8_t y_is_more_than(int16_t y)
+{
+ int16_t posy;
+
+ posy = position_get_y_s16(&mainboard.pos);
+ if (mainboard.our_color == I2C_COLOR_RED) {
+ if (posy > y)
+ return 1;
+ else
+ return 0;
+ }
+ else {
+ if (posy < (AREA_Y-y))
+ return 1;
+ else
+ return 0;
+ }
+}
+
+/* return 1 or 0 depending on which side of a line (x=cste) is the
+ * robot. works in red or green color. */
+uint8_t x_is_more_than(int16_t x)
+{
+ int16_t posx;
+
+ posx = position_get_x_s16(&mainboard.pos);
+ if (posx > x)
+ return 1;
+ else
+ return 0;
+}
+
+int16_t sin_table[] = {
+ 0,
+ 3211,
+ 6392,
+ 9512,
+ 12539,
+ 15446,
+ 18204,
+ 20787,
+ 23170,
+ 25330,
+ 27245,
+ 28898,
+ 30273,
+ 31357,
+ 32138,
+ 32610,
+ 32767,
+};
+
+int16_t fast_sin(int16_t deg)
+{
+ deg %= 360;
+
+ if (deg < 0)
+ deg += 360;
+
+ if (deg < 90)
+ return sin_table[(deg*16)/90];
+ else if (deg < 180)
+ return sin_table[((180-deg)*16)/90];
+ else if (deg < 270)
+ return -sin_table[((deg-180)*16)/90];
+ else
+ return -sin_table[((360-deg)*16)/90];
+}
+
+int16_t fast_cos(int16_t deg)
+{
+ return fast_sin(90+deg);
+}
+
+
+/* get the color of our robot */
+uint8_t get_color(void)
+{
+ return mainboard.our_color;
+}
+
+/* get the color of the opponent robot */
+uint8_t get_opponent_color(void)
+{
+ if (mainboard.our_color == I2C_COLOR_RED)
+ return I2C_COLOR_GREEN;
+ else
+ return I2C_COLOR_RED;
+}
+
+/* get the xy pos of the opponent robot */
+int8_t get_opponent_xy(int16_t *x, int16_t *y)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ *x = sensorboard.opponent_x;
+ *y = sensorboard.opponent_y;
+ IRQ_UNLOCK(flags);
+ if (*x == I2C_OPPONENT_NOT_THERE)
+ return -1;
+ return 0;
+}
+
+/* get the da pos of the opponent robot */
+int8_t get_opponent_da(int16_t *d, int16_t *a)
+{
+ uint8_t flags;
+ int16_t x_tmp;
+ IRQ_LOCK(flags);
+ x_tmp = sensorboard.opponent_x;
+ *d = sensorboard.opponent_d;
+ *a = sensorboard.opponent_a;
+ IRQ_UNLOCK(flags);
+ if (x_tmp == I2C_OPPONENT_NOT_THERE)
+ return -1;
+ return 0;
+}
+
+/* get the da pos of the opponent robot */
+int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ *x = sensorboard.opponent_x;
+ *y = sensorboard.opponent_y;
+ *d = sensorboard.opponent_d;
+ *a = sensorboard.opponent_a;
+ IRQ_UNLOCK(flags);
+ if (*x == I2C_OPPONENT_NOT_THERE)
+ return -1;
+ return 0;
+}
+
+uint8_t pump_left1_is_full(void)
+{
+ return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) &&
+ (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN));
+}
+
+uint8_t pump_left2_is_full(void)
+{
+ return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) &&
+ (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN));
+}
+
+uint8_t pump_right1_is_full(void)
+{
+ return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) &&
+ (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN));
+}
+
+uint8_t pump_right2_is_full(void)
+{
+ return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) &&
+ (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN));
+}
+
+/* number of column owned by the robot */
+uint8_t get_column_count_left(void)
+{
+ uint8_t ret = 0;
+ ret += pump_left1_is_full();
+ ret += pump_left2_is_full();
+ return ret;
+}
+
+/* number of column owned by the robot */
+uint8_t get_column_count_right(void)
+{
+ uint8_t ret = 0;
+ ret += pump_right1_is_full();
+ ret += pump_right2_is_full();
+ return ret;
+}
+
+/* number of column owned by the robot */
+uint8_t get_column_count(void)
+{
+ uint8_t ret = 0;
+ ret += pump_left1_is_full();
+ ret += pump_left2_is_full();
+ ret += pump_right1_is_full();
+ ret += pump_right2_is_full();
+ return ret;
+}
+
+uint8_t get_lintel_count(void)
+{
+ return mechboard.lintel_count;
+}
+
+uint8_t get_mechboard_mode(void)
+{
+ return mechboard.mode;
+}
+
+uint8_t get_scanner_status(void)
+{
+ return sensorboard.scan_status;
+}
+
+/* return 0 if timeout, or 1 if cond is true */
+uint8_t wait_scan_done(uint16_t timeout)
+{
+ uint8_t err;
+ err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout);
+ return err;
+}
+
+uint8_t opponent_is_behind(void)
+{
+ int8_t opp_there;
+ int16_t opp_d, opp_a;
+
+ opp_there = get_opponent_da(&opp_d, &opp_a);
+ if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600)
+ return 1;
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_utils.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+
+#define DEG(x) (((double)(x)) * (180.0 / M_PI))
+#define RAD(x) (((double)(x)) * (M_PI / 180.0))
+#define M_2PI (2*M_PI)
+
+struct xy_point {
+ int16_t x;
+ int16_t y;
+};
+
+/* wait traj end flag or cond. return 0 if cond become true, else
+ * return the traj flag */
+#define WAIT_COND_OR_TRAJ_END(cond, mask) \
+ ({ \
+ uint8_t __err = 0; \
+ while ( (! (cond)) && (__err == 0)) { \
+ __err = test_traj_end(TRAJ_FLAGS_NO_NEAR); \
+ } \
+ __err; \
+ }) \
+
+int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
+int16_t distance_from_robot(int16_t x, int16_t y);
+int16_t simple_modulo_360(int16_t a);
+double simple_modulo_2pi(double a);
+int16_t angle_abs_to_rel(int16_t a_abs);
+void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs);
+double norm(double x, double y);
+void rel_xy_to_abs_xy(double x_rel, double y_rel, double *x_abs, double *y_abs);
+void abs_xy_to_rel_da(double x_abs, double y_abs, double *d_rel, double *a_rel_rad);
+void rotate(double *x, double *y, double rot);
+uint8_t is_in_area(int16_t x, int16_t y, int16_t margin);
+uint8_t robot_is_in_area(int16_t margin);
+uint8_t robot_is_near_disc(void);
+uint8_t y_is_more_than(int16_t y);
+uint8_t x_is_more_than(int16_t x);
+int16_t fast_sin(int16_t deg);
+int16_t fast_cos(int16_t deg);
+uint8_t get_color(void);
+uint8_t get_opponent_color(void);
+int8_t get_opponent_xy(int16_t *x, int16_t *y);
+int8_t get_opponent_da(int16_t *d, int16_t *a);
+int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a);
+uint8_t get_column_count(void);
+uint8_t get_column_count_right(void);
+uint8_t get_column_count_left(void);
+uint8_t get_lintel_count(void);
+uint8_t get_mechboard_mode(void);
+uint8_t pump_left1_is_full(void);
+uint8_t pump_left2_is_full(void);
+uint8_t pump_right1_is_full(void);
+uint8_t pump_right2_is_full(void);
+uint8_t get_scanner_status(void);
+uint8_t wait_scan_done(uint16_t timeout);
+uint8_t opponent_is_behind(void);
--- /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-02-20 21:10:01 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-02-20 21:10:01 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:24:33 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 127\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
+\r
+#define UART3_RX_FIFO_SIZE 64\r
+#define UART3_TX_FIFO_SIZE 64\r
+#define UART3_NBITS 8\r
+\r
+#define UART3_PARITY UART_PARTITY_NONE\r
+\r
+#define UART3_STOP_BIT UART_STOP_BITS_1\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_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
+#! /usr/bin/env python
+
+import os,sys,termios,atexit
+import serial
+from select import select
+import cmd
+#import pylab
+from matplotlib import pylab
+from math import *
+
+import numpy
+import shlex
+import time
+import math
+import warnings
+warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
+
+import logging
+log = logging.getLogger("MicrobShell")
+_handler = logging.StreamHandler()
+_handler.setFormatter(logging.Formatter("%(levelname)s: %(message)s"))
+log.addHandler(_handler)
+log.setLevel(1)
+
+MICROB_PATH=os.path.dirname(sys.argv[0])
+
+SPM_PAGE_SIZE = 256
+
+def crc_ccitt_update (crc, data):
+ """crc argument is the previous value of 16 bits crc (the initial
+ value is 0xffff). 'data' is the 8 bits value added to crc. The
+ function returns the new crc value."""
+
+ data ^= (crc & 0xff)
+ data ^= (data << 4)
+ data &= 0xff
+
+ ret = (data << 8) & 0xffff
+ ret |= ((crc >> 8) & 0xff)
+ ret ^= ((data >> 4) & 0xff)
+ ret ^= ((data << 3) & 0xffff)
+ return ret
+
+def do_crc(buf):
+ i = 0
+ crc = 0xffff
+ sum = 0
+ while i < len(buf):
+ crc = crc_ccitt_update(crc, ord(buf[i]))
+ sum += ord(buf[i])
+ i += 1
+ return (crc << 16) + (sum & 0xffff)
+
+def prog_page(ser, addr, buf):
+ """program a page from buf at addr"""
+
+ # switch in program mode
+ ser.flushInput()
+ ser.write('p')
+
+ # send address
+ s = ser.readline()
+ if not s.endswith("addr?\r\n"):
+ print "failed (don't match addr)"
+ return -1
+ ser.write("%x\n"%addr)
+ s = ser.readline()
+ if not s.startswith("ok"):
+ print "failed"
+ return -1
+
+ # fill page with buf data
+ page = [ '\xff' ] * SPM_PAGE_SIZE
+ i = 0
+ while i < SPM_PAGE_SIZE and i < len(buf):
+ page[i] = buf[i]
+ i += 1
+
+ # send data
+ i = 0
+ while i < SPM_PAGE_SIZE:
+ c = page[i]
+ ser.write(c)
+ i += 1
+
+ sys.stdout.write(".")
+ sys.stdout.flush()
+
+ # compare crc
+ avr_crc = int(ser.readline()[0:8], 16)
+
+ crc = do_crc(page)
+ if crc != avr_crc:
+ print "failed: bad crc %x %x"%(crc, avr_crc)
+ ser.write('n')
+ return -1
+
+ ser.write('y')
+ s = ser.readline()
+ if not s.startswith("OK"):
+ print "failed"
+ return -1
+ return 0
+
+def read32(ser, addr):
+ """read a 32 bits value at addr"""
+
+ # switch in program mode
+ ser.flushInput()
+ ser.write('d')
+
+ # send address
+ s = ser.readline()
+ if not s.endswith("addr?\r\n"):
+ print "failed (don't match addr)"
+ return -1
+ ser.write("%x\n"%addr)
+ s = ser.readline()
+ return int(s)
+
+def check_crc(ser, buf, offset, size):
+ """Process the crc of buf, ask for a crc of the flash, and check
+ that value is correct"""
+ if size <= 0:
+ return 0
+
+ # go in CRC mode
+ ser.flushInput()
+ ser.write('c')
+
+ # send addr
+ s = ser.readline()
+ if not s.endswith("addr?\r\n"):
+ print "failed <%s>"%s
+ return -1
+ ser.write("%x\n"%offset)
+
+ # send size
+ s = ser.readline()
+ if not s.startswith("size?"):
+ print "failed"
+ return -1
+ ser.write("%x\n"%size)
+
+ # compare CRC
+ crc = do_crc(buf[offset:offset+size])
+ avr_crc = int(ser.readline()[0:8], 16)
+ if crc != avr_crc:
+ return -1
+ return 0
+
+class SerialLogger:
+ def __init__(self, ser, filein, fileout=None):
+ self.ser = ser
+ self.filein = filein
+ self.fin = open(filein, "a", 0)
+ if fileout:
+ self.fileout = fileout
+ self.fout = open(fileout, "a", 0)
+ else:
+ self.fileout = filein
+ self.fout = self.fin
+ def fileno(self):
+ return self.ser.fileno()
+ def read(self, *args):
+ res = self.ser.read(*args)
+ self.fin.write(res)
+ return res
+ def write(self, s):
+ self.fout.write(s)
+ self.ser.write(s)
+
+
+
+
+
+"""
+fig = figure()
+
+ax = subplot(111)
+
+
+X = 45.
+Y = -10.
+l1 = 9.
+l2 = 21.13
+l3 = 47.14
+
+l_mirror = 249.
+h_mirror = 13.
+
+
+def ang2_a_mirror(b):
+ x2 = X+l1*math.cos(b)
+ y2 = Y+l1*math.sin(b)
+
+ A = (l3**2+x2**2+y2**2-l2**2)/(2*l3)
+
+ DELTA = -(A**2-x2**2-y2**2)
+ B = +math.sqrt(DELTA)
+
+ D = x2**2+y2**2
+
+ c_a = (x2*A+y2*B)/D
+ s_a = -(x2*B-y2*A)/D
+
+ a = math.atan2(s_a, c_a)
+ return x2, y2, c_a, s_a, a
+
+
+def ang2_H_L(l_telemetre, c_a, s_a, a):
+ d = h_mirror*c_a/s_a
+ H = (l_telemetre - l_mirror - d)*math.sin(2*a)
+ L = l_mirror + d + H/math.tan(2*a)
+ return H, L
+
+all_p = []
+for b in xrange(0, 360, 20):
+ b = b*2*math.pi / 360.
+
+ x2, y2, c_a, s_a, a = ang2_a_mirror(b)
+ x1 = l3*c_a
+ y1 = l3*s_a
+
+ px = [0, x1, x2, X]
+ py = [0, y1, y2, Y]
+
+ all_p+=[px, py]
+
+ print math.sqrt((x2-x1)**2+(y2-y1)**2)
+
+ H, L = ang2_H_L(400., c_a, s_a, a)
+ print H, L
+
+ax.plot(*all_p)
+
+show()
+
+"""
+
+
+
+
+class Interp(cmd.Cmd):
+ prompt = "Microb> "
+ def __init__(self, tty, baudrate=57600):
+ cmd.Cmd.__init__(self)
+ self.ser = serial.Serial(tty,baudrate=baudrate)
+ self.escape = "\x01" # C-a
+ self.quitraw = "\x02" # C-b
+ self.serial_logging = False
+ self.default_in_log_file = "/tmp/microb.in.log"
+ self.default_out_log_file = "/tmp/microb.out.log"
+
+ def do_quit(self, args):
+ return True
+
+ def do_log(self, args):
+ """Activate serial logs.
+ log <filename> logs input and output to <filename>
+ log <filein> <fileout> logs input to <filein> and output to <fileout>
+ log logs to /tmp/microb.log or the last used file"""
+
+ if self.serial_logging:
+ log.error("Already logging to %s and %s" % (self.ser.filein,
+ self.ser.fileout))
+ else:
+ self.serial_logging = True
+ files = [os.path.expanduser(x) for x in args.split()]
+ if len(files) == 0:
+ files = [self.default_in_log_file, self.default_out_log_file]
+ elif len(files) == 1:
+ self.default_in_log_file = files[0]
+ self.default_out_log_file = None
+ elif len(files) == 2:
+ self.default_in_log_file = files[0]
+ self.default_out_log_file = files[1]
+ else:
+ print "Can't parse arguments"
+
+ self.ser = SerialLogger(self.ser, *files)
+ log.info("Starting serial logging to %s and %s" % (self.ser.filein,
+ self.ser.fileout))
+
+
+ def do_unlog(self, args):
+ if self.serial_logging:
+ log.info("Stopping serial logging to %s and %s" % (self.ser.filein,
+ self.ser.fileout))
+ self.ser = self.ser.ser
+ self.serial_logging = False
+ else:
+ log.error("No log to stop")
+
+
+ def do_raw(self, args):
+ "Switch to RAW mode"
+ stdin = os.open("/dev/stdin",os.O_RDONLY)
+ stdout = os.open("/dev/stdout",os.O_WRONLY)
+
+ stdin_termios = termios.tcgetattr(stdin)
+ raw_termios = stdin_termios[:]
+
+ try:
+ log.info("Switching to RAW mode")
+
+ # iflag
+ raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT |
+ termios.PARMRK | termios.ISTRIP |
+ termios.INLCR | termios.IGNCR |
+ termios.ICRNL | termios.IXON)
+ # oflag
+ raw_termios[1] &= ~termios.OPOST;
+ # cflag
+ raw_termios[2] &= ~(termios.CSIZE | termios.PARENB);
+ raw_termios[2] |= termios.CS8;
+ # lflag
+ raw_termios[3] &= ~(termios.ECHO | termios.ECHONL |
+ termios.ICANON | termios.ISIG |
+ termios.IEXTEN);
+
+ termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios)
+
+ mode = "normal"
+ while True:
+ ins,outs,errs=select([stdin,self.ser],[],[])
+ for x in ins:
+ if x == stdin:
+ c = os.read(stdin,1)
+ if mode == "escape":
+ mode =="normal"
+ if c == self.escape:
+ self.ser.write(self.escape)
+ elif c == self.quitraw:
+ return
+ else:
+ self.ser.write(self.escape)
+ self.ser.write(c)
+ else:
+ if c == self.escape:
+ mode = "escape"
+ else:
+ self.ser.write(c)
+ elif x == self.ser:
+ os.write(stdout,self.ser.read())
+ finally:
+ termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios)
+ log.info("Back to normal mode")
+
+
+ def do_arm_x(self, args):
+ fsdf
+ my_h = 100
+ my_r = 220
+ my_ang = 90
+
+ self.ser.write("armxy %d %d %d\n"%(my_h, -my_r, my_ang))
+ time.sleep(1)
+
+ for i in xrange(-my_r, my_r, 25):
+ self.ser.write("armxy %d %d %d\n"%(my_h, i, my_ang))
+ self.ser.flushInput()
+
+ time.sleep(0.03)
+
+ def do_arm_y(self, args):
+ my_x = 80
+ my_r = 145
+ my_ang = 0
+ self.ser.write("armxy %d %d %d\n"%(-my_r, my_x, my_ang))
+ time.sleep(1)
+
+ for i in xrange(-my_r, my_r, 25):
+ self.ser.write("armxy %d %d %d\n"%(i, my_x, my_ang))
+ self.ser.flushInput()
+
+ time.sleep(0.03)
+
+ def do_arm_circ(self, args):
+ add_h = 120
+ add_d = 120
+ l = 70
+ for i in xrange(0, 360, 10):
+ x = l*math.cos(i*math.pi/180)
+ y = l*math.sin(i*math.pi/180)
+
+
+ self.ser.write("armxy %d %d 90\n"%(x+add_h, y+add_d))
+ self.ser.flushInput()
+
+ time.sleep(0.05)
+
+ def do_arm_init(self, args):
+ self.arm_h = 130
+ self.arm_v = 130
+ self.mov_max = 20
+
+ self.ser.write("armxy %d %d\n"%(self.arm_h, self.arm_v))
+
+ def arm_py_goto(self, h, v, a):
+ """
+ dh, dv = h-self.arm_h, v-self.arm_v
+ d = math.sqrt(dh**2 + dv**2)
+
+ old_h = self.arm_h
+ old_v = self.arm_v
+
+ mov_todo = int(d/self.mov_max)
+ for i in xrange(1, mov_todo):
+ p_h = dh*i/mov_todo
+ p_v = dv*i/mov_todo
+
+ new_h = old_h+p_h
+ new_v = old_v+p_v
+
+ self.ser.write("armxy %d %d %d\n"%(new_h, new_v, a))
+ self.ser.flushInput()
+ self.arm_h = new_h
+ self.arm_v = new_v
+
+ time.sleep(0.04)
+
+ self.ser.write("armxy %d %d %d\n"%(h, v, a))
+ self.ser.flushInput()
+ """
+
+ self.ser.write("armxy %d %d %d\n"%(h, v, a))
+ self.ser.flushInput()
+
+ time.sleep(0.2)
+
+
+
+ def do_arm_tt(self, args):
+ for i in xrange(2):
+ self.arm_py_goto(80, 80, 200)
+ self.arm_py_goto(80, 200, 200)
+ self.arm_py_goto(200, 200, 200)
+ self.arm_py_goto(200, 80, 200)
+
+ def do_arm_harve(self, args):
+ angl1 = 1
+ angl2 = 100
+ my_time = 0.03
+ self.arm_py_goto(130,130,angl1)
+ self.arm_py_goto(-150,60,angl1)
+ time.sleep(0.1)
+
+ self.ser.write("pwm 1B -3000\n")
+ self.ser.flushInput()
+ time.sleep(0.2)
+
+ self.arm_py_goto(-120,60,angl1)
+ time.sleep(2)
+ self.arm_py_goto(-120,60,angl2)
+ time.sleep(2)
+ self.arm_py_goto(-150,60,angl2)
+ self.ser.write("pwm 3C -3000\n")
+ self.ser.flushInput()
+ time.sleep(0.2)
+ self.arm_py_goto(-130,60,angl2)
+ self.arm_py_goto(0,160,angl2)
+
+ #middle point
+ self.arm_py_goto(-40,200,angl2)
+
+ h = -150
+ d = 210
+
+ self.arm_py_goto(h,d,angl2)
+ time.sleep(.3)
+ self.ser.write("pwm 3C 3000\n")
+ time.sleep(0.1)
+ self.arm_py_goto(h+60,d,angl2)
+ time.sleep(0.1)
+
+ self.arm_py_goto(h+60,d,angl1)
+ time.sleep(0.3)
+ self.arm_py_goto(h+40,d,angl1)
+ time.sleep(0.3)
+ self.arm_py_goto(h+30,d,angl1)
+ time.sleep(0.3)
+ self.ser.write("pwm 1B 3000\n")
+ time.sleep(0.1)
+ self.arm_py_goto(h+70,d,angl1)
+
+ self.ser.write("pwm 1B 0\n")
+ self.ser.write("pwm 3C 0\n")
+
+ self.arm_py_goto(130,130,angl2)
+
+
+
+ def update_graph(self, val):
+ freq = self.sfreq.val
+ self.theta_max = freq*math.pi*2.0
+ self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
+ self.theta = self.theta[:len(self.r)]
+
+ self.myplot.set_xdata(self.theta)
+ draw()
+ """
+ def do_graph(self, args):
+ self.ser.write("pwm 1A 2000\n")
+ time.sleep(0.5)
+ print "sampling..."
+ self.ser.write("sample start\n")
+ while True:
+ l = self.ser.readline()
+ if "dump end" in l:
+ break
+ #time.sleep(2)
+ self.ser.write("pwm 1A 0\n")
+ l = self.ser.readline()
+ l = self.ser.readline()
+
+ print "dumping..."
+ self.ser.write("sample dump\n")
+ vals = []
+ while True:
+ l = self.ser.readline()
+ if l[0] in ['s', 'c', 'a']:
+ continue
+ if l[0] in ['e']:
+ break
+ tokens = [x for x in shlex.shlex(l)]
+ v = int(tokens[0])
+ #v = min(v, 150)
+ vals.append(v)
+ vals.reverse()
+ print "total vals:", len(vals)
+
+ pylab.subplot(111, polar = True)
+ self.r = vals
+ valinit = 5.38
+ #theta_max = 4.8*2.3*pi
+ self.theta_max =valinit*pylab.pi
+ self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
+
+ self.myplot, = pylab.plot(self.theta, self.r)
+
+ #slide bar
+ axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
+ self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
+ self.sfreq.on_changed(self.update_graph)
+
+ pylab.show()
+ """
+
+
+ def do_dump(self, args):
+
+ t = [x for x in shlex.shlex(args)]
+
+ t.reverse()
+ do_img = False
+
+ #send speed,debug=off
+ #self.ser.write("scan_params 500 0\n")
+ #send algo 1 wrkazone 1 cx 15 cy 15
+ self.ser.write("scan_img 1 1 15 15\n")
+
+ print t
+ while len(t):
+ x = t.pop()
+ if x == 'img':
+ do_img = True
+
+ print "dumping..."
+ self.ser.write("sample dump 0 0 400 0\n")
+
+
+
+ while True:
+ l = self.ser.readline()
+
+ if "start dumping" in l:
+ tokens = [x for x in shlex.shlex(l)]
+ num_rows = int(tokens[-1])
+ print "num row: ", num_rows
+ break
+ print l.strip()
+ #scan_stop = time.time()
+ #print "total time:", scan_stop-scan_start
+
+
+ vals = []
+ while True:
+ l = self.ser.readline()
+
+ if l[0] in ['s', 'c', 'a']:
+ continue
+ if l[0] in ['e']:
+ break
+ tokens = [x for x in shlex.shlex(l)]
+ v = int(tokens[0])
+ #v = min(v, 150)
+ vals.append(v)
+
+
+ #vals.reverse()
+ print "total vals:", len(vals)
+ valinit = 5
+
+ #num_rows = int(600/valinit)
+ #num_cols = int(valinit)
+ num_rows_orig = num_rows
+ num_rows *= 1
+ num_cols = len(vals)/num_rows
+
+ data = []
+ pt_num = 0
+ my_min = None
+ my_max = None
+
+ print "dim", num_rows, num_cols
+ print "sav img to pgm"
+ fimg = open("dump.pgm", "wb")
+ fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
+ for i in xrange(num_cols):
+ data.append([])
+ #data[-1].append(0.0)
+
+ for j in xrange(num_rows):
+ if vals[pt_num]>0x10:
+ p = 0
+ else:
+ p=vals[pt_num] * 0x20
+ if (p>0xFF):
+ p = 0xFF
+
+ fimg.write(chr(p))
+ if my_min == None or my_min>p:
+ my_min = p
+ if p!=255 and (my_max == None or my_max<p):
+ my_max = p
+ if p >= 205:
+ p = 0
+ p/=1.
+
+
+
+ data[-1].append(p)
+ pt_num+=1
+ #data[-1].append(1.)
+ fimg.close()
+ print my_min, my_max
+ #print data
+ data = numpy.array(data)
+
+ if do_img:
+ ax = pylab.subplot(111)
+ ax.imshow(data)
+
+
+ #pylab.subplot(111, polar = True)
+ self.r = vals
+ #theta_max = 4.8*2.3*pi
+ self.theta_max =valinit*pylab.pi
+ self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
+
+ """
+ tmp = []
+ for x in data:
+ tmp+=list(x)
+ self.myplot, = pylab.plot(tmp)
+
+
+ """
+ if not do_img :
+ tmpx = []
+ tmpy = []
+ for x in data:
+ tmpy+=list(x)
+ tmpx+=range(len(x))
+ self.myplot, = pylab.plot(tmpx, tmpy)
+
+
+ #slide bar
+ #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
+ #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
+ #self.sfreq.on_changed(self.update_graph)
+
+ pylab.show()
+
+
+ def do_scan_params(self, args):
+ t = [x for x in shlex.shlex(args)]
+
+ if len(t)!=2:
+ return
+ t = [int(x) for x in t]
+ self.ser.write("scan_params %d %d\n"%tuple(t))
+
+ def do_graph(self, args):
+ t = [x for x in shlex.shlex(args)]
+
+ t.reverse()
+ do_img = False
+
+ #send speed,debug=off
+ #self.ser.write("scan_params 500 0\n")
+ #send algo 1 wrkazone 1 cx 15 cy 15
+ self.ser.write("scan_img 1 1 15 15\n")
+
+ print t
+ while len(t):
+ x = t.pop()
+ if x == 'img':
+ do_img = True
+
+
+ scan_start = time.time()
+ print "sampling..."
+
+ self.ser.write("scan_do\n")
+
+ flog = open('log.txt', 'w')
+
+ while True:
+ l = self.ser.readline()
+ flog.write(l)
+
+ if "dump end" in l:
+ break
+ flog.close()
+
+ #time.sleep(2)
+ #self.ser.write("pwm 1A 0\n")
+ #l = self.ser.readline()
+ #l = self.ser.readline()
+
+
+ print "dumping..."
+ self.ser.write("sample dump 0 0 400 0\n")
+
+
+
+ while True:
+ l = self.ser.readline()
+
+ if "start dumping" in l:
+ tokens = [x for x in shlex.shlex(l)]
+ num_rows = int(tokens[-1])
+ print "num row: ", num_rows
+ break
+ print l.strip()
+ scan_stop = time.time()
+ print "total time:", scan_stop-scan_start
+
+
+ vals = []
+ while True:
+ l = self.ser.readline()
+
+ if l[0] in ['s', 'c', 'a']:
+ continue
+ if l[0] in ['e']:
+ break
+ tokens = [x for x in shlex.shlex(l)]
+ v = int(tokens[0])
+ #v = min(v, 150)
+ vals.append(v)
+
+
+ #vals.reverse()
+ print "total vals:", len(vals)
+ valinit = 5
+
+ #num_rows = int(600/valinit)
+ #num_cols = int(valinit)
+ num_rows_orig = num_rows
+ num_rows *= 1
+ num_cols = len(vals)/num_rows
+
+ data = []
+ pt_num = 0
+ my_min = None
+ my_max = None
+
+ print "dim", num_rows, num_cols
+ print "sav img to pgm"
+ fimg = open("dump.pgm", "wb")
+ fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
+ for i in xrange(num_cols):
+ data.append([])
+ #data[-1].append(0.0)
+
+ for j in xrange(num_rows):
+ if vals[pt_num]>0x10:
+ p = 0
+ else:
+ p=vals[pt_num] * 0x20
+ if (p>0xFF):
+ p = 0xFF
+
+ fimg.write(chr(p))
+ if my_min == None or my_min>p:
+ my_min = p
+ if p!=255 and (my_max == None or my_max<p):
+ my_max = p
+ if p >= 205:
+ p = 0
+ p/=1.
+
+
+
+ data[-1].append(p)
+ pt_num+=1
+ #data[-1].append(1.)
+ fimg.close()
+ print my_min, my_max
+ #print data
+ data = numpy.array(data)
+
+ if do_img:
+ ax = pylab.subplot(111)
+ ax.imshow(data)
+
+
+ #pylab.subplot(111, polar = True)
+ self.r = vals
+ #theta_max = 4.8*2.3*pi
+ self.theta_max =valinit*pylab.pi
+ self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
+
+ """
+ tmp = []
+ for x in data:
+ tmp+=list(x)
+ self.myplot, = pylab.plot(tmp)
+
+
+ """
+ if not do_img :
+ tmpx = []
+ tmpy = []
+ for x in data:
+ tmpy+=list(x)
+ tmpx+=range(len(x))
+ self.myplot, = pylab.plot(tmpx, tmpy)
+
+
+ #slide bar
+ #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
+ #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
+ #self.sfreq.on_changed(self.update_graph)
+
+ pylab.show()
+
+
+ def bootloader(self, filename, boardnum):
+ self.ser.write("\n")
+ time.sleep(0.4)
+ self.ser.write("bootloader\n")
+ time.sleep(0.4)
+ self.ser.write("\n")
+
+ print "start programming"
+ self.ser.flushInput()
+ f = open(filename)
+ buf = f.read()
+ addr = 0
+ while addr < len(buf):
+ time.sleep(0.1)
+ if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0:
+ sys.stdout.write("*")
+ sys.stdout.flush()
+ elif prog_page(self.ser, addr,
+ buf[addr:addr+SPM_PAGE_SIZE]) != 0:
+ return
+ addr += SPM_PAGE_SIZE
+ if check_crc(self.ser, buf, 0, len(buf)):
+ print "crc failed"
+ return
+ print "Done."
+ self.ser.write("x")
+ self.do_raw("")
+
+ def do_bootloader(self, args):
+ self.bootloader(args, 0)
+
+ def do_mainboard(self, args):
+ filename = os.path.join(MICROB_PATH, "../mainboard/main.bin")
+ self.bootloader(filename, 1)
+
+ def do_mechboard(self, args):
+ filename = os.path.join(MICROB_PATH, "../mechboard/main.bin")
+ self.bootloader(filename, 2)
+
+ def do_sensorboard(self, args):
+ filename = os.path.join(MICROB_PATH, "../sensorboard/main.bin")
+ self.bootloader(filename, 3)
+
+ def do_toto(self, args):
+ for i in range(10):
+ time.sleep(1)
+ self.ser.write("pwm s3(3C) 200\n")
+ time.sleep(1)
+ self.ser.write("pwm s3(3C) 250\n")
+
+if __name__ == "__main__":
+ try:
+ import readline,atexit
+ except ImportError:
+ pass
+ else:
+ histfile = os.path.join(os.environ["HOME"], ".microb_history")
+ atexit.register(readline.write_history_file, histfile)
+ try:
+ readline.read_history_file(histfile)
+ except IOError:
+ pass
+
+ device = "/dev/ttyS0"
+ if len(sys.argv) > 1:
+ device = sys.argv[1]
+ interp = Interp(device)
+ while 1:
+ try:
+ interp.cmdloop()
+ except KeyboardInterrupt:
+ print
+ except Exception,e:
+ l = str(e).strip()
+ if l:
+ log.exception("%s" % l.splitlines()[-1])
+ continue
+ break
--- /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
--- /dev/null
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+CONFIG_MCU_ATMEGA8=y
+# 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 is not set
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+# CONFIG_MATH_LIB is not set
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+CONFIG_STANDARD_PRINTF=y
+# CONFIG_FORMAT_IHEX is not set
+# CONFIG_FORMAT_SREC is not set
+CONFIG_FORMAT_BINARY=y
+
+#
+# Base modules
+#
+# CONFIG_MODULE_CIRBUF is not set
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+# CONFIG_MODULE_GEOMETRY is not set
+# CONFIG_MODULE_SCHEDULER is not set
+# 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=y
+# CONFIG_MODULE_SCHEDULER_MANUAL is not set
+# CONFIG_MODULE_TIME is not set
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+# CONFIG_MODULE_UART is not set
+# CONFIG_MODULE_UART_9BITS is not set
+# CONFIG_MODULE_UART_CREATE_CONFIG is not set
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+# CONFIG_MODULE_TIMER is not set
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
+# CONFIG_MODULE_ADC is not set
+# CONFIG_MODULE_ADC_CREATE_CONFIG is not set
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+# CONFIG_MODULE_VT100 is not set
+# CONFIG_MODULE_RDLINE is not set
+# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set
+# CONFIG_MODULE_RDLINE_KILL_BUF is not set
+# CONFIG_MODULE_RDLINE_HISTORY is not set
+# CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+# CONFIG_MODULE_PID is not set
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+# CONFIG_MODULE_QUADRAMP is not set
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+# CONFIG_MODULE_DIAGNOSTIC is not set
+# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
+# CONFIG_MODULE_ERROR is not set
+# CONFIG_MODULE_ERROR_CREATE_CONFIG is not set
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
--- /dev/null
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+CONFIG_MCU_ATMEGA8=y
+# 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 is not set
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+# CONFIG_MATH_LIB is not set
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+CONFIG_STANDARD_PRINTF=y
+# CONFIG_FORMAT_IHEX is not set
+# CONFIG_FORMAT_SREC is not set
+CONFIG_FORMAT_BINARY=y
+
+#
+# Base modules
+#
+# CONFIG_MODULE_CIRBUF is not set
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+# CONFIG_MODULE_GEOMETRY is not set
+# CONFIG_MODULE_SCHEDULER is not set
+# 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=y
+# CONFIG_MODULE_SCHEDULER_MANUAL is not set
+# CONFIG_MODULE_TIME is not set
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+# CONFIG_MODULE_UART is not set
+# CONFIG_MODULE_UART_9BITS is not set
+# CONFIG_MODULE_UART_CREATE_CONFIG is not set
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+# CONFIG_MODULE_TIMER is not set
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
+# CONFIG_MODULE_ADC is not set
+# CONFIG_MODULE_ADC_CREATE_CONFIG is not set
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+# CONFIG_MODULE_VT100 is not set
+# CONFIG_MODULE_RDLINE is not set
+# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set
+# CONFIG_MODULE_RDLINE_KILL_BUF is not set
+# CONFIG_MODULE_RDLINE_HISTORY is not set
+# CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+# CONFIG_MODULE_PID is not set
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+# CONFIG_MODULE_QUADRAMP is not set
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+# CONFIG_MODULE_DIAGNOSTIC is not set
+# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
+# CONFIG_MODULE_ERROR is not set
+# CONFIG_MODULE_ERROR_CREATE_CONFIG is not set
+
+#
+# Programmer options
+#
+# CONFIG_AVRDUDE is not set
+CONFIG_AVARICE=y
+
+#
+# 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 = static_beacon
+
+AVERSIVE_DIR = ../../../..
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC = $(TARGET).c
+
+# List Assembler source files here.
+# Make them always end in a capital .S. Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC =
+
+AVRDUDE_DELAY=30
+
+CFLAGS += -W
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
--- /dev/null
+# 1 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c"
+# 1 "/home/zer0/zavr/projects/microb2010/tests/static_beacon//"
+# 1 "<built-in>"
+# 1 "<command-line>"
+# 1 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c"
+# 23 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c"
+# 1 "/home/zer0/zavr/include/aversive.h" 1
+# 31 "/home/zer0/zavr/include/aversive.h"
+# 1 "./autoconf.h" 1
+# 32 "/home/zer0/zavr/include/aversive.h" 2
+
+
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/interrupt.h" 1 3
+# 38 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/interrupt.h" 3
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 1 3
+# 94 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 3
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/sfr_defs.h" 1 3
+# 126 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/sfr_defs.h" 3
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 1 3
+# 37 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 3
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 1 3
+# 121 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3
+typedef int int8_t __attribute__((__mode__(__QI__)));
+typedef unsigned int uint8_t __attribute__((__mode__(__QI__)));
+typedef int int16_t __attribute__ ((__mode__ (__HI__)));
+typedef unsigned int uint16_t __attribute__ ((__mode__ (__HI__)));
+typedef int int32_t __attribute__ ((__mode__ (__SI__)));
+typedef unsigned int uint32_t __attribute__ ((__mode__ (__SI__)));
+
+typedef int int64_t __attribute__((__mode__(__DI__)));
+typedef unsigned int uint64_t __attribute__((__mode__(__DI__)));
+# 142 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3
+typedef int16_t intptr_t;
+
+
+
+
+typedef uint16_t uintptr_t;
+# 159 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3
+typedef int8_t int_least8_t;
+
+
+
+
+typedef uint8_t uint_least8_t;
+
+
+
+
+typedef int16_t int_least16_t;
+
+
+
+
+typedef uint16_t uint_least16_t;
+
+
+
+
+typedef int32_t int_least32_t;
+
+
+
+
+typedef uint32_t uint_least32_t;
+
+
+
+
+
+
+
+typedef int64_t int_least64_t;
+
+
+
+
+
+
+typedef uint64_t uint_least64_t;
+# 213 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3
+typedef int8_t int_fast8_t;
+
+
+
+
+typedef uint8_t uint_fast8_t;
+
+
+
+
+typedef int16_t int_fast16_t;
+
+
+
+
+typedef uint16_t uint_fast16_t;
+
+
+
+
+typedef int32_t int_fast32_t;
+
+
+
+
+typedef uint32_t uint_fast32_t;
+
+
+
+
+
+
+
+typedef int64_t int_fast64_t;
+
+
+
+
+
+
+typedef uint64_t uint_fast64_t;
+# 273 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3
+typedef int64_t intmax_t;
+
+
+
+
+typedef uint64_t uintmax_t;
+# 38 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 2 3
+# 77 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 3
+typedef int32_t int_farptr_t;
+
+
+
+typedef uint32_t uint_farptr_t;
+# 127 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/sfr_defs.h" 2 3
+# 95 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3
+# 229 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 3
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/iom8.h" 1 3
+# 230 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3
+# 317 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 3
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/portpins.h" 1 3
+# 318 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3
+
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/common.h" 1 3
+# 320 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3
+
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/version.h" 1 3
+# 322 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3
+
+
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/fuse.h" 1 3
+# 223 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/fuse.h" 3
+typedef struct
+{
+ unsigned char low;
+ unsigned char high;
+} __fuse_t;
+# 325 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3
+
+
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/lock.h" 1 3
+# 328 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3
+# 39 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/interrupt.h" 2 3
+# 35 "/home/zer0/zavr/include/aversive.h" 2
+
+
+
+# 1 "/home/zer0/zavr/include/aversive/types.h" 1
+# 47 "/home/zer0/zavr/include/aversive/types.h"
+typedef uint8_t u08;
+typedef uint16_t u16;
+typedef uint32_t u32;
+typedef uint64_t u64;
+typedef int8_t s08;
+typedef int16_t s16;
+typedef int32_t s32;
+typedef int64_t s64;
+# 39 "/home/zer0/zavr/include/aversive.h" 2
+# 1 "/home/zer0/zavr/include/aversive/errno.h" 1
+# 33 "/home/zer0/zavr/include/aversive/errno.h"
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/errno.h" 1 3
+# 55 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/errno.h" 3
+extern int errno;
+# 34 "/home/zer0/zavr/include/aversive/errno.h" 2
+# 40 "/home/zer0/zavr/include/aversive.h" 2
+# 1 "/home/zer0/zavr/include/aversive/irq_lock.h" 1
+# 41 "/home/zer0/zavr/include/aversive.h" 2
+# 125 "/home/zer0/zavr/include/aversive.h"
+struct extract32 {
+ union {
+ struct {
+
+ uint8_t u8_0;
+ uint8_t u8_1;
+ uint8_t u8_2;
+ uint8_t u8_3;
+
+
+
+
+
+
+ } __attribute__ ((packed)) u8;
+ struct {
+
+ uint16_t u16_0;
+ uint16_t u16_1;
+
+
+
+
+ } __attribute__ ((packed)) u16;
+ struct {
+
+ uint8_t u8_0;
+ uint16_t u16_mid;
+ uint8_t u8_3;
+
+
+
+
+
+ } __attribute__ ((packed)) u16_b;
+ uint32_t u32;
+ } __attribute__ ((packed)) u;
+} __attribute__ ((packed));
+# 174 "/home/zer0/zavr/include/aversive.h"
+struct extract16 {
+ union {
+ struct {
+
+ uint8_t u8_0;
+ uint8_t u8_1;
+
+
+
+
+ } __attribute__ ((packed)) u8;
+ uint16_t u16;
+ } __attribute__ ((packed)) u;
+} __attribute__ ((packed));
+# 24 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" 2
+# 1 "/home/zer0/zavr/include/aversive/wait.h" 1
+# 45 "/home/zer0/zavr/include/aversive/wait.h"
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 1 3
+# 39 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3
+# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 1 3
+# 65 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 3
+static inline void _delay_loop_1(uint8_t __count) __attribute__((always_inline));
+static inline void _delay_loop_2(uint16_t __count) __attribute__((always_inline));
+# 80 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 3
+void
+_delay_loop_1(uint8_t __count)
+{
+ __asm__ volatile (
+ "1: dec %0" "\n\t"
+ "brne 1b"
+ : "=r" (__count)
+ : "0" (__count)
+ );
+}
+# 102 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 3
+void
+_delay_loop_2(uint16_t __count)
+{
+ __asm__ volatile (
+ "1: sbiw %0,1" "\n\t"
+ "brne 1b"
+ : "=w" (__count)
+ : "0" (__count)
+ );
+}
+# 40 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 2 3
+# 79 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3
+static inline void _delay_us(double __us) __attribute__((always_inline));
+static inline void _delay_ms(double __ms) __attribute__((always_inline));
+# 107 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3
+void
+_delay_us(double __us)
+{
+ uint8_t __ticks;
+ double __tmp = ((((unsigned long)(16000000))) / 3e6) * __us;
+ if (__tmp < 1.0)
+ __ticks = 1;
+ else if (__tmp > 255)
+ {
+ _delay_ms(__us / 1000.0);
+ return;
+ }
+ else
+ __ticks = (uint8_t)__tmp;
+ _delay_loop_1(__ticks);
+}
+# 141 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3
+void
+_delay_ms(double __ms)
+{
+ uint16_t __ticks;
+ double __tmp = ((((unsigned long)(16000000))) / 4e3) * __ms;
+ if (__tmp < 1.0)
+ __ticks = 1;
+ else if (__tmp > 65535)
+ {
+
+ __ticks = (uint16_t) (__ms * 10.0);
+ while(__ticks)
+ {
+
+ _delay_loop_2(((((unsigned long)(16000000))) / 4e3) / 10);
+ __ticks --;
+ }
+ return;
+ }
+ else
+ __ticks = (uint16_t)__tmp;
+ _delay_loop_2(__ticks);
+}
+# 46 "/home/zer0/zavr/include/aversive/wait.h" 2
+# 59 "/home/zer0/zavr/include/aversive/wait.h"
+static inline void wait_ms(uint16_t n)
+{
+ while ( n -- )
+ _delay_loop_2(((unsigned long)(16000000))/4000);
+}
+# 25 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" 2
+# 100 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c"
+static inline void xmit_0(void)
+{
+ uint8_t t = (((17 + 17) * 10) / 3);
+ (*(volatile uint8_t *)((0x2E) + 0x20)) = 0;
+ (*(volatile uint8_t *)((0x2F) + 0x20)) = 0;
+ _delay_loop_1(t);
+}
+
+static inline void xmit_1(void)
+{
+ uint8_t t = (((17 + 17) * 10) / 3);
+ (*(volatile uint8_t *)((0x2E) + 0x20)) = (1 << (4)) | (1 << (3));
+ (*(volatile uint16_t *)((0x2C) + 0x20)) = (17 + 17)-1;
+ (*(volatile uint8_t *)((0x2F) + 0x20)) = (1 << (7)) | (1 << (1));
+ (*(volatile uint8_t *)((0x2E) + 0x20)) = (1 << (4)) | (1 << (3)) | (1 << (0));
+ _delay_loop_1(t);
+}
+
+
+
+static inline void xmit_manchester_0(void)
+{
+ xmit_0();
+ xmit_1();
+}
+
+static inline void xmit_manchester_1(void)
+{
+ xmit_1();
+ xmit_0();
+}
+
+
+
+static void xmit_bits(uint8_t *buf, uint8_t nbit)
+{
+ uint8_t i;
+ uint8_t byte = *buf;
+
+ for (i=0; i<nbit; i++) {
+ if (byte & 1)
+ xmit_manchester_1();
+ else
+ xmit_manchester_0();
+
+
+ byte >>= 1;
+
+
+ if (((i & 0x07) == 0) && (i != 0))
+ byte = *(++buf);
+ }
+ xmit_0();
+}
+
+
+static inline int8_t wait_laser(void)
+{
+ uint8_t photos;
+ uint8_t time;
+
+
+
+ while (((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1)))) != ((1 << (0)) | (1 << (1))));
+
+
+ while ((photos = ((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1))))) == ((1 << (0)) | (1 << (1))));
+
+ if (photos != ((1 << (1))))
+ return -1;
+ time = (*(volatile uint8_t *)((0x32) + 0x20));
+ ( (*(volatile uint8_t *)((0x12) + 0x20)) |= (1 << (5)));
+
+
+ while ((photos = ((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1))))) == ((1 << (1)))) {
+ if ((*(volatile uint8_t *)((0x32) + 0x20)) - time > ((uint8_t)25))
+ return -1;
+ }
+ if (photos != ((1 << (0)) | (1 << (1))))
+ return -1;
+
+
+
+ while ((photos = ((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1))))) == ((1 << (0)) | (1 << (1)))) {
+ if ((*(volatile uint8_t *)((0x32) + 0x20)) - time > ((uint8_t)250))
+ return -1;
+ }
+ ( (*(volatile uint8_t *)((0x12) + 0x20)) |= (1 << (6)));
+ if ((*(volatile uint8_t *)((0x32) + 0x20)) - time < ((uint8_t)0))
+ return -1;
+ if (photos != ((1 << (0))))
+ return -1;
+# 206 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c"
+ return 0;
+}
+
+
+
+int main(void)
+{
+
+ uint8_t frame = 0x0B;
+ int8_t ret;
+
+
+ (*(volatile uint8_t *)((0x11) + 0x20)) = (1 << (5)) | (1 << (6)) | (1 << (7));
+ (*(volatile uint8_t *)((0x17) + 0x20)) |= (1 << (1));
+# 240 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c"
+ (*(volatile uint16_t *)((0x26) + 0x20)) = (17 + 17);
+ (*(volatile uint16_t *)((0x2A) + 0x20)) = 17;
+ (*(volatile uint8_t *)((0x2F) + 0x20)) = (1 << (7)) | (1 << (1));
+ (*(volatile uint8_t *)((0x2E) + 0x20)) = (1 << (4)) | (1 << (3));
+
+
+ (*(volatile uint8_t *)((0x33) + 0x20)) = (1 << (1)) | (1 << (0));
+
+ while (1) {
+
+
+ ret = wait_laser();
+
+ ( (*(volatile uint8_t *)((0x12) + 0x20)) &= ~ (1 << (5)));
+ ( (*(volatile uint8_t *)((0x12) + 0x20)) &= ~ (1 << (6)));
+
+ if (ret)
+ continue;
+
+
+
+ ( (*(volatile uint8_t *)((0x12) + 0x20)) |= (1 << (7)));
+
+ xmit_bits(&frame, 4);
+
+
+ wait_ms(10);
+ ( (*(volatile uint8_t *)((0x12) + 0x20)) &= ~ (1 << (7)));
+
+
+
+
+
+ }
+ return 0;
+}
--- /dev/null
+:01000000C936
+:00000001FF
--- /dev/null
+:01000000FF00
+:00000001FF
--- /dev/null
+\12À,À+À*À)À(À'À&À%À$À#À"À!À À\1fÀ\1eÀ\1dÀ\1cÀ\eÀ\11$\1f¾ÏåÔàÞ¿Í¿\10à æ°àäéñà\ 2À\ 5\90\r\92 6±\aÙ÷\10à æ°à\ 1À\1d\92 6±\aá÷\ 2Ð\9aÀÑÏß\92ï\92ÿ\92\ f\93\1f\93ß\93Ï\93\ f\92Í·Þ·\8bà\89\83\80î\81»¹\9a\82â\90à\97½\86½\81á\90à\9b½\8a½\82è\8f½\88á\8e½\83à\83¿\8e\ 1\ f_\1fOQç(áÒ.aâpà\92èé.\89áø. ê¿à\83³\83p\830á÷\83³\83p\830áó\820ùô"·\95\9a\ 4À\82·\82\e\8a1Àô\83³\83p\820Áó\830\91ô\ 4À\82·\82\e\8b?hô\93³\93p\930Áó\96\9a\82·\82\e\8c0 ð\910\11ô\80à\ 1À\8fï\95\98\96\98\88#\91ö\97\9aKàø\ 1 à0à@ÿ\vÀÞ¼}½l½ï¼þ¼\85/\8a\95ñ÷\1e¼\1f¼
+À\1e¼\1f¼\85/\8a\95ñ÷Þ¼}½l½ï¼þ¼\85/\8a\95ñ÷F\95É\ 1\87p\90p\89+!ô"#\11ð1\96@\81/_?O$01\ 5¹ö\1e¼\1f¼\85/\8a\95ñ÷\8aà\90à\ 3Àý\ 11\97ñ÷\ 1\97/ï\8f?\92\aÁ÷\97\98\92Ïø\94ÿÏ
\ No newline at end of file
--- /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: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/wait.h>
+
+/*
+ * Leds: PD5 -> PD7
+ * Photodiodes: PC0 PC1
+ * IR: PB1, OC1A
+ */
+
+/*
+ * hfuse: RSTDISBL=1 WTDON=1 SPIEN=0 CKOPT=0 EESAVE=1 BOOTSZ1=0 BOOTSZ0=0 BOOTRST=1
+ * lfuse: BODLEVEL=1 BODEN=1 SUT1=1 SUT0=1 CKSEL3=1 CKSEL2=1 CKSEL1=1 CKSEL0=1
+ */
+
+#define N_PERIODS 10
+#define N_CYCLES_0 17
+#define N_CYCLES_1 17
+#define N_CYCLES_PERIOD (N_CYCLES_0 + N_CYCLES_1)
+
+#define LED_PORT PORTD
+#define LED_DDR DDRD
+#define LED1_BIT 5
+#define LED2_BIT 6
+#define LED3_BIT 7
+
+#define LED_TOGGLE(port, bit) do { \
+ if (port & _BV(bit)) \
+ port &= ~_BV(bit); \
+ else \
+ port |= _BV(bit); \
+ } while(0)
+
+#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)
+
+#define IR_PORT PORTB
+#define IR_DDR DDRB
+#define IR_BIT 1
+
+/* FRAME must be odd */
+#define FRAME 0x0B /* in little endian 1-1-0-1 */
+#define FRAME_LEN 4
+
+/* pin returns 1 when nothing, and 0 when laser is on photodiode */
+#define PHOTO_PIN PINC
+#define PHOTO1_BIT 0
+#define PHOTO2_BIT 1
+#define READ_PHOTOS() (PHOTO_PIN & (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT)))
+#define READ_PHOTO1() (PHOTO_PIN & _BV(PHOTO1_BIT))
+#define READ_PHOTO2() (PHOTO_PIN & _BV(PHOTO2_BIT))
+
+#define PHOTOS_ALL_OFF (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT))
+#define PHOTOS_ALL_ON (0)
+#define PHOTOS_1ON_2OFF (_BV(PHOTO2_BIT))
+#define PHOTOS_1OFF_2ON (_BV(PHOTO1_BIT))
+
+
+/* in cycles/64 (unit is 4 us at 16Mhz) */
+#define MAX_PHOTO_TIME ((uint8_t)25) /* t=100us len=5mm rps=40Hz dist=20cm */
+
+#define MIN_INTER_TIME ((uint8_t)12) /* t=50us len=50mm rps=40Hz dist=350cm */
+#define MAX_INTER_TIME ((uint8_t)250) /* t=1000us len=50mm rps=40Hz dist=20cm */
+
+/* in ms */
+#define INTER_LASER_TIME 10
+
+#define WAIT_LASER
+
+/* basic functions to transmit on IR */
+
+static inline void xmit_0(void)
+{
+ uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3);
+ TCCR1B = 0;
+ TCCR1A = 0;
+ _delay_loop_1(t); /* 3 cycles per loop */
+}
+
+static inline void xmit_1(void)
+{
+ uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3);
+ TCCR1B = _BV(WGM13) | _BV(WGM12);
+ TCNT1 = N_CYCLES_PERIOD-1;
+ TCCR1A = _BV(COM1A1) | _BV(WGM11);
+ TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
+ _delay_loop_1(t); /* 3 cycles per loop */
+}
+
+/* transmit in manchester code */
+
+static inline void xmit_manchester_0(void)
+{
+ xmit_0();
+ xmit_1();
+}
+
+static inline void xmit_manchester_1(void)
+{
+ xmit_1();
+ xmit_0();
+}
+
+/* transmit a full frame. Each byte is lsb first. */
+
+static void xmit_bits(uint8_t *buf, uint8_t nbit)
+{
+ uint8_t i;
+ uint8_t byte = *buf;
+
+ for (i=0; i<nbit; i++) {
+ if (byte & 1)
+ xmit_manchester_1();
+ else
+ xmit_manchester_0();
+
+ /* next bit */
+ byte >>= 1;
+
+ /* next byte */
+ if (((i & 0x07) == 0) && (i != 0))
+ byte = *(++buf);
+ }
+ xmit_0();
+}
+
+/* */
+static inline int8_t wait_laser(void)
+{
+ uint8_t photos;
+ uint8_t time;
+ uint8_t diff;
+
+ /* wait until all is off */
+ while (READ_PHOTOS() != PHOTOS_ALL_OFF);
+
+ /* wait an event, it must be photo1 on */
+ while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF);
+
+ if (photos != PHOTOS_1ON_2OFF)
+ return -1;
+ time = TCNT0;
+ LED1_ON();
+
+ /* wait an event, it must be photo1 off */
+ while ((photos = READ_PHOTOS()) == PHOTOS_1ON_2OFF) {
+ diff = TCNT0 - time;
+ if (diff > MAX_PHOTO_TIME)
+ return -1;
+ }
+ if (photos != PHOTOS_ALL_OFF)
+ return -1;
+ //time = TCNT0;
+
+ /* wait an event, it must be photo2 on */
+ while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF) {
+ diff = TCNT0 - time;
+ if (diff > MAX_INTER_TIME)
+ return -1;
+ }
+ LED2_ON();
+
+ diff = TCNT0 - time;
+ if (diff < MIN_INTER_TIME)
+ return -1;
+ if (photos != PHOTOS_1OFF_2ON)
+ return -1;
+
+#if 0
+ time = TCNT0;
+
+ /* wait an event, it must be photo2 off */
+ while ((photos = READ_PHOTOS()) == PHOTOS_1OFF_2ON) {
+ diff = TCNT0 - time;
+ if (diff > MAX_PHOTO_TIME)
+ return -1;
+ }
+ if (photos != PHOTOS_ALL_OFF)
+ return -1;
+#endif
+
+ /* laser ok */
+ return 0;
+}
+
+/* */
+
+int main(void)
+{
+ /* must be odd */
+ uint8_t frame = FRAME;
+ int8_t ret;
+
+ /* LEDS */
+ LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT);
+ IR_DDR |= _BV(IR_BIT);
+
+#if 0
+ while (1) {
+ LED1_ON();
+ wait_ms(500);
+ LED1_OFF();
+ wait_ms(500);
+ }
+#endif
+
+#if 0
+ while (1) {
+ if (READ_PHOTO() & _BV(PHOTO1_BIT))
+ LED1_ON();
+ else
+ LED1_OFF();
+ }
+#endif
+
+ /* configure PWM */
+ ICR1 = N_CYCLES_PERIOD;
+ OCR1A = N_CYCLES_1;
+ TCCR1A = _BV(COM1A1) | _BV(WGM11);
+ TCCR1B = _BV(WGM13) | _BV(WGM12);
+
+ /* configure timer 0, prescaler = 64 */
+ TCCR0 = _BV(CS01) | _BV(CS00);
+
+ while (1) {
+
+#ifdef WAIT_LASER
+ ret = wait_laser();
+
+ LED1_OFF();
+ LED2_OFF();
+
+ if (ret)
+ continue;
+#endif
+
+#if 1
+ LED3_ON();
+ /* ok, transmit frame */
+ xmit_bits(&frame, FRAME_LEN);
+
+ /* don't watch a laser during this time */
+ wait_ms(INTER_LASER_TIME);
+ LED3_OFF();
+#else
+ LED1_ON();
+ wait_ms(1);
+ LED1_OFF();
+#endif
+ }
+ return 0;
+}
--- /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=y
+# 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 is not set
+# 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=y
+CONFIG_MODULE_VECT2=y
+CONFIG_MODULE_GEOMETRY=y
+CONFIG_MODULE_SCHEDULER=y
+CONFIG_MODULE_SCHEDULER_STATS=y
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+# CONFIG_MODULE_SCHEDULER_TIMER0 is not set
+CONFIG_MODULE_SCHEDULER_MANUAL=y
+CONFIG_MODULE_TIME=y
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+
+#
+# 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 is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+CONFIG_MODULE_TIMER=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 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+CONFIG_MODULE_ENCODERS_MICROB=y
+CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG=y
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+CONFIG_MODULE_POSITION_MANAGER=y
+CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
+CONFIG_MODULE_TRAJECTORY_MANAGER=y
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE 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
+#
+# 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=y
+# 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 is not set
+# 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=y
+CONFIG_MODULE_VECT2=y
+CONFIG_MODULE_GEOMETRY=y
+CONFIG_MODULE_SCHEDULER=y
+CONFIG_MODULE_SCHEDULER_STATS=y
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+# CONFIG_MODULE_SCHEDULER_TIMER0 is not set
+CONFIG_MODULE_SCHEDULER_MANUAL=y
+CONFIG_MODULE_TIME=y
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+
+#
+# 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 is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+CONFIG_MODULE_TIMER=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 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+CONFIG_MODULE_ENCODERS_MICROB=y
+CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG=y
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+CONFIG_MODULE_POSITION_MANAGER=y
+CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
+CONFIG_MODULE_TRAJECTORY_MANAGER=y
+CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE 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 = ../../../..
+
+SRC = $(TARGET).c cmdline.c commands_gen.c
+SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
+SRC += sensor.c actuator.c cs.c strat_base.c strat_utils.c
+
+ASRC =
+
+CFLAGS += -Wall -Werror
+#CFLAGS += -DHOMOLOGATION
+#CFLAGS += -DTEST_BEACON
+#LDFLAGS = -T ../common/avr6.x
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+AVRDUDE_DELAY=50
+
+program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep
+ echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+ $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\
+
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: action.c,v 1.5 2008-03-31 22:22:52 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <aversive.h>
+
+#include <uart.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "action.h"
+
+void action_init(void)
+{
+}
+
+/*
+ * Protocol:
+ * 0000 XXXX select servo XXXX
+ * 01XX XXXX set LSB of servo
+ * 10XX XXXX set MSB of servo and validate value
+ */
+void action_servo_set(uint8_t num, uint16_t us)
+{
+ uart1_send(num);
+ uart1_send(0x40 | (us&0x3F));
+ uart1_send(0x80 | ((us>>6)&0x3F));
+}
--- /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: action.h,v 1.3 2008-03-31 22:22:52 zer0 Exp $
+ *
+ */
+
+#ifndef _ACTION_H_
+#define _ACTION_H_
+
+#define BRAKE_PORT PORTG
+//#define BRAKE_DDR DDRG
+#define BRAKE_PIN PING
+#define BRAKE_MASK 0x03
+
+//#define BRAKE_ON() do { BRAKE_PORT = BRAKE_PIN | BRAKE_MASK; } while(0)
+//#define BRAKE_OFF() do { BRAKE_PORT = BRAKE_PIN & (~BRAKE_MASK); } while(0)
+#define BRAKE_IS_ON ((BRAKE_PIN & BRAKE_MASK) == BRAKE_MASK)
+
+void action_init(void);
+void action_servo_set(uint8_t num, uint16_t us);
+
+#endif
--- /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.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <encoders_microb.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+
+#include "sensor.h"
+#include "main.h"
+
+#define OFF 0
+#define WAIT_SENSOR 1
+#define SENSOR_OK 2
+#define WAIT_DOWN 3
+
+static volatile uint8_t fessor_state = OFF;
+static volatile uint32_t fessor_pos_up = 35000;
+static volatile uint32_t fessor_pos_down = 63500;
+static volatile uint32_t fessor_delay_up = 500;
+static volatile uint32_t fessor_delay_down = 2000;
+static volatile uint32_t delay = 0;
+static volatile int32_t fessor_k1 = 500, fessor_k2 = 20;
+static volatile int32_t fessor_cmd = 0;
+
+
+static volatile uint8_t elevator_state = OFF;
+static volatile uint32_t elevator_pos_up = -8000;
+static volatile uint32_t elevator_pos_down = 0;
+static volatile uint32_t elevator_delay_up = 500;
+static volatile uint32_t elevator_delay_down = 2000;
+//static volatile uint32_t delay = 0;
+static volatile int32_t elevator_k1 = 500, elevator_k2 = 20;
+static volatile int32_t elevator_cmd = 0;
+
+void pwm_set_and_save(void *pwm, int32_t val)
+{
+ /* we need to do the saturation here, before saving the
+ * value */
+ if (val > 4095)
+ val = 4095;
+ if (val < -4095)
+ val = -4095;
+
+ if (pwm == LEFT_PWM)
+ mainboard.pwm_l = val;
+ else if (pwm == RIGHT_PWM)
+ mainboard.pwm_r = val;
+ pwm_ng_set(pwm, val);
+}
+
+void pickup_wheels_on(void)
+{
+ mainboard.enable_pickup_wheels = 1;
+}
+
+void pickup_wheels_off(void)
+{
+ mainboard.enable_pickup_wheels = 0;
+}
+
+/* init fessor position at beginning */
+void fessor_autopos(void)
+{
+ pwm_ng_set(FESSOR_PWM, -500);
+ wait_ms(3000);
+ pwm_ng_set(FESSOR_PWM, 0);
+}
+
+/* set CS command for fessor */
+void fessor_set(void *dummy, int32_t cmd)
+{
+ fessor_cmd = cmd;
+}
+
+void fessor_set_coefs(uint32_t k1, uint32_t k2)
+{
+ fessor_k1 = k1;
+ fessor_k2 = k2;
+}
+
+void fessor_set_delays(uint32_t delay_up, uint32_t delay_down)
+{
+ fessor_delay_up = delay_up;
+ fessor_delay_down = delay_down;
+}
+
+void fessor_set_pos(uint32_t pos_up, uint32_t pos_down)
+{
+ fessor_pos_up = pos_up;
+ fessor_pos_down = pos_down;
+}
+
+void fessor_dump_params(void)
+{
+ printf_P(PSTR("coef %ld %ld\r\n"), fessor_k1, fessor_k2);
+ printf_P(PSTR("pos %ld %ld\r\n"), fessor_pos_up, fessor_pos_down);
+ printf_P(PSTR("delay %ld %ld\r\n"), fessor_delay_up, fessor_delay_down);
+}
+
+/* called by CS perdiodically (current limit) */
+void fessor_manage(void)
+{
+ static int32_t oldpos = 0;
+ int32_t pos, maxcmd, speed, cmd;
+
+ cmd = fessor_cmd;
+ pos = encoders_microb_get_value(FESSOR_ENC);
+ speed = pos - oldpos;
+ if (speed > 0 && cmd < 0)
+ maxcmd = fessor_k1;
+ else if (speed < 0 && cmd > 0)
+ maxcmd = fessor_k1;
+ else {
+ speed = ABS(speed);
+ maxcmd = fessor_k1 + fessor_k2 * speed;
+ }
+ if (cmd > maxcmd)
+ cmd = maxcmd;
+ else if (cmd < -maxcmd)
+ cmd = -maxcmd;
+
+ pwm_ng_set(FESSOR_PWM, cmd);
+
+ oldpos = pos;
+}
+
+void fessor_up(void)
+{
+ fessor_state = 0;
+ cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
+}
+
+void fessor_down(void)
+{
+ fessor_state = 0;
+ cs_set_consign(&mainboard.fessor.cs, fessor_pos_down);
+}
+
+void fessor_stop(void)
+{
+ fessor_state = 0;
+}
+
+void fessor_auto(void)
+{
+ fessor_state = WAIT_SENSOR;
+ cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
+}
+
+/* for fessor auto mode */
+static void fessor_cb(void *dummy)
+{
+ static uint8_t prev = 0;
+ uint8_t val;
+
+ val = !sensor_get(0);
+
+ switch (fessor_state) {
+ case OFF:
+ break;
+ case WAIT_SENSOR:
+ if (val && !prev) {
+ delay = fessor_delay_up;
+ fessor_state = SENSOR_OK;
+ }
+ break;
+ case SENSOR_OK:
+ if (delay-- == 0) {
+ cs_set_consign(&mainboard.fessor.cs, fessor_pos_down);
+ fessor_state = WAIT_DOWN;
+ delay = fessor_delay_down;
+ }
+ break;
+ case WAIT_DOWN:
+ if (delay-- == 0) {
+ cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
+ fessor_state = WAIT_SENSOR;
+ }
+ break;
+ default:
+ break;
+ }
+ prev = val;
+}
+
+void fessor_init(void)
+{
+ scheduler_add_periodical_event_priority(fessor_cb, NULL,
+ 1000L / SCHEDULER_UNIT,
+ FESSOR_PRIO);
+}
+
+
+
+
+
+
+
+
+/* init elevator position at beginning */
+void elevator_autopos(void)
+{
+ pwm_ng_set(ELEVATOR_PWM, 300);
+ wait_ms(4000);
+ pwm_ng_set(ELEVATOR_PWM, 0);
+}
+
+/* set CS command for elevator */
+void elevator_set(void *dummy, int32_t cmd)
+{
+ elevator_cmd = cmd;
+}
+
+void elevator_set_coefs(uint32_t k1, uint32_t k2)
+{
+ elevator_k1 = k1;
+ elevator_k2 = k2;
+}
+
+void elevator_set_delays(uint32_t delay_up, uint32_t delay_down)
+{
+ elevator_delay_up = delay_up;
+ elevator_delay_down = delay_down;
+}
+
+void elevator_set_pos(uint32_t pos_up, uint32_t pos_down)
+{
+ elevator_pos_up = pos_up;
+ elevator_pos_down = pos_down;
+}
+
+void elevator_dump_params(void)
+{
+ printf_P(PSTR("coef %ld %ld\r\n"), elevator_k1, elevator_k2);
+ printf_P(PSTR("pos %ld %ld\r\n"), elevator_pos_up, elevator_pos_down);
+ printf_P(PSTR("delay %ld %ld\r\n"), elevator_delay_up, elevator_delay_down);
+}
+
+/* called by CS perdiodically (current limit) */
+void elevator_manage(void)
+{
+ static int32_t oldpos = 0;
+ int32_t pos, maxcmd, speed, cmd;
+
+ cmd = elevator_cmd;
+ pos = encoders_microb_get_value(ELEVATOR_ENC);
+ speed = pos - oldpos;
+ if (speed > 0 && cmd < 0)
+ maxcmd = elevator_k1;
+ else if (speed < 0 && cmd > 0)
+ maxcmd = elevator_k1;
+ else {
+ speed = ABS(speed);
+ maxcmd = elevator_k1 + elevator_k2 * speed;
+ }
+ if (cmd > maxcmd)
+ cmd = maxcmd;
+ else if (cmd < -maxcmd)
+ cmd = -maxcmd;
+
+ pwm_ng_set(ELEVATOR_PWM, cmd);
+
+ oldpos = pos;
+}
+
+void elevator_up(void)
+{
+ elevator_state = 0;
+ cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
+}
+
+void elevator_down(void)
+{
+ elevator_state = 0;
+ cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
+}
+
+void elevator_stop(void)
+{
+ elevator_state = 0;
+}
+
+void elevator_auto(void)
+{
+ elevator_state = WAIT_SENSOR;
+ cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
+}
+
+/* for elevator auto mode */
+static void elevator_cb(void *dummy)
+{
+ static uint8_t prev = 0;
+ uint8_t val;
+
+ val = !sensor_get(0);
+
+ switch (elevator_state) {
+ case OFF:
+ break;
+ case WAIT_SENSOR:
+ if (val && !prev) {
+ delay = elevator_delay_up;
+ elevator_state = SENSOR_OK;
+ }
+ break;
+ case SENSOR_OK:
+ if (delay-- == 0) {
+ cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
+ elevator_state = WAIT_DOWN;
+ delay = elevator_delay_down;
+ }
+ break;
+ case WAIT_DOWN:
+ if (delay-- == 0) {
+ cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
+ elevator_state = WAIT_SENSOR;
+ }
+ break;
+ default:
+ break;
+ }
+ prev = val;
+}
+
+void elevator_init(void)
+{
+ scheduler_add_periodical_event_priority(elevator_cb, NULL,
+ 1000L / SCHEDULER_UNIT,
+ ELEVATOR_PRIO);
+}
--- /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:41 zer0 Exp $
+ *
+ */
+
+void pwm_set_and_save(void *pwm, int32_t val);
+void pickup_wheels_on(void);
+void pickup_wheels_off(void);
+
+void fessor_set(void *dummy, int32_t cmd);
+void fessor_manage(void);
+void fessor_up(void);
+void fessor_down(void);
+void fessor_stop(void);
+void fessor_auto(void);
+void fessor_init(void);
+void fessor_autopos(void);
+void fessor_dump_params(void);
+
+void fessor_set_coefs(uint32_t k1, uint32_t k2);
+void fessor_set_delays(uint32_t delay_up, uint32_t delay_down);
+void fessor_set_pos(uint32_t pos_up, uint32_t pos_down);
+
+
+void elevator_set(void *dummy, int32_t cmd);
+void elevator_manage(void);
+void elevator_up(void);
+void elevator_down(void);
+void elevator_stop(void);
+void elevator_auto(void);
+void elevator_init(void);
+void elevator_autopos(void);
+void elevator_dump_params(void);
+
+void elevator_set_coefs(uint32_t k1, uint32_t k2);
+void elevator_set_delays(uint32_t delay_up, uint32_t delay_down);
+void elevator_set_pos(uint32_t pos_up, uint32_t pos_down);
--- /dev/null
+#ifndef _ADC_CONFIG_H_
+#define _ADC_CONFIG_H_
+
+
+
+
+#endif // _ADC_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: cmdline.c,v 1.5 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include "main.h"
+#include "strat_base.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;
+
+ /* reset CTRL-C for trajectory interruption each time we
+ * receive a new command */
+ interrupt_traj_reset();
+
+ ret = parse(main_ctx, buf);
+ if (ret == PARSE_AMBIGUOUS)
+ printf_P(PSTR("Ambiguous command\r\n"));
+ else if (ret == PARSE_NOMATCH)
+ printf_P(PSTR("Command not found\r\n"));
+ else if (ret == PARSE_BAD_ARGS)
+ printf_P(PSTR("Bad arguments\r\n"));
+}
+
+static int8_t
+complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
+ int16_t *state)
+{
+ return complete(main_ctx, buf, state, dstbuf, dstsize);
+}
+
+
+/* sending "pop" on cmdline uart resets the robot */
+void emergency(char c)
+{
+ static uint8_t i = 0;
+
+ /* interrupt traj here */
+ if (c == '\003')
+ interrupt_traj();
+
+ if ((i == 0 && c == 'p') ||
+ (i == 1 && c == 'o') ||
+ (i == 2 && c == 'p'))
+ i++;
+ else if ( !(i == 1 && c == 'p') )
+ i = 0;
+ if (i == 3)
+ reset();
+}
+
+/* log function, add a command to configure
+ * it dynamically */
+void mylog(struct error * e, ...)
+{
+ va_list ap;
+ u16 stream_flags = stdout->flags;
+ uint8_t i;
+ time_h tv;
+
+ if (e->severity > ERROR_SEVERITY_ERROR) {
+ if (gen.log_level < e->severity)
+ return;
+
+ for (i=0; i<NB_LOGS+1; i++)
+ if (gen.logs[i] == e->err_num)
+ break;
+ if (i == NB_LOGS+1)
+ return;
+ }
+
+ va_start(ap, e);
+ tv = time_get_time();
+ printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
+
+ printf_P(PSTR("(%d,%d,%d) "),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+
+ vfprintf_P(stdout, e->text, ap);
+ printf_P(PSTR("\r\n"));
+ va_end(ap);
+ stdout->flags = stream_flags;
+}
+
+int cmdline_interact(void)
+{
+ const char *history, *buffer;
+ int8_t ret, same = 0;
+ int16_t c;
+
+ rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
+ snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > ");
+ rdline_newline(&gen.rdl, gen.prompt);
+
+ while (1) {
+ c = uart_recv_nowait(CMDLINE_UART);
+ if (c == -1)
+ continue;
+ ret = rdline_char_in(&gen.rdl, c);
+ if (ret != 2 && ret != 0) {
+ buffer = rdline_get_buffer(&gen.rdl);
+ history = rdline_get_history_item(&gen.rdl, 0);
+ if (history) {
+ same = !memcmp(buffer, history, strlen(history)) &&
+ buffer[strlen(history)] == '\n';
+ }
+ else
+ same = 0;
+ if (strlen(buffer) > 1 && !same)
+ rdline_add_history(&gen.rdl, buffer);
+ rdline_newline(&gen.rdl, gen.prompt);
+ }
+ }
+
+ return 0;
+}
--- /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-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#define CMDLINE_UART 0
+
+/* uart rx callback for reset() */
+void emergency(char c);
+
+/* log function */
+void mylog(struct error * e, ...);
+
+/* launch cmdline */
+int cmdline_interact(void);
+
+static inline uint8_t cmdline_keypressed(void) {
+ return (uart_recv_nowait(CMDLINE_UART) != -1);
+}
+
+static inline int16_t cmdline_getchar(void) {
+ return uart_recv_nowait(CMDLINE_UART);
+}
+
+static inline uint8_t cmdline_getchar_wait(void) {
+ return uart_recv(CMDLINE_UART);
+}
--- /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.7 2009-04-24 19:30:41 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdlib.h>
+#include <aversive/pgmspace.h>
+#include <parse.h>
+
+/* commands_gen.c */
+extern parse_pgm_inst_t cmd_reset;
+extern parse_pgm_inst_t cmd_bootloader;
+extern parse_pgm_inst_t cmd_encoders;
+extern parse_pgm_inst_t cmd_pwm;
+extern parse_pgm_inst_t cmd_adc;
+extern parse_pgm_inst_t cmd_sensor;
+extern parse_pgm_inst_t cmd_log;
+extern parse_pgm_inst_t cmd_log_show;
+extern parse_pgm_inst_t cmd_log_type;
+extern parse_pgm_inst_t cmd_stack_space;
+extern parse_pgm_inst_t cmd_scheduler;
+
+/* commands_ax12.c */
+extern parse_pgm_inst_t cmd_baudrate;
+extern parse_pgm_inst_t cmd_uint16_read;
+extern parse_pgm_inst_t cmd_uint16_write;
+extern parse_pgm_inst_t cmd_uint8_read;
+extern parse_pgm_inst_t cmd_uint8_write;
+
+/* commands_cs.c */
+extern parse_pgm_inst_t cmd_gain;
+extern parse_pgm_inst_t cmd_gain_show;
+extern parse_pgm_inst_t cmd_speed;
+extern parse_pgm_inst_t cmd_speed_show;
+extern parse_pgm_inst_t cmd_derivate_filter;
+extern parse_pgm_inst_t cmd_derivate_filter_show;
+extern parse_pgm_inst_t cmd_consign;
+extern parse_pgm_inst_t cmd_maximum;
+extern parse_pgm_inst_t cmd_maximum_show;
+extern parse_pgm_inst_t cmd_quadramp;
+extern parse_pgm_inst_t cmd_quadramp_show;
+extern parse_pgm_inst_t cmd_cs_status;
+extern parse_pgm_inst_t cmd_blocking_i;
+extern parse_pgm_inst_t cmd_blocking_i_show;
+
+/* commands_mainboard.c */
+extern parse_pgm_inst_t cmd_event;
+extern parse_pgm_inst_t cmd_spi_test;
+extern parse_pgm_inst_t cmd_opponent;
+extern parse_pgm_inst_t cmd_opponent_set;
+extern parse_pgm_inst_t cmd_start;
+extern parse_pgm_inst_t cmd_interact;
+extern parse_pgm_inst_t cmd_color;
+extern parse_pgm_inst_t cmd_rs;
+extern parse_pgm_inst_t cmd_i2cdebug;
+extern parse_pgm_inst_t cmd_mechboard_show;
+extern parse_pgm_inst_t cmd_mechboard_setmode1;
+extern parse_pgm_inst_t cmd_mechboard_setmode2;
+extern parse_pgm_inst_t cmd_mechboard_setmode3;
+extern parse_pgm_inst_t cmd_mechboard_setmode4;
+extern parse_pgm_inst_t cmd_mechboard_setmode5;
+extern parse_pgm_inst_t cmd_pickup_wheels;
+extern parse_pgm_inst_t cmd_beacon_start;
+extern parse_pgm_inst_t cmd_pump_current;
+extern parse_pgm_inst_t cmd_build_test;
+extern parse_pgm_inst_t cmd_column_test;
+extern parse_pgm_inst_t cmd_column_test2;
+extern parse_pgm_inst_t cmd_lintel_test;
+extern parse_pgm_inst_t cmd_pickup_test;
+extern parse_pgm_inst_t cmd_scan_test;
+extern parse_pgm_inst_t cmd_scan_test2;
+extern parse_pgm_inst_t cmd_time_monitor;
+extern parse_pgm_inst_t cmd_scanner;
+extern parse_pgm_inst_t cmd_build_z1;
+#ifdef TEST_BEACON
+extern parse_pgm_inst_t cmd_beacon_opp_dump;
+#endif
+extern parse_pgm_inst_t cmd_test;
+extern parse_pgm_inst_t cmd_fessor;
+extern parse_pgm_inst_t cmd_fessor_params;
+extern parse_pgm_inst_t cmd_fessor_params_show;
+
+
+extern parse_pgm_inst_t cmd_elevator;
+extern parse_pgm_inst_t cmd_elevator_params;
+extern parse_pgm_inst_t cmd_elevator_params_show;
+
+/* commands_traj.c */
+extern parse_pgm_inst_t cmd_traj_speed;
+extern parse_pgm_inst_t cmd_traj_speed_show;
+extern parse_pgm_inst_t cmd_trajectory;
+extern parse_pgm_inst_t cmd_trajectory_show;
+extern parse_pgm_inst_t cmd_rs_gains;
+extern parse_pgm_inst_t cmd_rs_gains_show;
+extern parse_pgm_inst_t cmd_track;
+extern parse_pgm_inst_t cmd_track_show;
+extern parse_pgm_inst_t cmd_pt_list;
+extern parse_pgm_inst_t cmd_pt_list_append;
+extern parse_pgm_inst_t cmd_pt_list_del;
+extern parse_pgm_inst_t cmd_pt_list_show;
+extern parse_pgm_inst_t cmd_goto1;
+extern parse_pgm_inst_t cmd_goto2;
+extern parse_pgm_inst_t cmd_goto3;
+extern parse_pgm_inst_t cmd_position;
+extern parse_pgm_inst_t cmd_position_set;
+extern parse_pgm_inst_t cmd_strat_infos;
+extern parse_pgm_inst_t cmd_strat_conf;
+extern parse_pgm_inst_t cmd_strat_conf2;
+extern parse_pgm_inst_t cmd_strat_conf3;
+extern parse_pgm_inst_t cmd_strat_conf4;
+extern parse_pgm_inst_t cmd_subtraj;
+
+/* in progmem */
+parse_pgm_ctx_t main_ctx[] = {
+
+ /* commands_gen.c */
+ (parse_pgm_inst_t *)&cmd_reset,
+ (parse_pgm_inst_t *)&cmd_bootloader,
+ (parse_pgm_inst_t *)&cmd_encoders,
+ (parse_pgm_inst_t *)&cmd_pwm,
+ (parse_pgm_inst_t *)&cmd_adc,
+ (parse_pgm_inst_t *)&cmd_sensor,
+ (parse_pgm_inst_t *)&cmd_log,
+ (parse_pgm_inst_t *)&cmd_log_show,
+ (parse_pgm_inst_t *)&cmd_log_type,
+ (parse_pgm_inst_t *)&cmd_stack_space,
+ (parse_pgm_inst_t *)&cmd_scheduler,
+
+ /* commands_cs.c */
+ (parse_pgm_inst_t *)&cmd_gain,
+ (parse_pgm_inst_t *)&cmd_gain_show,
+ (parse_pgm_inst_t *)&cmd_speed,
+ (parse_pgm_inst_t *)&cmd_speed_show,
+ (parse_pgm_inst_t *)&cmd_consign,
+ (parse_pgm_inst_t *)&cmd_derivate_filter,
+ (parse_pgm_inst_t *)&cmd_derivate_filter_show,
+ (parse_pgm_inst_t *)&cmd_maximum,
+ (parse_pgm_inst_t *)&cmd_maximum_show,
+ (parse_pgm_inst_t *)&cmd_quadramp,
+ (parse_pgm_inst_t *)&cmd_quadramp_show,
+ (parse_pgm_inst_t *)&cmd_cs_status,
+ (parse_pgm_inst_t *)&cmd_blocking_i,
+ (parse_pgm_inst_t *)&cmd_blocking_i_show,
+
+ /* commands_mainboard.c */
+ (parse_pgm_inst_t *)&cmd_event,
+ (parse_pgm_inst_t *)&cmd_interact,
+ (parse_pgm_inst_t *)&cmd_rs,
+ (parse_pgm_inst_t *)&cmd_fessor,
+ (parse_pgm_inst_t *)&cmd_fessor_params,
+ (parse_pgm_inst_t *)&cmd_fessor_params_show,
+ (parse_pgm_inst_t *)&cmd_elevator,
+ (parse_pgm_inst_t *)&cmd_elevator_params,
+ (parse_pgm_inst_t *)&cmd_elevator_params_show,
+ (parse_pgm_inst_t *)&cmd_test,
+
+ /* commands_traj.c */
+ (parse_pgm_inst_t *)&cmd_traj_speed,
+ (parse_pgm_inst_t *)&cmd_traj_speed_show,
+ (parse_pgm_inst_t *)&cmd_trajectory,
+ (parse_pgm_inst_t *)&cmd_trajectory_show,
+ (parse_pgm_inst_t *)&cmd_rs_gains,
+ (parse_pgm_inst_t *)&cmd_rs_gains_show,
+ (parse_pgm_inst_t *)&cmd_track,
+ (parse_pgm_inst_t *)&cmd_track_show,
+ (parse_pgm_inst_t *)&cmd_pt_list,
+ (parse_pgm_inst_t *)&cmd_pt_list_append,
+ (parse_pgm_inst_t *)&cmd_pt_list_del,
+ (parse_pgm_inst_t *)&cmd_pt_list_show,
+ (parse_pgm_inst_t *)&cmd_goto1,
+ (parse_pgm_inst_t *)&cmd_goto2,
+ (parse_pgm_inst_t *)&cmd_position,
+ (parse_pgm_inst_t *)&cmd_position_set,
+ 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-05-02 10:08:09 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+
+uint8_t addr_from_string(const char *s)
+{
+ /* 16 bits */
+ if (!strcmp_P(s, PSTR("cw_angle_limit")))
+ return AA_CW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("ccw_angle_limit")))
+ return AA_CCW_ANGLE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("max_torque")))
+ return AA_MAX_TORQUE_L;
+ if (!strcmp_P(s, PSTR("down_calibration")))
+ return AA_DOWN_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("up_calibration")))
+ return AA_UP_CALIBRATION_L;
+ if (!strcmp_P(s, PSTR("torque_limit")))
+ return AA_TORQUE_LIMIT_L;
+ if (!strcmp_P(s, PSTR("position")))
+ return AA_PRESENT_POSITION_L;
+ if (!strcmp_P(s, PSTR("speed")))
+ return AA_PRESENT_SPEED_L;
+ if (!strcmp_P(s, PSTR("load")))
+ return AA_PRESENT_LOAD_L;
+ if (!strcmp_P(s, PSTR("moving_speed")))
+ return AA_MOVING_SPEED_L;
+ if (!strcmp_P(s, PSTR("model")))
+ return AA_MODEL_NUMBER_L;
+ if (!strcmp_P(s, PSTR("goal_pos")))
+ return AA_GOAL_POSITION_L;
+ if (!strcmp_P(s, PSTR("punch")))
+ return AA_PUNCH_L;
+
+ /* 8 bits */
+ if (!strcmp_P(s, PSTR("firmware")))
+ return AA_FIRMWARE;
+ if (!strcmp_P(s, PSTR("id")))
+ return AA_ID;
+ if (!strcmp_P(s, PSTR("baudrate")))
+ return AA_BAUD_RATE;
+ if (!strcmp_P(s, PSTR("delay")))
+ return AA_DELAY_TIME;
+ if (!strcmp_P(s, PSTR("high_lim_temp")))
+ return AA_HIGHEST_LIMIT_TEMP;
+ if (!strcmp_P(s, PSTR("low_lim_volt")))
+ return AA_LOWEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("high_lim_volt")))
+ return AA_HIGHEST_LIMIT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("status_return")))
+ return AA_STATUS_RETURN_LEVEL;
+ if (!strcmp_P(s, PSTR("alarm_led")))
+ return AA_ALARM_LED;
+ if (!strcmp_P(s, PSTR("alarm_shutdown")))
+ return AA_ALARM_SHUTDOWN;
+ if (!strcmp_P(s, PSTR("torque_enable")))
+ return AA_TORQUE_ENABLE;
+ if (!strcmp_P(s, PSTR("led")))
+ return AA_LED;
+ if (!strcmp_P(s, PSTR("cw_comp_margin")))
+ return AA_CW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("ccw_comp_margin")))
+ return AA_CCW_COMPLIANCE_MARGIN;
+ if (!strcmp_P(s, PSTR("cw_comp_slope")))
+ return AA_CW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("ccw_comp_slope")))
+ return AA_CCW_COMPLIANCE_SLOPE;
+ if (!strcmp_P(s, PSTR("voltage")))
+ return AA_PRESENT_VOLTAGE;
+ if (!strcmp_P(s, PSTR("temp")))
+ return AA_PRESENT_TEMP;
+ if (!strcmp_P(s, PSTR("reginst")))
+ return AA_PRESENT_REGINST;
+ if (!strcmp_P(s, PSTR("moving")))
+ return AA_MOVING;
+ if (!strcmp_P(s, PSTR("lock")))
+ return AA_LOCK;
+
+ return 0;
+}
+
+/**********************************************************/
+/* Ax12_Stress */
+
+/* this structure is filled when cmd_ax12_stress is parsed successfully */
+struct cmd_ax12_stress_result {
+ fixed_string_t arg0;
+ uint8_t id;
+};
+
+/* function called when cmd_ax12_stress is parsed successfully */
+static void cmd_ax12_stress_parsed(void *parsed_result, void *data)
+{
+ struct cmd_ax12_stress_result *res = parsed_result;
+ int i, nb_errs = 0;
+ uint8_t val;
+ microseconds t = time_get_us2();
+
+ for (i=0; i<1000; i++) {
+ if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
+ nb_errs ++;
+ }
+
+ printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
+ t = (time_get_us2() - t) / 1000;
+ printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
+}
+
+prog_char str_ax12_stress_arg0[] = "ax12_stress";
+parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
+parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
+
+prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands";
+parse_pgm_inst_t cmd_ax12_stress = {
+ .f = cmd_ax12_stress_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ax12_stress,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ax12_stress_arg0,
+ (prog_void *)&cmd_ax12_stress_id,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_baudrate is parsed successfully */
+struct cmd_baudrate_result {
+ fixed_string_t arg0;
+ uint32_t arg1;
+};
+
+/* function called when cmd_baudrate is parsed successfully */
+static void cmd_baudrate_parsed(void * parsed_result, void * data)
+{
+ struct cmd_baudrate_result *res = parsed_result;
+ struct uart_config c;
+
+ printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+ uart_getconf(1, &c);
+ c.baudrate = res->arg1;
+ uart_setconf(1, &c);
+ printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+}
+
+prog_char str_baudrate_arg0[] = "baudrate";
+parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
+parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
+
+prog_char help_baudrate[] = "Change ax12 baudrate";
+parse_pgm_inst_t cmd_baudrate = {
+ .f = cmd_baudrate_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_baudrate,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_baudrate_arg0,
+ (prog_void *)&cmd_baudrate_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint16 */
+
+
+/* this structure is filled when cmd_uint16_read is parsed successfully */
+struct cmd_uint16_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint16_t val;
+};
+
+/* function called when cmd_uint16_read is parsed successfully */
+static void cmd_uint16_read_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint16_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+ ret = AX12_read_int(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint16_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
+prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#down_calibration#up_calibration#torque_limit#"
+ "position#speed#load#punch";
+parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
+parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
+
+prog_char help_uint16_read[] = "Read uint16 value (type, num)";
+parse_pgm_inst_t cmd_uint16_read = {
+ .f = cmd_uint16_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0,
+ (prog_void *)&cmd_uint16_arg1,
+ (prog_void *)&cmd_uint16_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint16_write is parsed successfully */
+static void cmd_uint16_write_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint16_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t addr = addr_from_string(res->arg1);
+ printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = AX12_write_int(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint16_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
+prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
+ "max_torque#torque_limit#punch";
+parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
+parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
+
+prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
+parse_pgm_inst_t cmd_uint16_write = {
+ .f = cmd_uint16_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint16_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint16_arg0_w,
+ (prog_void *)&cmd_uint16_arg1_w,
+ (prog_void *)&cmd_uint16_num,
+ (prog_void *)&cmd_uint16_val,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Uint8 */
+
+
+/* this structure is filled when cmd_uint8_read is parsed successfully */
+struct cmd_uint8_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t num;
+ uint8_t val;
+};
+
+/* function called when cmd_uint8_read is parsed successfully */
+static void cmd_uint8_read_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t ret;
+ uint8_t val;
+ uint8_t addr = addr_from_string(res->arg1);
+
+ ret = AX12_read_byte(&gen.ax12, res->num, addr, &val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+ printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
+}
+
+prog_char str_uint8_arg0[] = "read";
+parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
+prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "voltage#temp#reginst#moving#lock";
+parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
+parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
+
+prog_char help_uint8_read[] = "Read uint8 value (type, num)";
+parse_pgm_inst_t cmd_uint8_read = {
+ .f = cmd_uint8_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_read,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0,
+ (prog_void *)&cmd_uint8_arg1,
+ (prog_void *)&cmd_uint8_num,
+ NULL,
+ },
+};
+
+/* function called when cmd_uint8_write is parsed successfully */
+static void cmd_uint8_write_parsed(void * parsed_result, void * data)
+{
+ struct cmd_uint8_result *res = parsed_result;
+ uint8_t addr = addr_from_string(res->arg1);
+ uint8_t ret;
+ printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
+ res->val, res->val);
+ ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val);
+ if (ret)
+ printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
+}
+
+prog_char str_uint8_arg0_w[] = "write";
+parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
+prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
+ "low_lim_volt#high_lim_volt#status_return#alarm_led#"
+ "alarm_shutdown#torque_enable#led#cw_comp_margin#"
+ "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
+ "reginst#lock";
+parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
+parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
+
+prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
+parse_pgm_inst_t cmd_uint8_write = {
+ .f = cmd_uint8_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_uint8_write,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_uint8_arg0_w,
+ (prog_void *)&cmd_uint8_arg1_w,
+ (prog_void *)&cmd_uint8_num,
+ (prog_void *)&cmd_uint8_val,
+ NULL,
+ },
+};
--- /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.4 2009-05-02 10:08:09 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "cs.h"
+#include "cmdline.h"
+
+struct csb_list {
+ const prog_char *name;
+ struct cs_block *csb;
+};
+
+prog_char csb_angle_str[] = "angle";
+prog_char csb_distance_str[] = "distance";
+prog_char csb_fessor_str[] = "fessor";
+prog_char csb_wheel_str[] = "wheel";
+prog_char csb_elevator_str[] = "elevator";
+
+struct csb_list csb_list[] = {
+ { .name = csb_angle_str, .csb = &mainboard.angle },
+ { .name = csb_distance_str, .csb = &mainboard.distance },
+ { .name = csb_fessor_str, .csb = &mainboard.fessor },
+ { .name = csb_wheel_str, .csb = &mainboard.wheel },
+ { .name = csb_elevator_str, .csb = &mainboard.elevator },
+};
+
+struct cmd_cs_result {
+ fixed_string_t cmdname;
+ fixed_string_t csname;
+};
+
+/* token to be used for all cs-related commands */
+prog_char str_csb_name[] = "angle#distance#fessor#wheel#elevator";
+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.6 2009-05-02 10:08:09 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <encoders_microb.h>
+#include <adc.h>
+
+#include <scheduler.h>
+#include <scheduler_stats.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include <diagnostic.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "sensor.h"
+
+#include "action.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_microb_set_value((void *)0, 0);
+ encoders_microb_set_value((void *)1, 0);
+ encoders_microb_set_value((void *)2, 0);
+ encoders_microb_set_value((void *)3, 0);
+ return;
+ }
+
+ /* show */
+ while(!cmdline_keypressed()) {
+ printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
+ encoders_microb_get_value((void *)0),
+ encoders_microb_get_value((void *)1),
+ encoders_microb_get_value((void *)2),
+ encoders_microb_get_value((void *)3));
+ wait_ms(100);
+ }
+}
+
+prog_char str_encoders_arg0[] = "encoders";
+parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
+prog_char str_encoders_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
+
+prog_char help_encoders[] = "Show encoders values";
+parse_pgm_inst_t cmd_encoders = {
+ .f = cmd_encoders_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_encoders,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_encoders_arg0,
+ (prog_void *)&cmd_encoders_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Scheduler show */
+
+/* this structure is filled when cmd_scheduler is parsed successfully */
+struct cmd_scheduler_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_scheduler is parsed successfully */
+static void cmd_scheduler_parsed(void *parsed_result, void *data)
+{
+ scheduler_dump_events();
+ scheduler_stats_dump();
+}
+
+prog_char str_scheduler_arg0[] = "scheduler";
+parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
+prog_char str_scheduler_arg1[] = "show";
+parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
+
+prog_char help_scheduler[] = "Show scheduler events";
+parse_pgm_inst_t cmd_scheduler = {
+ .f = cmd_scheduler_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_scheduler,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_scheduler_arg0,
+ (prog_void *)&cmd_scheduler_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Pwms tests */
+
+/* this structure is filled when cmd_pwm is parsed successfully */
+struct cmd_pwm_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t arg2;
+};
+
+/* function called when cmd_pwm is parsed successfully */
+static void cmd_pwm_parsed(void * parsed_result, void * data)
+{
+ void * pwm_ptr = NULL;
+ struct cmd_pwm_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("1(1A)")))
+ pwm_ptr = &gen.pwm1_1A;
+ else if (!strcmp_P(res->arg1, PSTR("2(1B)")))
+ pwm_ptr = &gen.pwm2_1B;
+ else if (!strcmp_P(res->arg1, PSTR("3(3A)")))
+ pwm_ptr = &gen.pwm3_3A;
+ else if (!strcmp_P(res->arg1, PSTR("4(3B)")))
+ pwm_ptr = &gen.pwm4_3B;
+
+ if (pwm_ptr)
+ pwm_ng_set(pwm_ptr, res->arg2);
+
+ printf_P(PSTR("done %p\r\n"), pwm_ptr);
+}
+
+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(1A)#2(1B)#3(3A)#4(3B)";
+parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
+parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
+
+prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
+parse_pgm_inst_t cmd_pwm = {
+ .f = cmd_pwm_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pwm,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pwm_arg0,
+ (prog_void *)&cmd_pwm_arg1,
+ (prog_void *)&cmd_pwm_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Adcs tests */
+
+/* this structure is filled when cmd_adc is parsed successfully */
+struct cmd_adc_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_adc is parsed successfully */
+static void cmd_adc_parsed(void *parsed_result, void *data)
+{
+ struct cmd_adc_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("ADC values: "));
+ for (i=0; i<ADC_MAX; i++) {
+ printf_P(PSTR("%.4d "), sensor_get_adc(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_adc_arg0[] = "adc";
+parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
+prog_char str_adc_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
+
+prog_char help_adc[] = "Show adc values";
+parse_pgm_inst_t cmd_adc = {
+ .f = cmd_adc_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_adc,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_adc_arg0,
+ (prog_void *)&cmd_adc_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Sensors tests */
+
+/* this structure is filled when cmd_sensor is parsed successfully */
+struct cmd_sensor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_sensor is parsed successfully */
+static void cmd_sensor_parsed(void *parsed_result, void *data)
+{
+ struct cmd_sensor_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("SENSOR values: "));
+ for (i=0; i<SENSOR_MAX; i++) {
+ printf_P(PSTR("%d "), !!sensor_get(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_sensor_arg0[] = "sensor";
+parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
+prog_char str_sensor_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
+
+prog_char help_sensor[] = "Show sensor values";
+parse_pgm_inst_t cmd_sensor = {
+ .f = cmd_sensor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sensor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_sensor_arg0,
+ (prog_void *)&cmd_sensor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+ fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const prog_char uart_log[] = "uart";
+static const prog_char rs_log[] = "rs";
+static const prog_char traj_log[] = "traj";
+static const prog_char i2c_log[] = "i2c";
+static const prog_char oa_log[] = "oa";
+static const prog_char strat_log[] = "strat";
+static const prog_char i2cproto_log[] = "i2cproto";
+static const prog_char sensor_log[] = "sensor";
+static const prog_char block_log[] = "bd";
+static const prog_char cs_log[] = "cs";
+
+struct log_name_and_num {
+ const prog_char * name;
+ uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+ { uart_log, E_UART },
+ { rs_log, E_ROBOT_SYSTEM },
+ { traj_log, E_TRAJECTORY },
+ { i2c_log, E_I2C },
+ { oa_log, E_OA },
+ { strat_log, E_USER_STRAT },
+ { i2cproto_log, E_USER_I2C_PROTO },
+ { sensor_log, E_USER_SENSOR },
+ { block_log, E_BLOCKING_DETECTION_MANAGER },
+ { cs_log, E_USER_CS },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (!strcmp_P(s, log_name_and_num[i].name)) {
+ return log_name_and_num[i].num;
+ }
+ }
+ return 0;
+}
+
+const prog_char *
+log_num2name(uint8_t num)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (num == log_name_and_num[i].num) {
+ return log_name_and_num[i].name;
+ }
+ }
+ return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+ uint8_t i, empty=1;
+ const prog_char * name;
+
+ printf_P(PSTR("log level is %d\r\n"), gen.log_level);
+ for (i=0; i<NB_LOGS; i++) {
+ name = log_num2name(gen.logs[i]);
+ if (name) {
+ printf_P(PSTR("log type %S is on\r\n"), name);
+ empty = 0;
+ }
+ }
+ if (empty)
+ printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, void * data)
+{
+ struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("level"))) {
+ gen.log_level = res->arg2;
+ }
+
+ /* else it is a show */
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg0[] = "log";
+parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
+prog_char str_log_arg1[] = "level";
+parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
+parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
+
+prog_char help_log[] = "Set log options: level (0 -> 5)";
+parse_pgm_inst_t cmd_log = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1,
+ (prog_void *)&cmd_log_arg2,
+ NULL,
+ },
+};
+
+prog_char str_log_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
+
+prog_char help_log_show[] = "Show configured logs";
+parse_pgm_inst_t cmd_log_show = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_show,
+ NULL,
+ },
+};
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_type_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_type_parsed(void * parsed_result, void * data)
+{
+ struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
+ uint8_t lognum;
+ uint8_t i;
+
+ lognum = log_name2num(res->arg2);
+ if (lognum == 0) {
+ printf_P(PSTR("Cannot find log num\r\n"));
+ return;
+ }
+
+ if (!strcmp_P(res->arg3, PSTR("on"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ printf_P(PSTR("Already on\r\n"));
+ return;
+ }
+ }
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == 0) {
+ gen.logs[i] = lognum;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("no more room\r\n"));
+ }
+ }
+ else if (!strcmp_P(res->arg3, PSTR("off"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (gen.logs[i] == lognum) {
+ gen.logs[i] = 0;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("already off\r\n"));
+ }
+ }
+ cmd_log_do_show();
+}
+
+prog_char str_log_arg1_type[] = "type";
+parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+prog_char str_log_arg2_type[] = "uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor#bd#cs";
+parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+prog_char str_log_arg3[] = "on#off";
+parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+prog_char help_log_type[] = "Set log type";
+parse_pgm_inst_t cmd_log_type = {
+ .f = cmd_log_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_type,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_log_arg0,
+ (prog_void *)&cmd_log_arg1_type,
+ (prog_void *)&cmd_log_arg2_type,
+ (prog_void *)&cmd_log_arg3,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(void *parsed_result, void *data)
+{
+ printf("res stack: %d\r\n", min_stack_space_available());
+
+}
+
+prog_char str_stack_space_arg0[] = "stack_space";
+parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+prog_char help_stack_space[] = "Display remaining stack space";
+parse_pgm_inst_t cmd_stack_space = {
+ .f = cmd_stack_space_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_stack_space,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_stack_space_arg0,
+ NULL,
+ },
+};
+
+#if 0
+/**********************************************************/
+/* Servo */
+
+
+/* this structure is filled when cmd_servo is parsed successfully */
+struct cmd_servo_result {
+ fixed_string_t arg0;
+ uint8_t num;
+ uint16_t us;
+};
+
+/* function called when cmd_servo is parsed successfully */
+static void cmd_servo_parsed(void * parsed_result, void * data)
+{
+ struct cmd_servo_result *res = (struct cmd_servo_result *) parsed_result;
+ action_servo_set(res->num, res->us);
+}
+
+prog_char str_servo_arg0[] = "servo";
+parse_pgm_token_string_t cmd_servo_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg0, str_servo_arg0);
+parse_pgm_token_num_t cmd_servo_num = TOKEN_NUM_INITIALIZER(struct cmd_servo_result, num, UINT8);
+parse_pgm_token_num_t cmd_servo_us = TOKEN_NUM_INITIALIZER(struct cmd_servo_result, us, UINT16);
+
+prog_char help_servo[] = "Set servo position/speed";
+parse_pgm_inst_t cmd_servo = {
+ .f = cmd_servo_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_servo_arg0,
+ (prog_void *)&cmd_servo_num,
+ (prog_void *)&cmd_servo_us,
+ NULL,
+ },
+};
+#endif
--- /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_mainboard.c,v 1.7 2009-05-02 10:08:09 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <avr/eeprom.h>
+
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "sensor.h"
+#include "cmdline.h"
+#include "actuator.h"
+#include "strat_base.h"
+
+struct cmd_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+
+/* function called when cmd_event is parsed successfully */
+static void cmd_event_parsed(void *parsed_result, void *data)
+{
+ u08 bit=0;
+
+ struct cmd_event_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("all"))) {
+ bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
+ DO_BD | DO_TIMER | DO_POWER;
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ mainboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ mainboard.flags &= bit;
+ else { /* show */
+ printf_P(PSTR("encoders is %s\r\n"),
+ (DO_ENCODERS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("cs is %s\r\n"),
+ (DO_CS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("rs is %s\r\n"),
+ (DO_RS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("pos is %s\r\n"),
+ (DO_POS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("bd is %s\r\n"),
+ (DO_BD & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("timer is %s\r\n"),
+ (DO_TIMER & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("power is %s\r\n"),
+ (DO_POWER & mainboard.flags) ? "on":"off");
+ }
+ return;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("encoders")))
+ bit = DO_ENCODERS;
+ else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ strat_hardstop();
+ bit = DO_CS;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("rs")))
+ bit = DO_RS;
+ else if (!strcmp_P(res->arg1, PSTR("pos")))
+ bit = DO_POS;
+ else if (!strcmp_P(res->arg1, PSTR("bd")))
+ bit = DO_BD;
+ else if (!strcmp_P(res->arg1, PSTR("timer"))) {
+ time_reset();
+ bit = DO_TIMER;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("power")))
+ bit = DO_POWER;
+
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ mainboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off"))) {
+ if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ pwm_ng_set(LEFT_PWM, 0);
+ pwm_ng_set(RIGHT_PWM, 0);
+ }
+ mainboard.flags &= (~bit);
+ }
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ (bit & mainboard.flags) ? "on":"off");
+}
+
+prog_char str_event_arg0[] = "event";
+parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
+prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
+parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
+prog_char str_event_arg2[] = "on#off#show";
+parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
+
+prog_char help_event[] = "Enable/disable events";
+parse_pgm_inst_t cmd_event = {
+ .f = cmd_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_event_arg0,
+ (prog_void *)&cmd_event_arg1,
+ (prog_void *)&cmd_event_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Interact */
+
+/* this structure is filled when cmd_interact is parsed successfully */
+struct cmd_interact_result {
+ fixed_string_t arg0;
+};
+
+static void print_cs(void)
+{
+ printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
+ "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
+ cs_get_consign(&mainboard.distance.cs),
+ cs_get_consign(&mainboard.angle.cs),
+ cs_get_filtered_consign(&mainboard.distance.cs),
+ cs_get_filtered_consign(&mainboard.angle.cs),
+ cs_get_error(&mainboard.distance.cs),
+ cs_get_error(&mainboard.angle.cs),
+ cs_get_out(&mainboard.distance.cs),
+ cs_get_out(&mainboard.angle.cs));
+}
+
+static void print_pos(void)
+{
+ printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+}
+
+static void print_time(void)
+{
+ printf_P(PSTR("time %d\r\n"),time_get_s());
+}
+
+
+static void print_sensors(void)
+{
+#ifdef notyet
+ if (sensor_start_switch())
+ printf_P(PSTR("Start switch | "));
+ else
+ printf_P(PSTR(" | "));
+
+ if (IR_DISP_SENSOR())
+ printf_P(PSTR("IR disp | "));
+ else
+ printf_P(PSTR(" | "));
+
+ printf_P(PSTR("\r\n"));
+#endif
+}
+
+static void print_pid(void)
+{
+ printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
+ "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
+ pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
+ pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
+ pid_get_value_out(&mainboard.distance.pid),
+ pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
+ pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
+ pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
+ pid_get_value_out(&mainboard.angle.pid));
+}
+
+#define PRINT_POS (1<<0)
+#define PRINT_PID (1<<1)
+#define PRINT_CS (1<<2)
+#define PRINT_SENSORS (1<<3)
+#define PRINT_TIME (1<<4)
+#define PRINT_BLOCKING (1<<5)
+
+static void cmd_interact_parsed(void * parsed_result, void * data)
+{
+ int c;
+ int8_t cmd;
+ uint8_t print = 0;
+ struct vt100 vt100;
+
+ vt100_init(&vt100);
+
+ printf_P(PSTR("Display debugs:\r\n"
+ " 1:pos\r\n"
+ " 2:pid\r\n"
+ " 3:cs\r\n"
+ " 4:sensors\r\n"
+ " 5:time\r\n"
+ /* " 6:blocking\r\n" */
+ "Commands:\r\n"
+ " arrows:move\r\n"
+ " space:stop\r\n"
+ " q:quit\r\n"));
+
+ /* stop motors */
+ mainboard.flags &= (~DO_CS);
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
+
+ while(1) {
+ if (print & PRINT_POS) {
+ print_pos();
+ }
+
+ if (print & PRINT_PID) {
+ print_pid();
+ }
+
+ if (print & PRINT_CS) {
+ print_cs();
+ }
+
+ if (print & PRINT_SENSORS) {
+ print_sensors();
+ }
+
+ if (print & PRINT_TIME) {
+ print_time();
+ }
+/* if (print & PRINT_BLOCKING) { */
+/* printf_P(PSTR("%s %s blocking=%d\r\n"), */
+/* mainboard.blocking ? "BLOCK1":" ", */
+/* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
+/* rs_get_blocking(&mainboard.rs)); */
+/* } */
+
+ c = cmdline_getchar();
+ if (c == -1) {
+ wait_ms(10);
+ continue;
+ }
+ cmd = vt100_parser(&vt100, c);
+ if (cmd == -2) {
+ wait_ms(10);
+ continue;
+ }
+
+ if (cmd == -1) {
+ switch(c) {
+ case '1': print ^= PRINT_POS; break;
+ case '2': print ^= PRINT_PID; break;
+ case '3': print ^= PRINT_CS; break;
+ case '4': print ^= PRINT_SENSORS; break;
+ case '5': print ^= PRINT_TIME; break;
+ case '6': print ^= PRINT_BLOCKING; break;
+
+ case 'q':
+/* if (mainboard.flags & DO_CS) */
+/* strat_hardstop(); */
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
+ return;
+ case ' ':
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
+ break;
+ default:
+ break;
+ }
+ }
+ else {
+ switch(cmd) {
+ case KEY_UP_ARR:
+ pwm_set_and_save(LEFT_PWM, 1200);
+ pwm_set_and_save(RIGHT_PWM, 1200);
+ break;
+ case KEY_LEFT_ARR:
+ pwm_set_and_save(LEFT_PWM, -1200);
+ pwm_set_and_save(RIGHT_PWM, 1200);
+ break;
+ case KEY_DOWN_ARR:
+ pwm_set_and_save(LEFT_PWM, -1200);
+ pwm_set_and_save(RIGHT_PWM, -1200);
+ break;
+ case KEY_RIGHT_ARR:
+ pwm_set_and_save(LEFT_PWM, 1200);
+ pwm_set_and_save(RIGHT_PWM, -1200);
+ break;
+ }
+ }
+ wait_ms(10);
+ }
+}
+
+prog_char str_interact_arg0[] = "interact";
+parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
+
+prog_char help_interact[] = "Interactive mode";
+parse_pgm_inst_t cmd_interact = {
+ .f = cmd_interact_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_interact,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_interact_arg0,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Rs tests */
+
+/* this structure is filled when cmd_rs is parsed successfully */
+struct cmd_rs_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_rs is parsed successfully */
+static void cmd_rs_parsed(void *parsed_result, void *data)
+{
+ // struct cmd_rs_result *res = parsed_result;
+ do {
+ printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
+ cs_get_consign(&mainboard.angle.cs),
+ cs_get_filtered_feedback(&mainboard.angle.cs),
+ cs_get_out(&mainboard.angle.cs));
+ printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
+ cs_get_consign(&mainboard.distance.cs),
+ cs_get_filtered_feedback(&mainboard.distance.cs),
+ cs_get_out(&mainboard.distance.cs));
+ printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
+ mainboard.pwm_r);
+ wait_ms(100);
+ } while(!cmdline_keypressed());
+}
+
+prog_char str_rs_arg0[] = "rs";
+parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
+prog_char str_rs_arg1[] = "show";
+parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
+
+prog_char help_rs[] = "Show rs (robot system) values";
+parse_pgm_inst_t cmd_rs = {
+ .f = cmd_rs_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_arg0,
+ (prog_void *)&cmd_rs_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Fessors tests */
+
+/* this structure is filled when cmd_fessor is parsed successfully */
+struct cmd_fessor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_fessor is parsed successfully */
+static void cmd_fessor_parsed(void * parsed_result, void * data)
+{
+ struct cmd_fessor_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("up"))) {
+ fessor_up();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("down"))) {
+ fessor_down();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("stop"))) {
+ fessor_stop();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("auto"))) {
+ fessor_auto();
+ }
+
+ printf_P(PSTR("done\r\n"));
+}
+
+prog_char str_fessor_arg0[] = "fessor";
+parse_pgm_token_string_t cmd_fessor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg0, str_fessor_arg0);
+prog_char str_fessor_arg1[] = "auto#up#down#stop";
+parse_pgm_token_string_t cmd_fessor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg1, str_fessor_arg1);
+
+prog_char help_fessor[] = "fessor auto mode: fessor auto delay_up delay_down";
+parse_pgm_inst_t cmd_fessor = {
+ .f = cmd_fessor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_fessor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_fessor_arg0,
+ (prog_void *)&cmd_fessor_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Fessor_Paramss tests */
+
+/* this structure is filled when cmd_fessor_params is parsed successfully */
+struct cmd_fessor_params_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+};
+
+/* function called when cmd_fessor_params is parsed successfully */
+static void cmd_fessor_params_parsed(void * parsed_result, void * data)
+{
+ struct cmd_fessor_params_result * res = parsed_result;
+
+
+ if (!strcmp_P(res->arg1, PSTR("delay"))) {
+ fessor_set_delays(res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("coef"))) {
+ fessor_set_coefs(res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("pos"))) {
+ fessor_set_pos(res->arg2, res->arg3);
+ }
+
+ /* else show */
+ fessor_dump_params();
+}
+
+prog_char str_fessor_params_arg0[] = "fessor_params";
+parse_pgm_token_string_t cmd_fessor_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg0, str_fessor_params_arg0);
+prog_char str_fessor_params_arg1[] = "delay#pos#coef";
+parse_pgm_token_string_t cmd_fessor_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1);
+parse_pgm_token_num_t cmd_fessor_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg2, INT32);
+parse_pgm_token_num_t cmd_fessor_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg3, INT32);
+
+prog_char help_fessor_params[] = "Set fessor_params values";
+parse_pgm_inst_t cmd_fessor_params = {
+ .f = cmd_fessor_params_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_fessor_params,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_fessor_params_arg0,
+ (prog_void *)&cmd_fessor_params_arg1,
+ (prog_void *)&cmd_fessor_params_arg2,
+ (prog_void *)&cmd_fessor_params_arg3,
+ NULL,
+ },
+};
+
+prog_char str_fessor_params_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_fessor_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1_show);
+
+prog_char help_fessor_params_show[] = "show fessor params";
+parse_pgm_inst_t cmd_fessor_params_show = {
+ .f = cmd_fessor_params_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_fessor_params_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_fessor_params_arg0,
+ (prog_void *)&cmd_fessor_params_arg1_show,
+ NULL,
+ },
+};
+
+
+
+
+/**********************************************************/
+/* Elevators tests */
+
+/* this structure is filled when cmd_elevator is parsed successfully */
+struct cmd_elevator_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_elevator is parsed successfully */
+static void cmd_elevator_parsed(void * parsed_result, void * data)
+{
+ struct cmd_elevator_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("up"))) {
+ elevator_up();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("down"))) {
+ elevator_down();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("stop"))) {
+ elevator_stop();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("auto"))) {
+ elevator_auto();
+ }
+
+ printf_P(PSTR("done\r\n"));
+}
+
+prog_char str_elevator_arg0[] = "elevator";
+parse_pgm_token_string_t cmd_elevator_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg0, str_elevator_arg0);
+prog_char str_elevator_arg1[] = "auto#up#down#stop";
+parse_pgm_token_string_t cmd_elevator_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg1, str_elevator_arg1);
+
+prog_char help_elevator[] = "elevator auto mode: elevator auto delay_up delay_down";
+parse_pgm_inst_t cmd_elevator = {
+ .f = cmd_elevator_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_elevator,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_elevator_arg0,
+ (prog_void *)&cmd_elevator_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Elevator_Paramss tests */
+
+/* this structure is filled when cmd_elevator_params is parsed successfully */
+struct cmd_elevator_params_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+};
+
+/* function called when cmd_elevator_params is parsed successfully */
+static void cmd_elevator_params_parsed(void * parsed_result, void * data)
+{
+ struct cmd_elevator_params_result * res = parsed_result;
+
+
+ if (!strcmp_P(res->arg1, PSTR("delay"))) {
+ elevator_set_delays(res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("coef"))) {
+ elevator_set_coefs(res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("pos"))) {
+ elevator_set_pos(res->arg2, res->arg3);
+ }
+
+ /* else show */
+ elevator_dump_params();
+}
+
+prog_char str_elevator_params_arg0[] = "elevator_params";
+parse_pgm_token_string_t cmd_elevator_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg0, str_elevator_params_arg0);
+prog_char str_elevator_params_arg1[] = "delay#pos#coef";
+parse_pgm_token_string_t cmd_elevator_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1);
+parse_pgm_token_num_t cmd_elevator_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg2, INT32);
+parse_pgm_token_num_t cmd_elevator_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg3, INT32);
+
+prog_char help_elevator_params[] = "Set elevator_params values";
+parse_pgm_inst_t cmd_elevator_params = {
+ .f = cmd_elevator_params_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_elevator_params,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_elevator_params_arg0,
+ (prog_void *)&cmd_elevator_params_arg1,
+ (prog_void *)&cmd_elevator_params_arg2,
+ (prog_void *)&cmd_elevator_params_arg3,
+ NULL,
+ },
+};
+
+prog_char str_elevator_params_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_elevator_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1_show);
+
+prog_char help_elevator_params_show[] = "show elevator params";
+parse_pgm_inst_t cmd_elevator_params_show = {
+ .f = cmd_elevator_params_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_elevator_params_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_elevator_params_arg0,
+ (prog_void *)&cmd_elevator_params_arg1_show,
+ 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)
+{
+}
+
+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 (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_traj.c,v 1.6 2009-05-02 10:08:09 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <encoders_microb.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "main.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "cs.h"
+#include "cmdline.h"
+
+/**********************************************************/
+/* Traj_Speeds for trajectory_manager */
+
+/* this structure is filled when cmd_traj_speed is parsed successfully */
+struct cmd_traj_speed_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint16_t s;
+};
+
+/* function called when cmd_traj_speed is parsed successfully */
+static void cmd_traj_speed_parsed(void *parsed_result, void *data)
+{
+ struct cmd_traj_speed_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("angle"))) {
+ trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+ trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
+ }
+ /* else it is a "show" */
+
+ printf_P(PSTR("angle %u, distance %u\r\n"),
+ mainboard.traj.a_speed,
+ mainboard.traj.d_speed);
+}
+
+prog_char str_traj_speed_arg0[] = "traj_speed";
+parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
+prog_char str_traj_speed_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
+parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
+
+prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
+parse_pgm_inst_t cmd_traj_speed = {
+ .f = cmd_traj_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_traj_speed,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_traj_speed_arg0,
+ (prog_void *)&cmd_traj_speed_arg1,
+ (prog_void *)&cmd_traj_speed_s,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_traj_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
+
+prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
+parse_pgm_inst_t cmd_traj_speed_show = {
+ .f = cmd_traj_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_traj_speed_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_traj_speed_arg0,
+ (prog_void *)&cmd_traj_speed_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* trajectory window configuration */
+
+/* this structure is filled when cmd_trajectory is parsed successfully */
+struct cmd_trajectory_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float d_win;
+ float a_win;
+ float a_start;
+};
+
+
+/* function called when cmd_trajectory is parsed successfully */
+static void cmd_trajectory_parsed(void * parsed_result, void * data)
+{
+ struct cmd_trajectory_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ trajectory_set_windows(&mainboard.traj, res->d_win,
+ res->a_win, res->a_start);
+ }
+
+ printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
+ DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
+}
+
+prog_char str_trajectory_arg0[] = "trajectory";
+parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
+prog_char str_trajectory_arg1[] = "set";
+parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
+parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
+parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
+parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
+
+prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
+parse_pgm_inst_t cmd_trajectory = {
+ .f = cmd_trajectory_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_trajectory,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_trajectory_arg0,
+ (prog_void *)&cmd_trajectory_arg1,
+ (prog_void *)&cmd_trajectory_d,
+ (prog_void *)&cmd_trajectory_a,
+ (prog_void *)&cmd_trajectory_as,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_trajectory_show_arg[] = "show";
+parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
+
+prog_char help_trajectory_show[] = "Show trajectory window configuration";
+parse_pgm_inst_t cmd_trajectory_show = {
+ .f = cmd_trajectory_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_trajectory_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_trajectory_arg0,
+ (prog_void *)&cmd_trajectory_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* rs_gains configuration */
+
+/* this structure is filled when cmd_rs_gains is parsed successfully */
+struct cmd_rs_gains_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float left;
+ float right;
+};
+
+/* function called when cmd_rs_gains is parsed successfully */
+static void cmd_rs_gains_parsed(void * parsed_result, void * data)
+{
+ struct cmd_rs_gains_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value,
+ LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
+ rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value,
+ RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
+ }
+ printf_P(PSTR("rs_gains set %f %f"),
+ mainboard.rs.left_ext_gain,
+ mainboard.rs.right_ext_gain);
+/* f64_print(mainboard.rs.left_ext_gain); */
+/* printf_P(PSTR(" ")); */
+/* f64_print(mainboard.rs.right_ext_gain); */
+ printf_P(PSTR("\r\n"));
+}
+
+prog_char str_rs_gains_arg0[] = "rs_gains";
+parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
+prog_char str_rs_gains_arg1[] = "set";
+parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
+parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
+parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
+
+prog_char help_rs_gains[] = "Set rs_gains (left, right)";
+parse_pgm_inst_t cmd_rs_gains = {
+ .f = cmd_rs_gains_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs_gains,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_arg1,
+ (prog_void *)&cmd_rs_gains_l,
+ (prog_void *)&cmd_rs_gains_r,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_rs_gains_show_arg[] = "show";
+parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
+
+prog_char help_rs_gains_show[] = "Show rs_gains";
+parse_pgm_inst_t cmd_rs_gains_show = {
+ .f = cmd_rs_gains_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs_gains_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_show_arg,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* track configuration */
+
+/* this structure is filled when cmd_track is parsed successfully */
+struct cmd_track_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float val;
+};
+
+/* function called when cmd_track is parsed successfully */
+static void cmd_track_parsed(void * parsed_result, void * data)
+{
+ struct cmd_track_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
+ }
+ printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
+}
+
+prog_char str_track_arg0[] = "track";
+parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
+prog_char str_track_arg1[] = "set";
+parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
+parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
+
+prog_char help_track[] = "Set track in mm";
+parse_pgm_inst_t cmd_track = {
+ .f = cmd_track_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_track,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_track_arg0,
+ (prog_void *)&cmd_track_arg1,
+ (prog_void *)&cmd_track_val,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_track_show_arg[] = "show";
+parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
+
+prog_char help_track_show[] = "Show track";
+parse_pgm_inst_t cmd_track_show = {
+ .f = cmd_track_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_track_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_track_arg0,
+ (prog_void *)&cmd_track_show_arg,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* Pt_Lists for testing traj */
+
+#define PT_LIST_SIZE 10
+static struct xy_point pt_list[PT_LIST_SIZE];
+static uint16_t pt_list_len = 0;
+
+/* this structure is filled when cmd_pt_list is parsed successfully */
+struct cmd_pt_list_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint16_t arg2;
+ int16_t arg3;
+ int16_t arg4;
+};
+
+/* function called when cmd_pt_list is parsed successfully */
+static void cmd_pt_list_parsed(void * parsed_result, void * data)
+{
+ struct cmd_pt_list_result * res = parsed_result;
+ uint8_t i, why=0;
+
+ if (!strcmp_P(res->arg1, PSTR("append"))) {
+ res->arg2 = pt_list_len;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("insert")) ||
+ !strcmp_P(res->arg1, PSTR("append"))) {
+ if (res->arg2 > pt_list_len) {
+ printf_P(PSTR("Index too large\r\n"));
+ return;
+ }
+ if (pt_list_len >= PT_LIST_SIZE) {
+ printf_P(PSTR("List is too large\r\n"));
+ return;
+ }
+ memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
+ PT_LIST_SIZE-1-res->arg2);
+ pt_list[res->arg2].x = res->arg3;
+ pt_list[res->arg2].y = res->arg4;
+ pt_list_len++;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("del"))) {
+ if (pt_list_len <= 0) {
+ printf_P(PSTR("Error: list empty\r\n"));
+ return;
+ }
+ if (res->arg2 > pt_list_len) {
+ printf_P(PSTR("Index too large\r\n"));
+ return;
+ }
+ memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
+ (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
+ pt_list_len--;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ pt_list_len = 0;
+ }
+
+ /* else it is a "show" or a "start" */
+ if (pt_list_len == 0) {
+ printf_P(PSTR("List empty\r\n"));
+ return;
+ }
+ for (i=0 ; i<pt_list_len ; i++) {
+ printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
+ if (!strcmp_P(res->arg1, PSTR("start"))) {
+ trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
+ why = wait_traj_end(0xFF); /* all */
+ }
+ else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
+ while (1) {
+ //why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
+ printf("next point\r\n");
+ if (why != END_OBSTACLE)
+ break;
+ }
+ }
+ if (why & (~(END_TRAJ | END_NEAR)))
+ trajectory_stop(&mainboard.traj);
+ }
+}
+
+prog_char str_pt_list_arg0[] = "pt_list";
+parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
+prog_char str_pt_list_arg1[] = "insert";
+parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
+parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
+parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
+parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
+
+prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
+parse_pgm_inst_t cmd_pt_list = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_arg1,
+ (prog_void *)&cmd_pt_list_arg2,
+ (prog_void *)&cmd_pt_list_arg3,
+ (prog_void *)&cmd_pt_list_arg4,
+ NULL,
+ },
+};
+
+/* append */
+
+prog_char str_pt_list_arg1_append[] = "append";
+parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
+
+prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
+parse_pgm_inst_t cmd_pt_list_append = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list_append,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_arg1_append,
+ (prog_void *)&cmd_pt_list_arg3,
+ (prog_void *)&cmd_pt_list_arg4,
+ NULL,
+ },
+};
+
+/* del */
+
+prog_char str_pt_list_del_arg[] = "del";
+parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
+
+prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
+parse_pgm_inst_t cmd_pt_list_del = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list_del,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_del_arg,
+ (prog_void *)&cmd_pt_list_arg2,
+ NULL,
+ },
+};
+/* show */
+
+prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
+parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
+
+prog_char help_pt_list_show[] = "Show, start or reset pt_list";
+parse_pgm_inst_t cmd_pt_list_show = {
+ .f = cmd_pt_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pt_list_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_show_arg,
+ NULL,
+ },
+};
+
+
+
+/**********************************************************/
+/* Goto function */
+
+/* this structure is filled when cmd_goto is parsed successfully */
+struct cmd_goto_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+ int32_t arg4;
+};
+
+/* function called when cmd_goto is parsed successfully */
+static void cmd_goto_parsed(void * parsed_result, void * data)
+{
+ struct cmd_goto_result * res = parsed_result;
+ uint8_t err;
+ microseconds t1, t2;
+
+ interrupt_traj_reset();
+ if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
+ trajectory_a_rel(&mainboard.traj, res->arg2);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
+ trajectory_d_rel(&mainboard.traj, res->arg2);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
+ trajectory_a_abs(&mainboard.traj, res->arg2);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
+ trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
+ trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
+ trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
+ trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
+ //err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
+ err=0;
+ if (err != END_TRAJ && err != END_NEAR)
+ strat_hardstop();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
+ //err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
+ err=0;
+ if (err != END_TRAJ && err != END_NEAR)
+ strat_hardstop();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
+ trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
+ trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
+ trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
+ }
+ t1 = time_get_us2();
+ while ((err = test_traj_end(0xFF)) == 0) {
+ t2 = time_get_us2();
+ if (t2 - t1 > 200000) {
+ dump_cs_debug("angle", &mainboard.angle.cs);
+ dump_cs_debug("distance", &mainboard.distance.cs);
+ t1 = t2;
+ }
+ }
+ if (err != END_TRAJ && err != END_NEAR)
+ strat_hardstop();
+ printf_P(PSTR("returned %s\r\n"), get_err(err));
+}
+
+prog_char str_goto_arg0[] = "goto";
+parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
+prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
+parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
+parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
+
+/* 1 params */
+prog_char help_goto1[] = "Change orientation of the mainboard";
+parse_pgm_inst_t cmd_goto1 = {
+ .f = cmd_goto_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_goto1,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_goto_arg0,
+ (prog_void *)&cmd_goto_arg1_a,
+ (prog_void *)&cmd_goto_arg2,
+ NULL,
+ },
+};
+
+prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy";
+parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
+parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
+
+/* 2 params */
+prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
+parse_pgm_inst_t cmd_goto2 = {
+ .f = cmd_goto_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_goto2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_goto_arg0,
+ (prog_void *)&cmd_goto_arg1_b,
+ (prog_void *)&cmd_goto_arg2,
+ (prog_void *)&cmd_goto_arg3,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Position tests */
+
+/* this structure is filled when cmd_position is parsed successfully */
+struct cmd_position_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+ int32_t arg4;
+};
+
+#define AUTOPOS_SPEED_FAST 200
+static void auto_position(void)
+{
+ uint8_t err;
+ uint16_t old_spdd, old_spda;
+
+ interrupt_traj_reset();
+ strat_get_speed(&old_spdd, &old_spda);
+ strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
+
+ trajectory_d_rel(&mainboard.traj, -300);
+ err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+ strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
+
+ trajectory_d_rel(&mainboard.traj, 120);
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+
+ trajectory_a_rel(&mainboard.traj, 90);
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+
+ trajectory_d_rel(&mainboard.traj, -300);
+ err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+ strat_reset_pos(DO_NOT_SET_POS, ROBOT_LENGTH/2,
+ 90);
+
+ trajectory_d_rel(&mainboard.traj, 120);
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+
+ trajectory_a_rel(&mainboard.traj, -40);
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (err == END_INTR)
+ goto intr;
+ wait_ms(100);
+
+ strat_set_speed(old_spdd, old_spda);
+ return;
+
+intr:
+ strat_hardstop();
+ strat_set_speed(old_spdd, old_spda);
+}
+
+/* function called when cmd_position is parsed successfully */
+static void cmd_position_parsed(void * parsed_result, void * data)
+{
+ struct cmd_position_result * res = parsed_result;
+
+ /* display raw position values */
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ position_set(&mainboard.pos, 0, 0, 0);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("set"))) {
+ position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
+ auto_position();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
+ auto_position();
+ }
+
+ /* else it's just a "show" */
+ printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
+ position_get_x_double(&mainboard.pos),
+ position_get_y_double(&mainboard.pos),
+ DEG(position_get_a_rad_double(&mainboard.pos)));
+}
+
+prog_char str_position_arg0[] = "position";
+parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
+prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
+parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
+
+prog_char help_position[] = "Show/reset (x,y,a) position";
+parse_pgm_inst_t cmd_position = {
+ .f = cmd_position_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_position,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_position_arg0,
+ (prog_void *)&cmd_position_arg1,
+ NULL,
+ },
+};
+
+
+prog_char str_position_arg1_set[] = "set";
+parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
+parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
+parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
+parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
+
+prog_char help_position_set[] = "Set (x,y,a) position";
+parse_pgm_inst_t cmd_position_set = {
+ .f = cmd_position_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_position_set,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_position_arg0,
+ (prog_void *)&cmd_position_arg1_set,
+ (prog_void *)&cmd_position_arg2,
+ (prog_void *)&cmd_position_arg3,
+ (prog_void *)&cmd_position_arg4,
+ NULL,
+ },
+};
+
--- /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.7 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+#include <aversive/wait.h>
+
+#include <encoders_microb.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "actuator.h"
+
+static int32_t wheel_speed = 0;
+
+static int32_t wheel_get_value(void * dummy)
+{
+ return wheel_speed;
+}
+
+static void wheel_set(void *dummy, int32_t val)
+{
+ if (val < 0)
+ val = 0;
+ pwm_ng_set(WHEEL_PWM, val);
+}
+
+static void wheel_update_value(void)
+{
+ static int32_t prev = 0;
+ int32_t val;
+
+ val = encoders_microb_get_value(WHEEL_ENC);
+ wheel_speed = val - prev;
+ prev = val;
+}
+
+/* called every 5 ms */
+static void do_cs(void *dummy)
+{
+ static uint16_t cpt = 0;
+ static int32_t old_a = 0, old_d = 0;
+
+ wheel_update_value();
+
+ /* robot system, conversion to angle,distance */
+ if (mainboard.flags & DO_RS) {
+ int16_t a,d;
+ rs_update(&mainboard.rs); /* takes about 0.5 ms */
+ /* process and store current speed */
+ a = rs_get_angle(&mainboard.rs);
+ d = rs_get_distance(&mainboard.rs);
+ mainboard.speed_a = a - old_a;
+ mainboard.speed_d = d - old_d;
+ old_a = a;
+ old_d = d;
+ }
+
+ /* control system */
+ if (mainboard.flags & DO_CS) {
+ if (mainboard.angle.on)
+ cs_manage(&mainboard.angle.cs);
+ if (mainboard.distance.on)
+ cs_manage(&mainboard.distance.cs);
+ if (mainboard.fessor.on)
+ cs_manage(&mainboard.fessor.cs);
+ if (mainboard.wheel.on)
+ cs_manage(&mainboard.wheel.cs);
+ if (mainboard.elevator.on)
+ cs_manage(&mainboard.elevator.cs);
+
+ fessor_manage();
+ elevator_manage();
+ }
+ if ((cpt & 1) && (mainboard.flags & DO_POS)) {
+ /* about 1.5ms (worst case without centrifugal force
+ * compensation) */
+ position_manage(&mainboard.pos);
+ }
+ if (mainboard.flags & DO_BD) {
+ bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
+ bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
+ }
+ if (mainboard.flags & DO_TIMER) {
+ uint8_t second;
+ /* the robot should stop correctly in the strat, but
+ * in some cases, we must force the stop from an
+ * interrupt */
+ second = time_get_s();
+ if (second >= MATCH_TIME + 2) {
+ pwm_ng_set(LEFT_PWM, 0);
+ pwm_ng_set(RIGHT_PWM, 0);
+ printf_P(PSTR("END OF TIME\r\n"));
+ while(1);
+ }
+ }
+ /* brakes */
+ if (mainboard.flags & DO_POWER)
+ BRAKE_OFF();
+ else
+ BRAKE_ON();
+ cpt++;
+}
+
+void dump_cs_debug(const char *name, struct cs *cs)
+{
+ DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "
+ "in=% .5ld out=% .5ld",
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_cs(const char *name, struct cs *cs)
+{
+ printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
+ "in=% .5ld out=% .5ld\r\n"),
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_pid(const char *name, struct pid_filter *pid)
+{
+ printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ name,
+ pid_get_value_in(pid) * pid_get_gain_P(pid),
+ pid_get_value_I(pid) * pid_get_gain_I(pid),
+ pid_get_value_D(pid) * pid_get_gain_D(pid),
+ pid_get_value_out(pid));
+}
+
+void microb_cs_init(void)
+{
+ /* ROBOT_SYSTEM */
+ rs_init(&mainboard.rs);
+ rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
+ rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM);
+ /* increase gain to decrease dist, increase left and it will turn more left */
+ rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value,
+ LEFT_ENCODER, IMP_COEF * -1.0000);
+ rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value,
+ RIGHT_ENCODER, IMP_COEF * 1.0000);
+ /* rs will use external encoders */
+ rs_set_flags(&mainboard.rs, RS_USE_EXT);
+
+ /* POSITION MANAGER */
+ position_init(&mainboard.pos);
+ position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
+ position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
+ //position_set_centrifugal_coef(&mainboard.pos, 0.000016);
+ position_use_ext(&mainboard.pos);
+
+ /* TRAJECTORY MANAGER */
+ trajectory_init(&mainboard.traj);
+ trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
+ &mainboard.angle.cs);
+ trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
+ trajectory_set_speed(&mainboard.traj, 1500, 1500); /* d, a */
+ /* distance window, angle window, angle start */
+ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
+
+ /* ---- CS angle */
+ /* PID */
+ pid_init(&mainboard.angle.pid);
+ pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
+ pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095);
+ pid_set_out_shift(&mainboard.angle.pid, 10);
+ pid_set_derivate_filter(&mainboard.angle.pid, 4);
+
+ /* QUADRAMP */
+ quadramp_init(&mainboard.angle.qr);
+ quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */
+ quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */
+
+ /* CS */
+ cs_init(&mainboard.angle.cs);
+ cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr);
+ cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid);
+ cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs);
+ cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs);
+ cs_set_consign(&mainboard.angle.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&mainboard.angle.bd);
+ bd_set_speed_threshold(&mainboard.angle.bd, 80);
+ bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50);
+
+ /* ---- CS distance */
+ /* PID */
+ pid_init(&mainboard.distance.pid);
+ pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
+ pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
+ pid_set_out_shift(&mainboard.distance.pid, 10);
+ pid_set_derivate_filter(&mainboard.distance.pid, 6);
+
+ /* QUADRAMP */
+ quadramp_init(&mainboard.distance.qr);
+ quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */
+ quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */
+
+ /* CS */
+ cs_init(&mainboard.distance.cs);
+ cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr);
+ cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid);
+ cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs);
+ cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs);
+ cs_set_consign(&mainboard.distance.cs, 0);
+
+ /* Blocking detection */
+ bd_init(&mainboard.distance.bd);
+ bd_set_speed_threshold(&mainboard.distance.bd, 60);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
+
+ /* ---- CS fessor */
+
+ fessor_autopos();
+ /* PID */
+ pid_init(&mainboard.fessor.pid);
+ pid_set_gains(&mainboard.fessor.pid, 300, 10, 150);
+ pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095);
+ pid_set_out_shift(&mainboard.fessor.pid, 10);
+ pid_set_derivate_filter(&mainboard.fessor.pid, 4);
+
+ /* CS */
+ cs_init(&mainboard.fessor.cs);
+ cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid);
+ cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL);
+ cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC);
+ fessor_up();
+
+
+
+ /* ---- CS elevator */
+
+ elevator_autopos();
+ /* PID */
+ pid_init(&mainboard.elevator.pid);
+ pid_set_gains(&mainboard.elevator.pid, 300, 10, 150);
+ pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095);
+ pid_set_out_shift(&mainboard.elevator.pid, 10);
+ pid_set_derivate_filter(&mainboard.elevator.pid, 4);
+
+ /* CS */
+ cs_init(&mainboard.elevator.cs);
+ cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid);
+ cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL);
+ cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC);
+ elevator_down();
+
+ /* ---- CS wheel */
+
+ /* PID */
+ pid_init(&mainboard.wheel.pid);
+ pid_set_gains(&mainboard.wheel.pid, 100, 100, 0);
+ pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095);
+ pid_set_out_shift(&mainboard.wheel.pid, 5);
+ pid_set_derivate_filter(&mainboard.wheel.pid, 4);
+
+ /* CS */
+ cs_init(&mainboard.wheel.cs);
+ cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid);
+ cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL);
+ cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL);
+ cs_set_consign(&mainboard.wheel.cs, 1000);
+
+ /* set them on !! */
+ mainboard.angle.on = 0;
+ mainboard.distance.on = 0;
+ mainboard.fessor.on = 1;
+ mainboard.elevator.on = 1;
+ mainboard.wheel.on = 1;
+ mainboard.flags |= DO_CS;
+
+ 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.3 2009-03-29 18:42:41 zer0 Exp $
+ *
+ */
+
+void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_cs_debug(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
--- /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-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _DEBUG_CONFIG_
+#define _DEBUG_CONFIG_ 1.0 // version
+
+
+/** port line definition for the show_int_loop() function */
+/* undefine it to disable this functionnality */
+#define INTERRUPT_SHOW_PORT PORTA
+#define INTERRUPT_SHOW_BIT 3
+
+
+
+/** memory mark for the min_stack_space_available() function
+ the ram is filled with this value after a reset ! */
+#define MARK 0x55
+
+/** the mark is inserted in whole RAM if this is enabled
+ (could lead to problems if you need to hold values through a reset...)
+ so it's better to disable it.
+ stack counting is not affected */
+//#define DIAG_FILL_ENTIRE_RAM
+
+
+#endif //_DEBUG_CONFIG_
--- /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: encoders_microb_config.h,v 1.3.4.1 2006-11-26 21:06:04 zer0 Exp $
+ *
+ */
+#ifndef _ENCODERS_MICROB_CONFIG_H_
+#define _ENCODERS_MICROB_CONFIG_H_
+
+/** number of the LAST encoders used + 1 */
+#define ENCODERS_MICROB_NUMBER 4
+
+#define ENCODERS_MICROB_SEL_PORT PORTB
+#define ENCODERS_MICROB_SEL_BIT 0
+
+#define ENCODERS_MICROB0_ENABLED
+#define ENCODERS_MICROB0_PIN PINA
+
+#define ENCODERS_MICROB1_ENABLED
+#define ENCODERS_MICROB1_PIN PINA
+
+#define ENCODERS_MICROB2_ENABLED
+#define ENCODERS_MICROB2_PIN PINC
+
+#define ENCODERS_MICROB3_ENABLED
+#define ENCODERS_MICROB3_PIN PINC
+
+
+// ... until 7
+
+#endif
--- /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-02-20 21:10:01 zer0 Exp $
+ *
+ */
+#ifndef _ENCODERS_SPI_CONFIG_H_
+#define _ENCODERS_SPI_CONFIG_H_
+
+#define ENCODERS_SPI_NUMBER 4
+#define ENCODERS_SPI_SS_PORT SS_PORT /* PORTB on atmega2560 */
+#define ENCODERS_SPI_SS_BIT SS_BIT /* 0 on atmega2560 */
+
+/* see spi configuration */
+#define ENCODERS_SPI_CLK_RATE SPI_CLK_RATE_16
+#define ENCODERS_SPI_FORMAT SPI_FORMAT_3
+#define ENCODERS_SPI_DATA_ORDER SPI_LSB_FIRST
+
+#endif
--- /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-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _ERROR_CONFIG_
+#define _ERROR_CONFIG_
+
+/** enable the dump of the comment */
+#define ERROR_DUMP_TEXTLOG
+
+/** enable the dump of filename and line number */
+#define ERROR_DUMP_FILE_LINE
+
+#endif
--- /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.8 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <avr/eeprom.h>
+
+#include <uart.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+#include <adc.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+#include <encoders_microb.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "sensor.h"
+#include "actuator.h"
+#include "cs.h"
+
+#if __AVR_LIBC_VERSION__ == 10602UL
+//#error "won't work with this version"
+#endif
+
+/* 0 means "programmed"
+ * ---- with 16 Mhz quartz
+ * CKSEL 3-0 : 0111
+ * 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 mainboard mainboard;
+
+void do_led_blink(void *dummy)
+{
+#if 1 /* simple blink */
+ LED1_TOGGLE();
+#endif
+}
+
+static void main_timer_interrupt(void)
+{
+ static uint8_t cpt = 0;
+ static uint8_t encoder_running = 0;
+
+ cpt++;
+
+ /* BAAAAd */
+ if (encoder_running)
+ return;
+
+ encoder_running = 1;
+ sei();
+
+ encoders_microb_manage(NULL);
+ encoder_running = 0;
+
+ if ((cpt & 0x3) == 0)
+ scheduler_interrupt();
+}
+
+int main(void)
+{
+ /* LEDs */
+ DDRG = 0x18;
+ DDRB = 0x10;
+
+ /* brake */
+ BRAKE_DDR();
+ BRAKE_OFF();
+
+ LED1_ON();
+ LED2_ON();
+ LED3_ON();
+
+ memset(&gen, 0, sizeof(gen));
+ memset(&mainboard, 0, sizeof(mainboard));
+ mainboard.flags = DO_ENCODERS | DO_RS |
+ DO_POS | DO_POWER | DO_BD;
+
+ /* UART */
+ uart_init();
+#if CMDLINE_UART == 0
+ fdevopen(uart0_dev_send, uart0_dev_recv);
+ uart_register_rx_event(0, emergency);
+#elif CMDLINE_UART == 1
+ fdevopen(uart1_dev_send, uart1_dev_recv);
+ uart_register_rx_event(1, emergency);
+#else
+# error not supported
+#endif
+
+ /* LOGS */
+ error_register_emerg(mylog);
+ error_register_error(mylog);
+ error_register_warning(mylog);
+ error_register_notice(mylog);
+ error_register_debug(mylog);
+
+ encoders_microb_init();
+
+ /* 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_INIT16(&gen.pwm1_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
+ &PORTG, 0);
+ PWM_NG_INIT16(&gen.pwm2_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
+ &PORTG, 1);
+
+
+ /* servos */
+ PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
+ TIMER1_PRESCALER_DIV_1);
+ PWM_NG_INIT16(&gen.pwm3_3A, 3, A, 10, PWM_NG_MODE_SIGNED,
+ &PORTG, 2);
+ PWM_NG_INIT16(&gen.pwm4_3B, 3, B, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED,
+ &PORTD, 7);
+
+ /* 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();
+ fessor_init();
+ elevator_init();
+
+ /* TIME */
+ time_init(TIME_PRIO);
+
+ /* strat */
+ gen.logs[0] = E_USER_STRAT;
+ gen.log_level = 5;
+
+ sei();
+
+ printf_P(PSTR("\r\n"));
+ printf_P(PSTR("Respekt Robustheit!\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.8 2009-05-02 10:08:09 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(PORTB, 4)
+#define LED1_OFF() cbi(PORTB, 4)
+#define LED1_TOGGLE() LED_TOGGLE(PORTB, 4)
+
+#define LED2_ON() sbi(PORTG, 3)
+#define LED2_OFF() cbi(PORTG, 3)
+#define LED2_TOGGLE() LED_TOGGLE(PORTG, 3)
+
+#define LED3_ON() sbi(PORTG, 4)
+#define LED3_OFF() cbi(PORTG, 4)
+#define LED3_TOGGLE() LED_TOGGLE(PORTG, 4)
+
+#define BRAKE_DDR() do { DDRF |= 0x01; } while(0)
+#define BRAKE_ON() do { PORTF |= 0x01; } while(0)
+#define BRAKE_OFF() do { PORTF &= 0xFE; } while(0)
+
+/* only 90 seconds, don't forget it :) */
+#define MATCH_TIME 89
+
+/* decrease track to decrease angle */
+#define EXT_TRACK_MM 302.0188
+#define VIRTUAL_TRACK_MM EXT_TRACK_MM
+
+#define ROBOT_LENGTH 320
+#define ROBOT_WIDTH 320
+
+/* it is a 2048 imps -> 8192 because we see 1/4 period
+ * and diameter: 55mm -> perimeter 173mm
+ * 8192/173 -> 473 */
+/* increase it to go further */
+#define IMP_ENCODERS 2048
+#define WHEEL_DIAMETER_MM 55.0
+#define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI)
+#define IMP_COEF 10.
+#define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF)
+
+#define LEFT_ENCODER ((void *)1)
+#define RIGHT_ENCODER ((void *)0)
+
+#define LEFT_PWM ((void *)&gen.pwm3_3A)
+#define RIGHT_PWM ((void *)&gen.pwm4_3B)
+
+#define FESSOR_PWM ((void *)&gen.pwm2_1B)
+#define FESSOR_ENC ((void *)2)
+
+#define WHEEL_PWM ((void *)&gen.pwm1_1A)
+#define WHEEL_ENC ((void *)3)
+
+#define ELEVATOR_PWM ((void *)&gen.pwm3_3A)
+#define ELEVATOR_ENC ((void *)0)
+
+/** ERROR NUMS */
+#define E_USER_STRAT 194
+#define E_USER_I2C_PROTO 195
+#define E_USER_SENSOR 196
+#define E_USER_CS 197
+
+#define LED_PRIO 170
+#define TIME_PRIO 160
+#define ADC_PRIO 120
+#define FESSOR_PRIO 130
+#define ELEVATOR_PRIO 130
+#define CS_PRIO 100
+#define STRAT_PRIO 30
+#define I2C_POLL_PRIO 20
+#define EEPROM_TIME_PRIO 10
+
+#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_1A;
+ struct pwm_ng pwm2_1B;
+ struct pwm_ng pwm3_3A;
+ struct pwm_ng pwm4_3B;
+
+ /* 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;
+};
+
+/* mainboard specific */
+struct mainboard {
+#define DO_ENCODERS 1
+#define DO_CS 2
+#define DO_RS 4
+#define DO_POS 8
+#define DO_BD 16
+#define DO_TIMER 32
+#define DO_POWER 64
+ uint8_t flags; /* misc flags */
+
+ /* control systems */
+ struct cs_block angle;
+ struct cs_block distance;
+ struct cs_block fessor;
+ struct cs_block wheel;
+ struct cs_block elevator;
+
+ /* x,y positionning */
+ struct robot_system rs;
+ struct robot_position pos;
+ struct trajectory traj;
+
+ /* robot status */
+ uint8_t our_color;
+ volatile int16_t speed_a; /* current angle speed */
+ volatile int16_t speed_d; /* current dist speed */
+ int32_t pwm_l; /* current left pwm */
+ int32_t pwm_r; /* current right pwm */
+ uint8_t enable_pickup_wheels; /* these PWM are on sensorboard */
+
+};
+
+/* state of mechboard, synchronized through i2c */
+struct mechboard {
+ uint8_t mode;
+ uint8_t status;
+ int8_t lintel_count;
+ uint8_t column_flags;
+
+ /* pwm */
+ int16_t pump_left1;
+ int16_t pump_right1;
+ int16_t pump_left2;
+ int16_t pump_right2;
+
+ /* currents (for left arm, we can just read it on adc) */
+ int16_t pump_right1_current;
+ int16_t pump_right2_current;
+
+ /* pwm for lintel servos */
+ uint16_t servo_lintel_left;
+ uint16_t servo_lintel_right;
+};
+
+/* state of sensorboard, synchronized through i2c */
+struct sensorboard {
+ uint8_t status;
+ /* opponent pos */
+ int16_t opponent_x;
+ int16_t opponent_y;
+ int16_t opponent_a;
+ int16_t opponent_d;
+
+ /* scanner */
+#define I2C_SCAN_DONE 1
+ uint8_t scan_status;
+#define I2C_COLUMN_NO_DROPZONE -1
+ int8_t dropzone_h;
+ int16_t dropzone_x;
+ int16_t dropzone_y;
+};
+
+extern struct genboard gen;
+extern struct mainboard mainboard;
+extern struct mechboard mechboard;
+extern struct sensorboard sensorboard;
+
+/* 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; \
+ } \
+ } \
+ if (__ret) \
+ DEBUG(E_USER_STRAT, "cond is true at line %d",\
+ __LINE__); \
+ else \
+ DEBUG(E_USER_STRAT, "timeout at line %d", \
+ __LINE__); \
+ \
+ __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-03-15 20:18:33 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 10
+
+
+#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-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <stdlib.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <adc.h>
+#include <scheduler.h>
+#include <pwm_ng.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_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 },
+/* [ADC_BATTERY1] = { .config = ADC_CONF(8), .filter = rii_strong }, */
+/* [ADC_BATTERY2] = { .config = ADC_CONF(9), .filter = rii_strong }, */
+
+ /* 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 1,5,6,7,8
+ */
+static struct sensor_filter sensor_filter[SENSOR_MAX] = {
+ [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
+ [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
+ [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */
+ [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */
+ [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 0 */
+ [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */
+ [S_DISP_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 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 |= ((PINE & _BV(5)) >> 5);
+
+/* 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);
+}
+
+/* virtual obstacle */
+
+#define DISABLE_CPT_MAX 200
+static uint8_t disable = 0; /* used to disable obstacle detection
+ * during some time */
+
+/* called every 10 ms */
+void
+sensor_obstacle_update(void)
+{
+ if (disable > 0) {
+ disable --;
+ if (disable == 0)
+ DEBUG(E_USER_STRAT, "re-enable sensor");
+ }
+}
+
+void sensor_obstacle_disable(void)
+{
+ DEBUG(E_USER_STRAT, "disable sensor");
+ disable = DISABLE_CPT_MAX;
+}
+
+void sensor_obstacle_enable(void)
+{
+ disable = 0;
+}
+
+uint8_t sensor_obstacle_is_disabled(void)
+{
+ return disable;
+}
+
+
+/************ global sensor init */
+
+/* called every 10 ms, see init below */
+static void do_sensors(void *dummy)
+{
+ do_adc(NULL);
+ do_boolean_sensors(NULL);
+ sensor_obstacle_update();
+}
+
+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:41 zer0 Exp $
+ *
+ */
+
+/* synchronize with sensor.c */
+#define ADC_CSENSE1 0
+#define ADC_CSENSE2 1
+#define ADC_CSENSE3 2
+#define ADC_CSENSE4 3
+#define ADC_BATTERY1 4
+#define ADC_BATTERY2 5
+#define ADC_MAX 6
+
+/* synchronize with sensor.c */
+#define S_CAP1 0
+#define S_CAP2 1
+#define S_COLUMN_RIGHT 2
+#define S_COLUMN_LEFT 3
+#define S_START_SWITCH 4
+#define S_DISP_LEFT 5
+#define S_DISP_RIGHT 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);
+
+void sensor_obstacle_disable(void);
+void sensor_obstacle_enable(void);
+uint8_t sensor_obstacle_is_disabled(void);
--- /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 (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_base.c,v 1.6 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "strat_base.h"
+#include "sensor.h"
+
+/* true if we want to interrupt a trajectory */
+static uint8_t traj_intr=0;
+
+/* filled when a END_OBSTACLE is returned */
+struct opponent_obstacle opponent_obstacle;
+
+/* asked speed */
+static volatile int16_t strat_speed_a = SPEED_DIST_FAST;
+static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST;
+static volatile uint16_t strat_limit_speed_a = 0; /* no limit */
+static volatile uint16_t strat_limit_speed_d = 0;
+
+static volatile uint8_t strat_limit_speed_enabled = 1;
+
+
+/* Strings that match the end traj cause */
+/* /!\ keep it sync with stat_base.h */
+const char *err_tab []= {
+ "END_TRAJ",
+ "END_BLOCKING",
+ "END_NEAR",
+ "END_OBSTACLE",
+ "END_ERROR",
+ "END_INTR",
+ "END_TIMER",
+ "END_RESERVED",
+};
+
+/* return string from end traj type num */
+const char *get_err(uint8_t err)
+{
+ uint8_t i;
+ if (err == 0)
+ return "SUCCESS";
+ for (i=0 ; i<8; i++) {
+ if (err & (1 <<i))
+ return err_tab[i];
+ }
+ return "END_UNKNOWN";
+}
+
+void strat_hardstop(void)
+{
+ trajectory_hardstop(&mainboard.traj);
+ pid_reset(&mainboard.angle.pid);
+ pid_reset(&mainboard.distance.pid);
+ bd_reset(&mainboard.angle.bd);
+ bd_reset(&mainboard.distance.bd);
+
+ while ((ABS(mainboard.speed_d) > 200) ||
+ (ABS(mainboard.speed_a) > 200))
+
+ trajectory_hardstop(&mainboard.traj);
+ pid_reset(&mainboard.angle.pid);
+ pid_reset(&mainboard.distance.pid);
+ bd_reset(&mainboard.angle.bd);
+ bd_reset(&mainboard.distance.bd);
+}
+
+/* reset position */
+void strat_reset_pos(int16_t x, int16_t y, int16_t a)
+{
+ int16_t posx = position_get_x_s16(&mainboard.pos);
+ int16_t posy = position_get_y_s16(&mainboard.pos);
+ int16_t posa = position_get_a_deg_s16(&mainboard.pos);
+
+ if (x == DO_NOT_SET_POS)
+ x = posx;
+ if (y == DO_NOT_SET_POS)
+ y = posy;
+ if (a == DO_NOT_SET_POS)
+ a = posa;
+
+ DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
+ x == DO_NOT_SET_POS ? "" : "x",
+ y == DO_NOT_SET_POS ? "" : "y",
+ a == DO_NOT_SET_POS ? "" : "a");
+ position_set(&mainboard.pos, x, y, a);
+ DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
+}
+
+/*
+ * decrease gain on angle PID, and go forward until we reach the
+ * border.
+ */
+uint8_t strat_calib(int16_t dist, uint8_t flags)
+{
+ int32_t p = pid_get_gain_P(&mainboard.angle.pid);
+ int32_t i = pid_get_gain_I(&mainboard.angle.pid);
+ int32_t d = pid_get_gain_D(&mainboard.angle.pid);
+ uint8_t err;
+
+ pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
+ trajectory_d_rel(&mainboard.traj, dist);
+ err = wait_traj_end(flags);
+ pid_set_gains(&mainboard.angle.pid, p, i, d);
+ return err;
+}
+
+static void strat_update_traj_speed(void)
+{
+ uint16_t d, a;
+
+ d = strat_speed_d;
+ if (strat_limit_speed_d && d > strat_limit_speed_d)
+ d = strat_limit_speed_d;
+ a = strat_speed_a;
+ if (strat_limit_speed_a && a > strat_limit_speed_a)
+ a = strat_limit_speed_a;
+
+ trajectory_set_speed(&mainboard.traj, d, a);
+}
+
+void strat_set_speed(uint16_t d, uint16_t a)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ strat_speed_d = d;
+ strat_speed_a = a;
+ strat_update_traj_speed();
+ IRQ_UNLOCK(flags);
+}
+
+void strat_get_speed(uint16_t *d, uint16_t *a)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ *d = strat_speed_d;
+ *a = strat_speed_a;
+ IRQ_UNLOCK(flags);
+}
+
+
+void interrupt_traj(void)
+{
+ traj_intr = 1;
+}
+
+void interrupt_traj_reset(void)
+{
+ traj_intr = 0;
+}
+
+uint8_t test_traj_end(uint8_t why)
+{
+ uint16_t cur_timer;
+/* point_t robot_pt; */
+
+/* robot_pt.x = position_get_x_s16(&mainboard.pos); */
+/* robot_pt.y = position_get_y_s16(&mainboard.pos); */
+
+ /* trigger an event at 3 sec before the end of the match if we
+ * have balls in the barrel */
+ cur_timer = time_get_s();
+
+ if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) {
+ /* end of match */
+ if (cur_timer >= MATCH_TIME)
+ return END_TIMER;
+ }
+
+ if ((why & END_INTR) && traj_intr) {
+ interrupt_traj_reset();
+ return END_INTR;
+ }
+
+ if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
+ return END_TRAJ;
+
+ /* we are near the destination point (depends on current
+ * speed) AND the robot is in the area bounding box. */
+ if (why & END_NEAR) {
+ int16_t d_near = 100;
+
+ if (mainboard.speed_d > 2000)
+ d_near = 150;
+
+/* if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && */
+/* is_in_boundingbox(&robot_pt)) */
+/* return END_NEAR; */
+ }
+
+ if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
+ strat_hardstop();
+ return END_BLOCKING;
+ }
+
+ if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) {
+ strat_hardstop();
+ return END_BLOCKING;
+ }
+
+/* if ((why & END_OBSTACLE) && strat_obstacle()) { */
+/* strat_hardstop(); */
+/* return END_OBSTACLE; */
+/* } */
+
+ return 0;
+}
+
+uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
+{
+ uint8_t ret = 0;
+/* int16_t opp_x, opp_y, opp_d, opp_a; */
+
+ while (ret == 0)
+ ret = test_traj_end(why);
+
+/* if (ret == END_OBSTACLE) { */
+/* if (get_opponent_xyda(&opp_x, &opp_y, */
+/* &opp_d, &opp_a) == 0) */
+/* DEBUG(E_USER_STRAT, "Got %s at line %d" */
+/* " xy=(%d,%d) da=(%d,%d)", get_err(ret), */
+/* line, opp_x, opp_y, opp_d, opp_a); */
+/* } */
+/* else { */
+/* DEBUG(E_USER_STRAT, "Got %s at line %d", */
+/* get_err(ret), line); */
+/* } */
+ return ret;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_base.h,v 1.3 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+
+/* return values for strats and sub trajs */
+#define END_TRAJ 1 /* traj successful */
+#define END_BLOCKING 2 /* blocking during traj */
+#define END_NEAR 4 /* we are near destination */
+#define END_OBSTACLE 8 /* There is an obstacle in front of us */
+#define END_ERROR 16 /* Cannot do the command */
+#define END_INTR 32 /* interrupted by user */
+#define END_TIMER 64 /* we don't a lot of time */
+#define END_RESERVED 128 /* reserved */
+
+/* default speeds */
+#define SPEED_DIST_FAST 2500
+#define SPEED_ANGLE_FAST 2000
+#define SPEED_DIST_SLOW 1000
+#define SPEED_ANGLE_SLOW 1000
+#define SPEED_DIST_VERY_SLOW 400
+#define SPEED_ANGLE_VERY_SLOW 400
+
+/* useful traj flags */
+#define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR))
+#define TRAJ_FLAGS_STD (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR|END_TIMER)
+#define TRAJ_FLAGS_NO_TIMER (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR)
+#define TRAJ_FLAGS_NO_NEAR (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR|END_TIMER)
+#define TRAJ_FLAGS_NO_NEAR_NO_TIMER (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR)
+#define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR)
+
+/* area */
+#define AREA_X 3000
+#define AREA_Y 2100
+
+#define START_X 200
+#define START_Y 200
+#define START_A 45
+
+#define CENTER_X 1500
+#define CENTER_Y 1050
+
+#define CORNER_X 3000
+#define CORNER_Y 2100
+
+/* only valid after a END_OBSTACLE */
+struct opponent_obstacle {
+ int16_t x;
+ int16_t y;
+ int16_t a;
+ int16_t d;
+};
+extern struct opponent_obstacle opponent_obstacle;
+
+/* stop as fast as possible, without ramp */
+void strat_hardstop(void);
+
+#define DO_NOT_SET_POS -1000
+/* Reset position. If arg == DO_NOT_SET_POS, don't update value for
+ * it. */
+void strat_reset_pos(int16_t x, int16_t y, int16_t a);
+
+/* decrease gain on angle PID, and go forward until we reach the
+ * border. */
+uint8_t strat_calib(int16_t d, uint8_t flags);
+
+/* launch start procedure */
+void strat_start(void);
+
+/* go to an x,y point without checking for obstacle or blocking. It
+ * should be used for very small dist only. Return END_TRAJ if we
+ * reach destination, or END_BLOCKING if the robot blocked more than 3
+ * times. */
+uint8_t strat_goto_xy_force(int16_t x, int16_t y);
+
+/* escape from disc polygon or another zone */
+struct build_zone;
+uint8_t strat_escape(struct build_zone *zone, uint8_t flags);
+
+/* return true if we have to brake due to an obstacle */
+uint8_t strat_obstacle(void);
+
+/* set/get user strat speed */
+void strat_set_speed(uint16_t d, uint16_t a);
+void strat_get_speed(uint16_t *d, uint16_t *a);
+
+/* when user type ctrl-c we can interrupt traj */
+void interrupt_traj(void);
+void interrupt_traj_reset(void);
+
+/* limit speed when we are close of opponent */
+void strat_limit_speed_enable(void);
+void strat_limit_speed_disable(void);
+void strat_limit_speed(void);
+
+/* get name of traj error with its number */
+const char *get_err(uint8_t err);
+
+/* test trajectory end condition */
+uint8_t test_traj_end(uint8_t why);
+
+/* loop until test_traj_end() is true */
+#define wait_traj_end(why) __wait_traj_end_debug(why, __LINE__)
+uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line);
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_utils.c,v 1.5 2009-05-02 10:08:09 zer0 Exp $
+ *
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "main.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "sensor.h"
+
+/* return the distance between two points */
+int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
+{
+ int32_t x,y;
+ x = (x2-x1);
+ x = x*x;
+ y = (y2-y1);
+ y = y*y;
+ return sqrt(x+y);
+}
+
+/* return the distance to a point in the area */
+int16_t distance_from_robot(int16_t x, int16_t y)
+{
+ return distance_between(position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos), x, y);
+}
+
+/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
+int16_t simple_modulo_360(int16_t a)
+{
+ if (a < -180) {
+ a += 360;
+ }
+ else if (a > 180) {
+ a -= 360;
+ }
+ return a;
+}
+
+/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
+double simple_modulo_2pi(double a)
+{
+ if (a < -M_PI) {
+ a += M_2PI;
+ }
+ else if (a > M_PI) {
+ a -= M_2PI;
+ }
+ return a;
+}
+
+/* return the distance to a point in the area */
+int16_t angle_abs_to_rel(int16_t a_abs)
+{
+ return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
+}
+
+void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
+ double *x_abs, double *y_abs)
+{
+ double x = position_get_x_double(&mainboard.pos);
+ double y = position_get_y_double(&mainboard.pos);
+ double a = position_get_a_rad_double(&mainboard.pos);
+
+ *x_abs = x + d_rel*cos(a+a_rel_rad);
+ *y_abs = y + d_rel*sin(a+a_rel_rad);
+}
+
+double norm(double x, double y)
+{
+ return sqrt(x*x + y*y);
+}
+
+void rel_xy_to_abs_xy(double x_rel, double y_rel,
+ double *x_abs, double *y_abs)
+{
+ double d_rel, a_rel;
+ d_rel = norm(x_rel, y_rel);
+ a_rel = atan2(y_rel, x_rel);
+ rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
+}
+
+/* return an angle between -pi and pi */
+void abs_xy_to_rel_da(double x_abs, double y_abs,
+ double *d_rel, double *a_rel_rad)
+{
+ double x = position_get_x_double(&mainboard.pos);
+ double y = position_get_y_double(&mainboard.pos);
+ double a = position_get_a_rad_double(&mainboard.pos);
+
+ *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
+ if (*a_rel_rad < -M_PI) {
+ *a_rel_rad += M_2PI;
+ }
+ else if (*a_rel_rad > M_PI) {
+ *a_rel_rad -= M_2PI;
+ }
+ *d_rel = norm(x_abs-x, y_abs-y);
+}
+
+void rotate(double *x, double *y, double rot)
+{
+ double l, a;
+
+ l = norm(*x, *y);
+ a = atan2(*y, *x);
+
+ a += rot;
+ *x = l * cos(a);
+ *y = l * sin(a);
+}
+
+/* return true if the point is in area */
+uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
+{
+ if (x < margin)
+ return 0;
+ if (x > (AREA_X - margin))
+ return 0;
+ if (y < margin)
+ return 0;
+ if (y > (AREA_Y - margin))
+ return 0;
+ return 1;
+}
+
+
+/* return true if the point is in area */
+uint8_t robot_is_in_area(int16_t margin)
+{
+ return is_in_area(position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ margin);
+}
+
+/* /\* return true if we are near the disc *\/ */
+/* uint8_t robot_is_near_disc(void) */
+/* { */
+/* if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) */
+/* return 1; */
+/* return 0; */
+/* } */
+
+/* /\* return 1 or 0 depending on which side of a line (y=cste) is the */
+/* * robot. works in red or green color. *\/ */
+/* uint8_t y_is_more_than(int16_t y) */
+/* { */
+/* int16_t posy; */
+
+/* posy = position_get_y_s16(&mainboard.pos); */
+/* if (mainboard.our_color == I2C_COLOR_RED) { */
+/* if (posy > y) */
+/* return 1; */
+/* else */
+/* return 0; */
+/* } */
+/* else { */
+/* if (posy < (AREA_Y-y)) */
+/* return 1; */
+/* else */
+/* return 0; */
+/* } */
+/* } */
+
+/* return 1 or 0 depending on which side of a line (x=cste) is the
+ * robot. works in red or green color. */
+uint8_t x_is_more_than(int16_t x)
+{
+ int16_t posx;
+
+ posx = position_get_x_s16(&mainboard.pos);
+ if (posx > x)
+ return 1;
+ else
+ return 0;
+}
+
+int16_t sin_table[] = {
+ 0,
+ 3211,
+ 6392,
+ 9512,
+ 12539,
+ 15446,
+ 18204,
+ 20787,
+ 23170,
+ 25330,
+ 27245,
+ 28898,
+ 30273,
+ 31357,
+ 32138,
+ 32610,
+ 32767,
+};
+
+int16_t fast_sin(int16_t deg)
+{
+ deg %= 360;
+
+ if (deg < 0)
+ deg += 360;
+
+ if (deg < 90)
+ return sin_table[(deg*16)/90];
+ else if (deg < 180)
+ return sin_table[((180-deg)*16)/90];
+ else if (deg < 270)
+ return -sin_table[((deg-180)*16)/90];
+ else
+ return -sin_table[((360-deg)*16)/90];
+}
+
+int16_t fast_cos(int16_t deg)
+{
+ return fast_sin(90+deg);
+}
+
+
+/* get the color of our robot */
+uint8_t get_color(void)
+{
+ return mainboard.our_color;
+}
+
+/* /\* get the color of the opponent robot *\/ */
+/* uint8_t get_opponent_color(void) */
+/* { */
+/* if (mainboard.our_color == I2C_COLOR_RED) */
+/* return I2C_COLOR_GREEN; */
+/* else */
+/* return I2C_COLOR_RED; */
+/* } */
+
+/* /\* get the xy pos of the opponent robot *\/ */
+/* int8_t get_opponent_xy(int16_t *x, int16_t *y) */
+/* { */
+/* uint8_t flags; */
+/* IRQ_LOCK(flags); */
+/* *x = sensorboard.opponent_x; */
+/* *y = sensorboard.opponent_y; */
+/* IRQ_UNLOCK(flags); */
+/* if (*x == I2C_OPPONENT_NOT_THERE) */
+/* return -1; */
+/* return 0; */
+/* } */
+
+/* /\* get the da pos of the opponent robot *\/ */
+/* int8_t get_opponent_da(int16_t *d, int16_t *a) */
+/* { */
+/* uint8_t flags; */
+/* int16_t x_tmp; */
+/* IRQ_LOCK(flags); */
+/* x_tmp = sensorboard.opponent_x; */
+/* *d = sensorboard.opponent_d; */
+/* *a = sensorboard.opponent_a; */
+/* IRQ_UNLOCK(flags); */
+/* if (x_tmp == I2C_OPPONENT_NOT_THERE) */
+/* return -1; */
+/* return 0; */
+/* } */
+
+/* /\* get the da pos of the opponent robot *\/ */
+/* int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a) */
+/* { */
+/* uint8_t flags; */
+/* IRQ_LOCK(flags); */
+/* *x = sensorboard.opponent_x; */
+/* *y = sensorboard.opponent_y; */
+/* *d = sensorboard.opponent_d; */
+/* *a = sensorboard.opponent_a; */
+/* IRQ_UNLOCK(flags); */
+/* if (*x == I2C_OPPONENT_NOT_THERE) */
+/* return -1; */
+/* return 0; */
+/* } */
+
+/* uint8_t pump_left1_is_full(void) */
+/* { */
+/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && */
+/* (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); */
+/* } */
+
+/* uint8_t pump_left2_is_full(void) */
+/* { */
+/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && */
+/* (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); */
+/* } */
+
+/* uint8_t pump_right1_is_full(void) */
+/* { */
+/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && */
+/* (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); */
+/* } */
+
+/* uint8_t pump_right2_is_full(void) */
+/* { */
+/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && */
+/* (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); */
+/* } */
+
+/* /\* number of column owned by the robot *\/ */
+/* uint8_t get_column_count_left(void) */
+/* { */
+/* uint8_t ret = 0; */
+/* ret += pump_left1_is_full(); */
+/* ret += pump_left2_is_full(); */
+/* return ret; */
+/* } */
+
+/* /\* number of column owned by the robot *\/ */
+/* uint8_t get_column_count_right(void) */
+/* { */
+/* uint8_t ret = 0; */
+/* ret += pump_right1_is_full(); */
+/* ret += pump_right2_is_full(); */
+/* return ret; */
+/* } */
+
+/* /\* number of column owned by the robot *\/ */
+/* uint8_t get_column_count(void) */
+/* { */
+/* uint8_t ret = 0; */
+/* ret += pump_left1_is_full(); */
+/* ret += pump_left2_is_full(); */
+/* ret += pump_right1_is_full(); */
+/* ret += pump_right2_is_full(); */
+/* return ret; */
+/* } */
+
+/* uint8_t get_lintel_count(void) */
+/* { */
+/* return mechboard.lintel_count; */
+/* } */
+
+/* uint8_t get_mechboard_mode(void) */
+/* { */
+/* return mechboard.mode; */
+/* } */
+
+/* uint8_t get_scanner_status(void) */
+/* { */
+/* return sensorboard.scan_status; */
+/* } */
+
+/* /\* return 0 if timeout, or 1 if cond is true *\/ */
+/* uint8_t wait_scan_done(uint16_t timeout) */
+/* { */
+/* uint8_t err; */
+/* err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); */
+/* return err; */
+/* } */
+
+/* uint8_t opponent_is_behind(void) */
+/* { */
+/* int8_t opp_there; */
+/* int16_t opp_d, opp_a; */
+
+/* opp_there = get_opponent_da(&opp_d, &opp_a); */
+/* if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) */
+/* return 1; */
+/* return 0; */
+/* } */
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_utils.h,v 1.3 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+
+#define DEG(x) (((double)(x)) * (180.0 / M_PI))
+#define RAD(x) (((double)(x)) * (M_PI / 180.0))
+#define M_2PI (2*M_PI)
+
+struct xy_point {
+ int16_t x;
+ int16_t y;
+};
+
+/* wait traj end flag or cond. return 0 if cond become true, else
+ * return the traj flag */
+#define WAIT_COND_OR_TRAJ_END(cond, mask) \
+ ({ \
+ uint8_t __err = 0; \
+ while ( (! (cond)) && (__err == 0)) { \
+ __err = test_traj_end(TRAJ_FLAGS_NO_NEAR); \
+ } \
+ __err; \
+ }) \
+
+int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
+int16_t distance_from_robot(int16_t x, int16_t y);
+int16_t simple_modulo_360(int16_t a);
+double simple_modulo_2pi(double a);
+int16_t angle_abs_to_rel(int16_t a_abs);
+void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs);
+double norm(double x, double y);
+void rel_xy_to_abs_xy(double x_rel, double y_rel, double *x_abs, double *y_abs);
+void abs_xy_to_rel_da(double x_abs, double y_abs, double *d_rel, double *a_rel_rad);
+void rotate(double *x, double *y, double rot);
+uint8_t is_in_area(int16_t x, int16_t y, int16_t margin);
+uint8_t robot_is_in_area(int16_t margin);
+uint8_t robot_is_near_disc(void);
+uint8_t y_is_more_than(int16_t y);
+uint8_t x_is_more_than(int16_t x);
+int16_t fast_sin(int16_t deg);
+int16_t fast_cos(int16_t deg);
+uint8_t get_color(void);
+uint8_t get_opponent_color(void);
+int8_t get_opponent_xy(int16_t *x, int16_t *y);
+int8_t get_opponent_da(int16_t *d, int16_t *a);
+int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a);
+uint8_t get_column_count(void);
+uint8_t get_column_count_right(void);
+uint8_t get_column_count_left(void);
+uint8_t get_lintel_count(void);
+uint8_t get_mechboard_mode(void);
+uint8_t pump_left1_is_full(void);
+uint8_t pump_left2_is_full(void);
+uint8_t pump_right1_is_full(void);
+uint8_t pump_right2_is_full(void);
+uint8_t get_scanner_status(void);
+uint8_t wait_scan_done(uint16_t timeout);
+uint8_t opponent_is_behind(void);
--- /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-02-20 21:10:01 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-02-20 21:10:01 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-02 10:08:09 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
+ * UART0 definitions \r
+ */\r
+\r
+/* compile uart0 fonctions, undefine it to pass compilation */\r
+#define UART0_COMPILE \r
+\r
+/* enable uart0 if == 1, disable if == 0 */\r
+#define UART0_ENABLED 1\r
+\r
+/* enable uart0 interrupts if == 1, disable if == 0 */\r
+#define UART0_INTERRUPT_ENABLED 1\r
+\r
+#define UART0_BAUDRATE 57600\r
+\r
+/* \r
+ * if you enable this, the maximum baudrate you can reach is \r
+ * higher, but the precision is lower. \r
+ */\r
+#define UART0_USE_DOUBLE_SPEED 1\r
+\r
+#define UART0_RX_FIFO_SIZE 64\r
+#define UART0_TX_FIFO_SIZE 127\r
+#define UART0_NBITS 8\r
+\r
+#define UART0_PARITY UART_PARTITY_NONE\r
+\r
+#define UART0_STOP_BIT UART_STOP_BITS_1\r
+\r
+\r
+\r
+\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+#endif\r
+\r
git.droids-corp.org / aversive.git / commitdiff
