summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorprot2004-12-09 23:57:42 +0000
committerprot2004-12-09 23:57:42 +0000
commit853fb5b5d899eb55f1f7d4044bff0a8d5d84b694 (patch)
treebe3ed35adefb3637e15963f70d12ce6402d35ccb
parent603d1062ef6decd1a3065a12efc8f12480edb71b (diff)
Importing files from n/asserv
-rw-r--r--n/line-follower/src/Makefile23
-rw-r--r--n/line-follower/src/Makefile.avr114
-rw-r--r--n/line-follower/src/avrconfig.h62
-rw-r--r--n/line-follower/src/counter.c135
-rw-r--r--n/line-follower/src/dsp.c347
-rw-r--r--n/line-follower/src/dsp.h44
-rw-r--r--n/line-follower/src/dsp.txt22
-rw-r--r--n/line-follower/src/main.c232
-rw-r--r--n/line-follower/src/postrack.c105
-rw-r--r--n/line-follower/src/pwm.c108
-rw-r--r--n/line-follower/src/speed.c159
-rw-r--r--n/line-follower/src/test_dsp.c208
-rw-r--r--n/line-follower/src/test_pwm.c69
-rw-r--r--n/line-follower/src/timer.c74
14 files changed, 1702 insertions, 0 deletions
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 <inttypes.h>
+
+/* +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 <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+#include <n/avr/utils/utils.h>
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+#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 <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+#include <n/avr/utils/utils.h>
+#include <avr/io.h>
+
+/* +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 <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+#include <n/avr/utils/utils.h>
+#include <avr/io.h>
+
+/* +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;
+}
+