From 853fb5b5d899eb55f1f7d4044bff0a8d5d84b694 Mon Sep 17 00:00:00 2001 From: prot Date: Thu, 9 Dec 2004 23:57:42 +0000 Subject: Importing files from n/asserv --- n/line-follower/src/Makefile | 23 +++ n/line-follower/src/Makefile.avr | 114 +++++++++++++ n/line-follower/src/avrconfig.h | 62 +++++++ n/line-follower/src/counter.c | 135 +++++++++++++++ n/line-follower/src/dsp.c | 347 +++++++++++++++++++++++++++++++++++++++ n/line-follower/src/dsp.h | 44 +++++ n/line-follower/src/dsp.txt | 22 +++ n/line-follower/src/main.c | 232 ++++++++++++++++++++++++++ n/line-follower/src/postrack.c | 105 ++++++++++++ n/line-follower/src/pwm.c | 108 ++++++++++++ n/line-follower/src/speed.c | 159 ++++++++++++++++++ n/line-follower/src/test_dsp.c | 208 +++++++++++++++++++++++ n/line-follower/src/test_pwm.c | 69 ++++++++ n/line-follower/src/timer.c | 74 +++++++++ 14 files changed, 1702 insertions(+) create mode 100644 n/line-follower/src/Makefile create mode 100644 n/line-follower/src/Makefile.avr create mode 100644 n/line-follower/src/avrconfig.h create mode 100644 n/line-follower/src/counter.c create mode 100644 n/line-follower/src/dsp.c create mode 100644 n/line-follower/src/dsp.h create mode 100644 n/line-follower/src/dsp.txt create mode 100644 n/line-follower/src/main.c create mode 100644 n/line-follower/src/postrack.c create mode 100644 n/line-follower/src/pwm.c create mode 100644 n/line-follower/src/speed.c create mode 100644 n/line-follower/src/test_dsp.c create mode 100644 n/line-follower/src/test_pwm.c create mode 100644 n/line-follower/src/timer.c (limited to 'n') diff --git a/n/line-follower/src/Makefile b/n/line-follower/src/Makefile new file mode 100644 index 0000000..dbfe3bc --- /dev/null +++ b/n/line-follower/src/Makefile @@ -0,0 +1,23 @@ +PROGS = linefol +linefol_OBJECTS = main.o rs232.o proto.o +DOC = +EXTRACTDOC = +MODULES = n/avr/rs232 n/avr/proto n/avr/utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +MCU_TARGET = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +DEFS = +LDLIBS = + +include Makefile.avr + +linefol.elf: $(linefol_OBJECTS) + +test_pwm.elf: $(test_pwm_OBJECTS) + +test_dsp.elf: $(test_dsp_OBJECTS) + diff --git a/n/line-follower/src/Makefile.avr b/n/line-follower/src/Makefile.avr new file mode 100644 index 0000000..c010642 --- /dev/null +++ b/n/line-follower/src/Makefile.avr @@ -0,0 +1,114 @@ +# Flags. {{{1 + +CFLAGS = -g -Wall $(OPTIMIZE) -mmcu=$(MCU_TARGET) +CPPFLAGS = $(DEFS) -MMD $(INCLUDES) \ + $(if $(CONFIGFILE), $(CONFIGFILE:%=-include %)) +INCLUDES = -Imodules +LDFLAGS = $(CFLAGS) + +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump + +# Main rules. {{{1 + +all: elf lst hex + +.PHONY: all clean elf lst doc text hex srec bin eeprom ehex esrec ebin + +# Rules for modules. {{{1 + +VPATH = $(MODULES:%=modules/%) + +modules_checkout: modules_co + +modules_co: $(MODULES:%=modules_co.%) + +$(MODULES:%=modules_co.%): + mkdir -p modules + cd modules && cvs co $(@:modules_co.%=%) + +modules_update: modules_up + +modules_up: $(MODULES:%=modules_up.%) + +$(MODULES:%=modules_up.%): + cd modules/$(@:modules_up.%=%) && cvs up + +# General rules. {{{1 + +ELFS = $(PROGS:%=%.elf) +OBJECTS = $(filter %.o,$(foreach prog,$(PROGS),$($(prog)_OBJECTS))) + +elf: $(ELFS) +lst: $(PROGS:%=%.lst) + +# Great piece of rules which only works in make 3.80+. +# define PROG_template +# $(1).elf: $$($(1)_OBJECTS) +# endef +# +# $(foreach prog,$(PROGS),$(eval $(call PROG_template,$(prog)))) + +$(ELFS): + $(LINK.o) $^ $(LDLIBS) -o $@ + +%.lst: %.elf + $(OBJDUMP) -h -S $< > $@ + +# Dependency checking. +-include $(OBJECTS:%.o=%.d) + +clean: + rm -f *.o *.d $(ELFS) *.lst *.map *.bak *~ + rm -f $(DOC) *.exd $(EXTRA_CLEAN_FILES) $(TEXTS) $(EEPROMS) + +# Rules for building the doc. {{{1 + +doc: $(DOC) + +%.html: %.txt %.exd + aft $< + +%.exd: $(EXTRACTDOC) + test -n "$^" && extractdoc $^ > $@ || true + +# Rules for building the .text rom images. {{{1 + +TEXTS = $(PROGS:%=%.hex) $(PROGS:%=%.bin) $(PROGS:%=%.srec) + +text: hex bin srec + +hex: $(PROGS:%=%.hex) +bin: $(PROGS:%=%.bin) +srec: $(PROGS:%=%.srec) + +%.hex: %.elf + $(OBJCOPY) -j .text -j .data -O ihex $< $@ + +%.srec: %.elf + $(OBJCOPY) -j .text -j .data -O srec $< $@ + +%.bin: %.elf + $(OBJCOPY) -j .text -j .data -O binary $< $@ + +# Rules for building the .eeprom rom images. {{{1 + +EEPROMS = $(PROGS:%=%_eeprom.hex) $(PROGS:%=%_eeprom.bin) \ + $(PROGS:%=%_eeprom.srec) + +eeprom: ehex ebin esrec + +ehex: $(PROGS:%=%_eeprom.hex) +ebin: $(PROGS:%=%_eeprom.bin) +esrec: $(PROGS:%=%_eeprom.srec) + +%_eeprom.hex: %.elf + $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O ihex $< $@ + +%_eeprom.srec: %.elf + $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O srec $< $@ + +%_eeprom.bin: %.elf + $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O binary $< $@ + diff --git a/n/line-follower/src/avrconfig.h b/n/line-follower/src/avrconfig.h new file mode 100644 index 0000000..bb19576 --- /dev/null +++ b/n/line-follower/src/avrconfig.h @@ -0,0 +1,62 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - config file for avr projects. */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 14745600 + +/* rs232 - RS232 Module. */ +/** Baudrate : 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_RS232_BAUDRATE 115200 +/** Send mode : + * - POLLING : no interrupts; + * - RING : interrupts, ring buffer. */ +#define AC_RS232_SEND_MODE POLLING +/** Recv mode, same as send mode. */ +#define AC_RS232_RECV_MODE POLLING +/** Character size : 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_RS232_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_RS232_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_RS232_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_RS232_SEND_BUFFER_SIZE 64 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_RS232_RECV_BUFFER_SIZE 32 +/** Select serial port (0 or 1). */ +#define AC_RS232_PORT 1 + +/* proto - Protocol module. */ +/** Maximum argument number. */ +#define AC_PROTO_MAX_ARGS 4 +/** Protocol arguments type. */ +#define AC_PROTO_ARG_TYPE uint8_t + +#endif /* avrconfig_h */ diff --git a/n/line-follower/src/counter.c b/n/line-follower/src/counter.c new file mode 100644 index 0000000..46dc05a --- /dev/null +++ b/n/line-follower/src/counter.c @@ -0,0 +1,135 @@ +/* counter.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Forward and reverse counter values. */ +static uint8_t counter_left_frw, counter_left_rev, + counter_right_frw, counter_right_rev; +/** Last TCNT values. */ +static uint8_t counter_left_old, counter_right_old; +/** Overall counter values. */ +static uint16_t counter_left, counter_right; +/** Counter differences since last update. + * Maximum of 9 significant bits, sign included. */ +static int16_t counter_left_diff, counter_right_diff; + +/* +AutoDec */ + +/** Initialize the counters. */ +static inline void +counter_init (void); + +/** Poll counters, should be called as often as possible. */ +static inline void +counter_poll (void); + +/** Update overall counter values and compute diffs. */ +static inline void +counter_update (void); + +/** Restart counting. */ +static inline void +counter_restart (void); + +/* -AutoDec */ + +/** Initialize the counters. */ +static inline void +counter_init (void) +{ + /* Left counter. */ + /* External, rising edge. */ + TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20, + 0, 0, 0, 0, 0, 1, 1, 1); + /* Right counter. */ + /* Normal counter. */ + //TCCR3A = 0; + /* External, rising edge. */ + TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30, + 0, 0, 0, 0, 0, 1, 1, 1); + /* Begin with safe values. */ + counter_restart (); +} + +/** Poll counters, should be called as often as possible. */ +static inline void +counter_poll (void) +{ + uint8_t c; + /* Read left counter. */ + c = TCNT2; + if (PINE & _BV (4)) + { + PORTD &= ~0x40; + counter_left_frw += c - counter_left_old; + } + else + { + PORTD |= 0x40; + counter_left_rev += c - counter_left_old; + } + counter_left_old = c; + /* Read right counter. */ + c = TCNT3L; + if (PINE & _BV (5)) + { + PORTD &= ~0x20; + counter_right_frw += c - counter_right_old; + } + else + { + PORTD |= 0x20; + counter_right_rev += c - counter_right_old; + } + counter_right_old = c; +} + +/** Update overall counter values and compute diffs. */ +static inline void +counter_update (void) +{ + counter_left_diff = counter_left_frw; + counter_left_frw = 0; + counter_left_diff -= counter_left_rev; + counter_left_rev = 0; + counter_left += counter_left_diff; + counter_right_diff = counter_right_frw; + counter_right_frw = 0; + counter_right_diff -= counter_right_rev; + counter_right_rev = 0; + counter_right += counter_right_diff; +} + +/** Restart counting. */ +static inline void +counter_restart (void) +{ + counter_left_frw = 0; + counter_left_rev = 0; + counter_left_old = TCNT2; + counter_right_frw = 0; + counter_right_rev = 0; + counter_right_old = TCNT3L; +} + diff --git a/n/line-follower/src/dsp.c b/n/line-follower/src/dsp.c new file mode 100644 index 0000000..4484b2f --- /dev/null +++ b/n/line-follower/src/dsp.c @@ -0,0 +1,347 @@ +/* dsp.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "dsp.h" + +/* +AutoDec */ +/* -AutoDec */ + +/** Numbers notation: + * [u]{i|f}x[.y] + * u: unsigned + * i: integer + * f: fixed point + * x: integral part size in bits + * y: fractionnal part size in bits + * + * Ex: i16: signed 16 bit word, uf8.8: unsigned fixed 8.8. + * + * Angles are mapped from [0, 2pi) to [0,1). */ + +/** Add two signed words (i16) and saturate. UNTESTED. */ +extern inline int16_t +dsp_add_sat_i16i16 (int16_t a, int16_t b) +{ + asm ("add %A0, %A1" "\n\t" + "adc %B0, %B1" "\n\t" + /* Branch if not V. */ + "brvc 1f" "\n\t" + /* Load Max. */ + "ldi %A0, 0xff" "\n\t" + "ldi %B0, 0x7f" "\n\t" + /* Branch if not S. */ + "brlt 1f" "\n\t" + /* Load Min. */ + "ldi %A0, 0x00" "\n\t" + "ldi %B0, 0x80" "\n\t" + "1:" "" "\n\t" + : "=r" (a) : "0" (a), "r" (b)); + return a; +} + +/** Multiply i16 by uf8.8, return i16. */ +extern inline int16_t +dsp_mul_i16f88 (int16_t a, uint16_t b) +{ + int16_t r; + asm ("mulsu %B1, %B2" "\n\t" + "mov %B0, r0" "\n\t" + "mul %A1, %A2" "\n\t" + "mov %A0, r1" "\n\t" + "mul %A1, %B2" "\n\t" + "add %A0, r0" "\n\t" + "adc %B0, r1" "\n\t" + "mulsu %B1, %A2" "\n\t" + "add %A0, r0" "\n\t" + "adc %B0, r1" "\n\t" + "clr r1" "\n\t" + : "=&r" (r) : "a" (a), "a" (b) : "r0"); + return r; +} + +/** Multiply f8.24 by f8.24, return f8.24. */ +int32_t +dsp_mul_f824 (int32_t a, int32_t b) +{ + register int32_t ar asm ("r16"), br asm ("r20"); + ar = a; + br = b; + int32_t r; + int8_t z; + asm ("" + "clr %1" "\n\t" + /* Low dword (>> 8, with 8 guards). */ + "mul %A2, %B3" "\n\t" + "movw %A0, r0" "\n\t" + "clr %C0" "\n\t" + "clr %D0" "\n\t" + "mul %A2, %A3" "\n\t" + "add %A0, r1" "\n\t" + "adc %B0, %1" "\n\t" + "adc %C0, %1" "\n\t" + "mul %B2, %A3" "\n\t" + "add %A0, r0" "\n\t" + "adc %B0, r1" "\n\t" + "adc %C0, %1" "\n\t" + "mul %A2, %C3" "\n\t" + "add %B0, r0" "\n\t" + "adc %C0, r1" "\n\t" + "adc %D0, %1" "\n\t" + "mul %B2, %B3" "\n\t" + "add %B0, r0" "\n\t" + "adc %C0, r1" "\n\t" + "adc %D0, %1" "\n\t" + "mul %C2, %A3" "\n\t" + "add %B0, r0" "\n\t" + "adc %C0, r1" "\n\t" + "adc %D0, %1" "\n\t" + /* Shift. */ + "movw %A0, %C0" "\n\t" + /* Upper word. */ + "mulsu %D3, %C2" "\n\t" + "movw %C0, r0" "\n\t" + "mulsu %D3, %A2" "\n\t" + "sbc %C0, %1" "\n\t" + "sbc %D0, %1" "\n\t" + "add %A0, r0" "\n\t" + "adc %B0, r1" "\n\t" + "adc %C0, %1" "\n\t" + "adc %D0, %1" "\n\t" + "mul %B2, %C3" "\n\t" + "add %A0, r0" "\n\t" + "adc %B0, r1" "\n\t" + "adc %C0, %1" "\n\t" + "adc %D0, %1" "\n\t" + "mul %C2, %B3" "\n\t" + "add %A0, r0" "\n\t" + "adc %B0, r1" "\n\t" + "adc %C0, %1" "\n\t" + "adc %D0, %1" "\n\t" + "mulsu %D2, %A3" "\n\t" + "sbc %C0, %1" "\n\t" + "sbc %D0, %1" "\n\t" + "add %A0, r0" "\n\t" + "adc %B0, r1" "\n\t" + "adc %C0, %1" "\n\t" + "adc %D0, %1" "\n\t" + "mulsu %D3, %B2" "\n\t" + "sbc %D0, %1" "\n\t" + "add %B0, r0" "\n\t" + "adc %C0, r1" "\n\t" + "adc %D0, %1" "\n\t" + "mul %C2, %C3" "\n\t" + "add %B0, r0" "\n\t" + "adc %C0, r1" "\n\t" + "adc %D0, %1" "\n\t" + "mulsu %D2, %B3" "\n\t" + "sbc %D0, %1" "\n\t" + "add %B0, r0" "\n\t" + "adc %C0, r1" "\n\t" + "adc %D0, %1" "\n\t" + "mulsu %D2, %C3" "\n\t" + "add %C0, r0" "\n\t" + "adc %D0, r1" "\n\t" + "muls %D2, %D3" "\n\t" + "add %D0, r0" "\n\t" + "clr r1" "\n\t" + : "=&r" (r), "=&r" (z) : "a" (ar), "a" (br) : "r0"); + return r; +} + +/** Divide f8.24 by f8.24, result f8.24. */ +int32_t +dsp_div_f824 (int32_t dd, int32_t dv) +{ + int32_t rem; + int8_t cnt; + asm ("" + /* Store sign. */ + "mov __tmp_reg__, %D1" "\n\t" + "eor __tmp_reg__, %D3" "\n\t" + /* Change sign. */ + "sbrs %D1, 7" "\n\t" + "rjmp 1f" "\n\t" + "com %A1" "\n\t" + "com %B1" "\n\t" + "com %C1" "\n\t" + "com %D1" "\n\t" + "subi %A1, 0xff" "\n\t" + "sbci %B1, 0xff" "\n\t" + "sbci %C1, 0xff" "\n\t" + "sbci %D1, 0xff" "\n\t" + "1:" "sbrs %D3, 7" "\n\t" + "rjmp 2f" "\n\t" + "com %A3" "\n\t" + "com %B3" "\n\t" + "com %C3" "\n\t" + "com %D3" "\n\t" + "subi %A3, 0xff" "\n\t" + "sbci %B3, 0xff" "\n\t" + "sbci %C3, 0xff" "\n\t" + "sbci %D3, 0xff" "\n\t" + /* Clear rem. */ + "2:" "clr %A0" "\n\t" + "clr %B0" "\n\t" + "movw %C0, %A0" "\n\t" + /* First loop, dropped bits. */ + "ldi %2, 24" "\n\t" + "1:" //"lsl %A1" "\n\t" // shift out dd + "lsl %B1" "\n\t" // do not touch A1 + "rol %C1" "\n\t" + "rol %D1" "\n\t" + "rol %A0" "\n\t" // shift in rem + "rol %B0" "\n\t" // 24 bits only + "rol %C0" "\n\t" + //"rol %D0" "\n\t" + "sub %A0, %A3" "\n\t" // rem -= dv + "sbc %B0, %B3" "\n\t" + "sbc %C0, %C3" "\n\t" + "sbc %D0, %D3" "\n\t" + "brcc 2f" "\n\t" // if negative, restore rem + "add %A0, %A3" "\n\t" + "adc %B0, %B3" "\n\t" + "adc %C0, %C3" "\n\t" + "adc %D0, %D3" "\n\t" + "2:" "dec %2" "\n\t" // test for loop + "brne 1b" "\n\t" + /* Second loop, stored bits. */ + "ldi %2, 8" "\n\t" + "3:" "rol %A1" "\n\t" // shift out dd, shift in result + "rol %A0" "\n\t" // shift in rem + "rol %B0" "\n\t" + "rol %C0" "\n\t" + "rol %D0" "\n\t" + "sub %A0, %A3" "\n\t" // rem -= dv + "sbc %B0, %B3" "\n\t" + "sbc %C0, %C3" "\n\t" + "sbc %D0, %D3" "\n\t" + "brcc 4f" "\n\t" // if negative, restore rem + "add %A0, %A3" "\n\t" + "adc %B0, %B3" "\n\t" + "adc %C0, %C3" "\n\t" + "adc %D0, %D3" "\n\t" + "clc" "\n\t" // result bit 0 + "sbrc __zero_reg__, 0" "\n\t" + "4:" "sec" "\n\t" // result bit 1 + "dec %2" "\n\t" // test for loop + "brne 3b" "\n\t" + /* Last loop, stored bits, dd padding bits. */ + "ldi %2, 24" "\n\t" + "5:" "rol %A1" "\n\t" // shift out dd, shift in result + "rol %B1" "\n\t" // 0s come from the first loop + "rol %C1" "\n\t" + "rol %D1" "\n\t" + "rol %A0" "\n\t" // shift in rem + "rol %B0" "\n\t" + "rol %C0" "\n\t" + "rol %D0" "\n\t" + "sub %A0, %A3" "\n\t" // rem -= dv + "sbc %B0, %B3" "\n\t" + "sbc %C0, %C3" "\n\t" + "sbc %D0, %D3" "\n\t" + "brcc 6f" "\n\t" // if negative, restore rem + "add %A0, %A3" "\n\t" + "adc %B0, %B3" "\n\t" + "adc %C0, %C3" "\n\t" + "adc %D0, %D3" "\n\t" + "clc" "\n\t" // result bit 0 + "sbrc __zero_reg__, 0" "\n\t" + "6:" "sec" "\n\t" // result bit 1 + "dec %2" "\n\t" // test for loop + "brne 5b" "\n\t" + /* Store last bit. */ + "rol %A1" "\n\t" // shift in result + "rol %B1" "\n\t" + "rol %C1" "\n\t" + "rol %D1" "\n\t" + /* Restore sign. */ + "sbrs __tmp_reg__, 7" "\n\t" + "rjmp 7f" "\n\t" + "com %A1" "\n\t" + "com %B1" "\n\t" + "com %C1" "\n\t" + "com %D1" "\n\t" + "subi %A1, 0xff" "\n\t" + "sbci %B1, 0xff" "\n\t" + "sbci %C1, 0xff" "\n\t" + "sbci %D1, 0xff" "\n\t" + "7:" "" "\n\t" + : "=&r" (rem), "=d" (dd), "=&d" (cnt) : "d" (dv), "1" (dd)); + return dd; +} + +/** Compute cosinus for angles between [0,pi/2]. */ +int32_t +dsp_cos_dli (int32_t a) +{ + static const int32_t f[] = { + (1L << 24) * -26.42625678337439745096, + (1L << 24) * 60.24464137187666035919, + (1L << 24) * -85.45681720669372773226, + (1L << 24) * 64.93939402266829148905, + (1L << 24) * -19.73920880217871723738, + (1L << 24) * 1 + }; + int32_t r; + int32_t a2; + uint8_t i; + a2 = dsp_mul_f824 (a, a); + r = f[0]; + for (i = 1; i < sizeof (f) / sizeof (f[0]); i++) + r = dsp_mul_f824 (r, a2) + f[i]; + return r; +} + +/** Compute cosinus, angle f8.24, result f8.24. */ +int32_t +dsp_cos (int32_t a) +{ + a &= (1L << 24) - 1; + uint8_t z = ((uint32_t) a >> 16) & 0xc0; + if (z == 0) + return dsp_cos_dli (a); + else if (z == 1 << 6) + return -dsp_cos_dli ((1L << 23) - a); + else if (z == 2 << 6) + return -dsp_cos_dli (a & 0xff7fffff); + else + return dsp_cos_dli ((1L << 24) - a); +} + +/** Compute sinus, angle f8.24, result f8.24. */ +int32_t +dsp_sin (int32_t a) +{ + a &= (1L << 24) - 1; + uint8_t z = ((uint32_t) a >> 16) & 0xc0; + if (z == 0) + return dsp_cos_dli ((1L << 22) - a); + else if (z == 1 << 6) + return dsp_cos_dli (a - (1L << 22)); + else if (z == 2 << 6) + return -dsp_cos_dli ((1L << 23) + (1L << 22) - a); + else + return -dsp_cos_dli (a - (1L << 23) - (1L << 22)); +} + diff --git a/n/line-follower/src/dsp.h b/n/line-follower/src/dsp.h new file mode 100644 index 0000000..701711a --- /dev/null +++ b/n/line-follower/src/dsp.h @@ -0,0 +1,44 @@ +#ifndef dsp_h +#define dsp_h +/* dsp.h */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#include + +/* +AutoDec */ + +/** Add two signed words and saturate. UNTESTED. */ +extern inline int16_t +dsp_add_sat_i16i16 (int16_t a, int16_t b); + +/** Multiply signed 16 by fixed unsigned 8.8. */ +extern inline int16_t +dsp_mul_i16f88 (int16_t a, uint16_t b); + +/* -AutoDec */ + +#include "dsp.c" + +#endif /* dsp_h */ diff --git a/n/line-follower/src/dsp.txt b/n/line-follower/src/dsp.txt new file mode 100644 index 0000000..a20f3db --- /dev/null +++ b/n/line-follower/src/dsp.txt @@ -0,0 +1,22 @@ +dsp_mul_f824 (int32_t a, int32_t b) + +a: D1.C1 B1 A1 +b: D2.C2 B2 A2 +r: D0.C0 B0 A0 + +D1xD2. | + |D1xC2 | + | .D1xB2 | + | . D1xA2| + |C1xD2 | + | .C1xC2 | + | . C1xB2| + | . C1xA2 + | .B1xD2 | + | . B1xC2| + | . B1xB2 + | . |B1xA2 + | . A1xD2| + | . A1xC2 + | . |A1xB2 + | . | A1xA2 diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c new file mode 100644 index 0000000..72d4b39 --- /dev/null +++ b/n/line-follower/src/main.c @@ -0,0 +1,232 @@ +/* main.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "dsp.h" +#include +#include +#include +#include +#include + +#include "pwm.c" +#include "timer.c" +#include "counter.c" +#include "speed.c" +#include "postrack.c" + +/** Motor mode : + * 0 - pwm setup; + * 1 - speed control; + * 2 - position control. */ +int8_t motor_mode; + +/** Main loop counter. */ +uint8_t motor_loop_cpt; + +/** Statistics about speed control. */ +uint8_t motor_stat_speed, motor_stat_speed_cpt; + +/** Statistics about pwm values. */ +uint8_t motor_stat_pwm, motor_stat_pwm_cpt; + +/** Report of timer. */ +uint8_t motor_stat_timer, motor_stat_timer_cpt; + +/** Report of counters. */ +uint8_t motor_stat_counter, motor_stat_counter_cpt; + +/** Report position. */ +uint8_t motor_stat_postrack, motor_stat_postrack_cpt; + +/** Record timer value at different stage of computing. Used for performance + * analisys. */ +uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3; + +/* +AutoDec */ + +/* Main loop. */ +static void +main_loop (void); + +/** Handle incoming messages. */ +static void +proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]); + +/* -AutoDec */ + +/* Entry point. */ +int +main (void) +{ + DDRD = 0x60; + pwm_init (); + timer_init (); + counter_init (); + speed_init (); + postrack_init (); + rs232_init (); + proto_init (proto_callback, rs232_putc); + proto_send0 ('z'); + sei (); + main_loop (); + return 0; +} + +/* Main loop. */ +static void +main_loop (void) +{ + while (1) + { + motor_timer_0 = timer_read (); + while (!timer_pending ()) + counter_poll (); + counter_update (); + motor_timer_3 = timer_read (); + postrack_update (); + motor_timer_2 = timer_read (); + /* Speed control. */ + if (motor_mode >= 1) + { + speed_update (); + speed_compute_left_pwm (); + speed_compute_right_pwm (); + } + motor_timer_1 = timer_read (); + /* Pwm setup. */ + pwm_update (); + /* Stats. */ + if (motor_stat_speed && !--motor_stat_speed_cpt) + { + proto_send4 ('U', + speed_left_e_old >> 8, speed_left_e_old, + speed_left_int >> 8, speed_left_int); + proto_send4 ('V', + speed_right_e_old >> 8, speed_right_e_old, + speed_right_int >> 8, speed_right_int); + motor_stat_speed_cpt = motor_stat_speed; + } + if (motor_stat_pwm && !--motor_stat_pwm_cpt) + { + proto_send4 ('W', + pwm_left >> 8, pwm_left, + pwm_right >> 8, pwm_right); + motor_stat_pwm_cpt = motor_stat_pwm; + } + if (motor_stat_timer && !--motor_stat_timer_cpt) + { + proto_send4 ('T', motor_timer_3, motor_timer_2, motor_timer_1, + motor_timer_0); + motor_stat_timer_cpt = motor_stat_timer; + } + if (motor_stat_counter && !--motor_stat_counter_cpt) + { + proto_send4 ('C', + counter_left >> 8, counter_left, + counter_right >> 8, counter_right); + motor_stat_counter_cpt = motor_stat_counter; + } + if (motor_stat_postrack && !--motor_stat_postrack_cpt) + { + proto_send4 ('X', + postrack_x >> 24, postrack_x >> 16, + postrack_x >> 8, postrack_x); + proto_send4 ('Y', + postrack_y >> 24, postrack_y >> 16, + postrack_y >> 8, postrack_y); + proto_send4 ('A', + postrack_a >> 24, postrack_a >> 16, + postrack_a >> 8, postrack_a); + motor_stat_postrack_cpt = motor_stat_postrack; + } + /* Misc. */ + if ((motor_loop_cpt & 7) == 0) + { + if (rs232_poll ()) + proto_accept (rs232_getc ()); + } + motor_loop_cpt++; + } +} + +/** Handle incoming messages. */ +static void +proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) +{ +#define c(cmd, argc) (cmd << 8 | argc) + switch (c (cmd, argc)) + { + case c ('z', 0): + reset (); + break; + case c ('w', 0): + speed_restart (); + motor_mode = 0; + pwm_left = 0; + pwm_right = 0; + break; + case c ('w', 4): + speed_restart (); + motor_mode = 0; + pwm_left = argv[0] << 8 | argv[1]; + pwm_right = argv[2] << 8 | argv[3]; + break; + case c ('s', 2): + motor_mode = 1; + speed_left_aim = argv[0]; + speed_right_aim = argv[1]; + break; + case c ('a', 1): + speed_acc_cpt = speed_acc = argv[0]; + break; + case c ('p', 2): + speed_kp = argv[0] << 8 | argv[1]; + break; + case c ('i', 2): + speed_ki = argv[0] << 8 | argv[1]; + break; + case c ('f', 2): + postrack_set_footing (argv[0] << 8 | argv[1]); + break; + case c ('m', 1): + motor_stat_speed_cpt = motor_stat_speed = argv[0]; + motor_stat_pwm_cpt = motor_stat_pwm = argv[0]; + break; + case c ('c', 1): + motor_stat_counter_cpt = motor_stat_counter = argv[0]; + break; + case c ('t', 1): + motor_stat_timer_cpt = motor_stat_timer = argv[0]; + break; + case c ('T', 1): + motor_stat_postrack_cpt = motor_stat_postrack = argv[0]; + break; + default: + proto_send0 ('e'); + return; + } + proto_send (cmd, argc, argv); +#undef c +} + diff --git a/n/line-follower/src/postrack.c b/n/line-follower/src/postrack.c new file mode 100644 index 0000000..5af427e --- /dev/null +++ b/n/line-follower/src/postrack.c @@ -0,0 +1,105 @@ +/* postrack.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Current position, f24.8. */ +int32_t postrack_x, postrack_y; +/** Current angle, f8.24. */ +int32_t postrack_a; +/** Distance between the weels, u16. */ +uint16_t postrack_footing; +/** Precomputed footing factor, f8.24. + * postrack_footing_factor = (1/2pi * 256) / postrack_footing + * Explanations: + * - Angles are between 0 and 1, corresponding to 0 and 2pi, therefore we + * must divide by 2pi to convert unit (Arc=Angle * Radius only works for + * radians). + * - dd (see postrack_update) is in f11.16 format, we multiply by 256 to have + * a angle in 8.24 format. + * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). */ +int32_t postrack_footing_factor; + +/* +AutoDec */ + +/** Initialise the position tracker. */ +static inline void +postrack_init (void); + +/** Update the current position. */ +static inline void +postrack_update (void); + +/** Change the footing value. */ +static inline void +postrack_set_footing (uint16_t footing); + +/* -AutoDec */ + +/** Initialise the position tracker. */ +static inline void +postrack_init (void) +{ + postrack_set_footing (5142); +} + +/** Update the current position. */ +static inline void +postrack_update (void) +{ + int32_t d, dd, da, r, na; + d = counter_right_diff + counter_left_diff; /* 10b */ + d <<= 16; /* 10.16b */ + d >>= 1; /* 9.16b */ + if (counter_right_diff == counter_left_diff) + { + postrack_x += dsp_mul_f824 (d, dsp_cos (postrack_a)) >> 8; + postrack_y += dsp_mul_f824 (d, dsp_sin (postrack_a)) >> 8; + } + else + { + dd = counter_right_diff - counter_left_diff; /* 11b */ + dd <<= 16; /* 11.16b */ + da = dsp_mul_f824 (dd, postrack_footing_factor); + // XXX: WARNING ! could r overflow ? + r = dsp_div_f824 (d, da); /* 16.16b */ + na = postrack_a + da; + postrack_x += + dsp_mul_f824 (r, dsp_sin (na) - dsp_sin (postrack_a)) >> 8; + postrack_y += + dsp_mul_f824 (r, dsp_cos (postrack_a) - dsp_cos (na)) >> 8; + postrack_a = na; + postrack_a &= 0x00ffffff; + } +} + +#define M_1_PI 0.31830988618379067154 /* 1/pi */ + +/** Change the footing value. */ +static inline void +postrack_set_footing (uint16_t footing) +{ + postrack_footing = footing; + postrack_footing_factor = + (int32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing; +} diff --git a/n/line-follower/src/pwm.c b/n/line-follower/src/pwm.c new file mode 100644 index 0000000..883201f --- /dev/null +++ b/n/line-follower/src/pwm.c @@ -0,0 +1,108 @@ +/* pwm.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* PWM values. */ +int16_t pwm_left, pwm_right; + +/* +AutoDec */ + +/** Initialise PWM generator. */ +static inline void +pwm_init (void); + +/** Preprocess a PWM value before sending it to hardware. */ +static inline uint8_t +pwm_preproc (uint16_t v); + +/** Update the hardware PWM values. */ +static inline void +pwm_update (void); + +/* -AutoDec */ + +/** Initialise PWM generator. */ +static inline void +pwm_init (void) +{ + /* Phase correct PWM, TOP = 0xff, OC1B & OC1C with positive logic. + f_IO without prescaler. + Fpwm = f_IO / (2 * prescaler * TOP) = 28912 Hz. */ + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 0, 0, 1, 0, 1, 0, 0, 1); + TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, + 0, 0, 0, 0, 0, 0, 0, 1); + /* Enable pwm and direction outputs in DDRB. */ + DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2); +} + +/** Preprocess a PWM value before sending it to hardware. */ +static inline uint8_t +pwm_preproc (uint16_t v) +{ + // This is a try to correct the optocoupler problem... + //v += 0x10; + if (v > 255) + return 255; + else + return v; +} + +/** Update the hardware PWM values. */ +static inline void +pwm_update (void) +{ + /* Set left PWM. */ + if (pwm_left == 0) + { + OCR1B = 0; + } + else if (pwm_left < 0) + { + PORTB &= ~_BV (2); + OCR1B = pwm_preproc (-pwm_left); + } + else + { + PORTB |= _BV (2); + OCR1B = pwm_preproc (pwm_left); + } + /* Set right PWM. */ + if (pwm_right == 0) + { + OCR1C = 0; + } + else if (pwm_right < 0) + { + PORTB |= _BV (3); + OCR1C = pwm_preproc (-pwm_right); + } + else + { + PORTB &= ~_BV (3); + OCR1C = pwm_preproc (pwm_right); + } +} + diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c new file mode 100644 index 0000000..54962b3 --- /dev/null +++ b/n/line-follower/src/speed.c @@ -0,0 +1,159 @@ +/* speed.c - PI speed control. */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Actual speed. */ +int8_t speed_left, speed_right; +/** Wanted speed. */ +int8_t speed_left_aim, speed_right_aim; +/** Acceleration value. */ +uint8_t speed_acc = 8; +/** Acceleration counter, speed gets updated when it reachs 0. */ +uint8_t speed_acc_cpt; +/** Integral term. */ +int16_t speed_left_int, speed_right_int; +/** Integral max value. */ +int16_t speed_int_max = 1024; +/** Last error value. */ +int16_t speed_left_e_old, speed_right_e_old; +/** P coeficients. 5.8 fixed point format. */ +uint16_t speed_kp = 2 * 255; +/** I coeficients. 4.8 fixed point format. */ +uint16_t speed_ki = 1 * 255; + +/* +AutoDec */ + +/** Initialise speed parameters. */ +static inline void +speed_init (void); + +/** Update speeds according to the wanted speeds and the acceleration. */ +static inline void +speed_update (void); + +/** Compute new pwm value for left motor. */ +static inline void +speed_compute_left_pwm (void); + +/** Compute new pwm value for right motor. */ +static inline void +speed_compute_right_pwm (void); + +/** Forget past event, usefull when the speed control is disabled for some + * time. */ +static inline void +speed_restart (void); + +/* -AutoDec */ + +/** Initialise speed parameters. */ +static inline void +speed_init (void) +{ + speed_acc = 8; + speed_int_max = 1024; + speed_kp = 2 * 255; + speed_ki = 1 * 255; +} + +/** Update speeds according to the wanted speeds and the acceleration. */ +static inline void +speed_update (void) +{ + if (speed_acc) + { + speed_acc_cpt--; + if (speed_acc_cpt == 0) + { + speed_acc_cpt = speed_acc; + /* Update speeds. */ + if (speed_left > speed_left_aim) + speed_left--; + else if (speed_left < speed_left_aim) + speed_left++; + if (speed_right > speed_right_aim) + speed_right--; + else if (speed_right < speed_right_aim) + speed_right++; + } + } + else + { + speed_left = speed_left_aim; + speed_right = speed_right_aim; + } +} + +/** Compute new pwm value for left motor. */ +static inline void +speed_compute_left_pwm (void) +{ + int16_t e; + int16_t pwm; + e = speed_left - counter_left_diff; /* 10b = 8b + 9b */ + /* Integral update. */ + speed_left_int += e; /* 12b = 11b + 10b */ + if (speed_left_int > speed_int_max) /* 11b */ + speed_left_int = speed_int_max; + else if (speed_left_int < -speed_int_max) + speed_left_int = -speed_int_max; + /* Compute PI. */ /* 16b = 15b + 15b */ + pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 10b * 5.8b */ + + dsp_mul_i16f88 (speed_left_int, speed_ki); /* 15b = 11b * 4.8b */ + /* Save result. */ + speed_left_e_old = e; + pwm_left = pwm; +} + +/** Compute new pwm value for right motor. */ +static inline void +speed_compute_right_pwm (void) +{ + int16_t e; + int16_t pwm; + e = speed_right - counter_right_diff; + /* Integral update. */ + speed_right_int += e; + if (speed_right_int > speed_int_max) + speed_right_int = speed_int_max; + else if (speed_right_int < -speed_int_max) + speed_right_int = -speed_int_max; + /* Compute PI. */ + pwm = dsp_mul_i16f88 (e, speed_kp) + + dsp_mul_i16f88 (speed_right_int, speed_ki); + /* Save result. */ + speed_right_e_old = e; + pwm_right = pwm; +} + +/** Forget past event, usefull when the speed control is disabled for some + * time. */ +static inline void +speed_restart (void) +{ + speed_left_int = 0; + speed_right_int = 0; + speed_left = 0; + speed_right = 0; +} diff --git a/n/line-follower/src/test_dsp.c b/n/line-follower/src/test_dsp.c new file mode 100644 index 0000000..27ddc9a --- /dev/null +++ b/n/line-follower/src/test_dsp.c @@ -0,0 +1,208 @@ +/* test_dsp.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "dsp.h" + +#include +#include +#include +#include + +/* +AutoDec */ +/* -AutoDec */ + +void +proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) +{ + int16_t a; + uint16_t b; + int16_t r; + int16_t i, j, k; + uint16_t w = 0xa66a; + int32_t al, bl, rl; + uint32_t wl[] = { 0xa66a6aa6, 0xffffffff }; + static int32_t sa, sb; + switch (c | argc << 8) + { + case 'a' | 4 << 8: + sa = argv[0]; + sa = sa << 8 | argv[1]; + sa = sa << 8 | argv[2]; + sa = sa << 8 | argv[3]; + proto_send ('a', argc, argv); + break; + case 'b' | 4 << 8: + sb = argv[0]; + sb = sb << 8 | argv[1]; + sb = sb << 8 | argv[2]; + sb = sb << 8 | argv[3]; + proto_send ('b', argc, argv); + break; + case 'm' | 0 << 8: + for (i = 16; i > 0; i--) + { + a = w >> i; + for (j = 16; j >= 0; j--) + { + b = w >> j; + proto_send4 ('m', + (a >> 8) & 0xff, a & 0xff, + (b >> 8) & 0xff, b & 0xff); + r = dsp_mul_i16f88 (a, b); + proto_send2 ('r', + (r >> 8) & 0xff, r & 0xff); + r = dsp_mul_i16f88 (-a, b); + proto_send2 ('R', + (r >> 8) & 0xff, r & 0xff); + } + } + break; + case 'M' | 1 << 8: + rl = dsp_mul_f824 (sa, sb); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + break; + case 'c' | 1 << 8: + rl = dsp_cos (sa); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_sin (sa); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + break; + case 'M' | 0 << 8: + for (k = 0; k < 2; k++) + { + for (i = 32; i > 0; i--) + { + al = wl[k] >> i; + for (j = 32; j >= 0; j--) + { + bl = wl[k] >> j; + proto_send4 ('A', + (al >> 24) & 0xff, (al >> 16) & 0xff, + (al >> 8) & 0xff, al & 0xff); + proto_send4 ('B', + (bl >> 24) & 0xff, (bl >> 16) & 0xff, + (bl >> 8) & 0xff, bl & 0xff); + rl = dsp_mul_f824 (al, bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_mul_f824 (-al, bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_mul_f824 (al, -bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_mul_f824 (-al, -bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + } + } + } + break; + case 'd' | 0 << 8: + for (k = 0; k < 2; k++) + { + for (i = 32; i > 0; i--) + { + al = wl[k] >> i; + for (j = 32; j >= 0; j--) + { + bl = wl[k] >> j; + proto_send4 ('d', + (al >> 24) & 0xff, (al >> 16) & 0xff, + (al >> 8) & 0xff, al & 0xff); + proto_send4 ('v', + (bl >> 24) & 0xff, (bl >> 16) & 0xff, + (bl >> 8) & 0xff, bl & 0xff); + rl = dsp_div_f824 (al, bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_div_f824 (-al, bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_div_f824 (al, -bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_div_f824 (-al, -bl); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + } + } + } + break; + case 'c' | 0 << 8: + for (al = 0; al < (1L << 24) + (1L << 21); al += 32 << 8) + { + proto_send4 ('c', + (al >> 24) & 0xff, (al >> 16) & 0xff, + (al >> 8) & 0xff, al & 0xff); + rl = dsp_cos (al); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + rl = dsp_sin (al); + proto_send4 ('r', + (rl >> 24) & 0xff, (rl >> 16) & 0xff, + (rl >> 8) & 0xff, rl & 0xff); + } + break; + case 'z' | 0 << 8: + reset (); + default: + proto_send0 ('e'); + return; + } +} + +int +main (void) +{ + rs232_init (); + proto_init (proto_callback, rs232_putc); + rs232_putc ('!'); + rs232_putc ('z'); + rs232_putc ('d'); + rs232_putc ('s'); + rs232_putc ('p'); + rs232_putc ('\r'); + while (1) + { + uint8_t c = rs232_getc (); + proto_accept (c); + } + return 0; +} diff --git a/n/line-follower/src/test_pwm.c b/n/line-follower/src/test_pwm.c new file mode 100644 index 0000000..37d6adf --- /dev/null +++ b/n/line-follower/src/test_pwm.c @@ -0,0 +1,69 @@ +/* test_pwm.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include +#include +#include +#include + +/* +AutoDec */ +/* -AutoDec */ + +#include "pwm.c" + +void +proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) +{ + switch (c | argc << 8) + { + case 'l' | 2 << 8: + pwm_left = argv[0] << 8 | argv[1]; + break; + case 'r' | 2 << 8: + pwm_right = argv[0] << 8 | argv[1]; + break; + default: + proto_send0 ('e'); + return; + } + pwm_update (); + proto_send (c, argc, argv); +} + +int +main (void) +{ + rs232_init (); + pwm_init (); + proto_init (proto_callback, rs232_putc); + rs232_putc ('!'); + rs232_putc ('z'); + rs232_putc ('\r'); + while (1) + { + uint8_t c = rs232_getc (); + proto_accept (c); + } + return 0; +} diff --git a/n/line-follower/src/timer.c b/n/line-follower/src/timer.c new file mode 100644 index 0000000..42b9a68 --- /dev/null +++ b/n/line-follower/src/timer.c @@ -0,0 +1,74 @@ +/* timer.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* +AutoDec */ + +/** Initialise the timer. */ +static inline void +timer_init (void); + +/** Test if a overflow occured and reset the flag. */ +static inline int +timer_pending (void); + +/** Read timer value. Used for performance analysis. */ +static inline int8_t +timer_read (void); + +/* -AutoDec */ + +/** Initialise the timer. */ +static inline void +timer_init (void) +{ + /* 256 prescaler. */ + TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, + 0, 0, 0, 0, 0, 1, 1, 0); + /* Fov = F_io / (prescaler * (TOP + 1)) + * TOP = 0xff + * Tov = 1 / Fov = 4.44 ms */ +} + +/** Test if a overflow occured and reset the flag. */ +static inline int +timer_pending (void) +{ + if (TIFR & _BV (TOV0)) + { + /* Write 1 to clear. */ + TIFR |= _BV (TOV0); + return 1; + } + else + return 0; +} + +/** Read timer value. Used for performance analysis. */ +static inline int8_t +timer_read (void) +{ + return TCNT0; +} + -- cgit v1.2.3