summaryrefslogtreecommitdiff
path: root/n
diff options
context:
space:
mode:
Diffstat (limited to 'n')
-rw-r--r--n/asserv/src/Makefile18
-rw-r--r--n/asserv/src/Makefile.avr100
-rw-r--r--n/asserv/src/avrconfig.h61
-rw-r--r--n/asserv/src/counter.c97
-rw-r--r--n/asserv/src/counter.h43
-rw-r--r--n/asserv/src/dsp.c55
-rw-r--r--n/asserv/src/main.c36
-rw-r--r--n/asserv/src/motor.c402
-rw-r--r--n/asserv/src/motor.h74
-rw-r--r--n/asserv/src/pwm.c92
-rw-r--r--n/asserv/src/pwm.h42
-rw-r--r--n/asserv/src/test_pwm.c66
-rw-r--r--n/asserv/src/timer.c55
-rw-r--r--n/asserv/src/timer.h39
14 files changed, 1180 insertions, 0 deletions
diff --git a/n/asserv/src/Makefile b/n/asserv/src/Makefile
new file mode 100644
index 0000000..474512c
--- /dev/null
+++ b/n/asserv/src/Makefile
@@ -0,0 +1,18 @@
+#PROGS = test_pwm
+#SOURCES = test_pwm.c pwm.c
+PROGS = asserv
+SOURCES = motor.c main.c
+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 =
+LIBS =
+
+include Makefile.avr
diff --git a/n/asserv/src/Makefile.avr b/n/asserv/src/Makefile.avr
new file mode 100644
index 0000000..01c9b7b
--- /dev/null
+++ b/n/asserv/src/Makefile.avr
@@ -0,0 +1,100 @@
+# Flags. {{{1
+
+CC = avr-gcc
+
+CFLAGS = -g -Wall $(OPTIMIZE) -mmcu=$(MCU_TARGET) \
+ $(if $(CONFIGFILE), $(CONFIGFILE:%=-include %))
+CPPFLAGS = $(DEFS) -Imodules
+LDFLAGS =
+
+OBJCOPY = avr-objcopy
+OBJDUMP = avr-objdump
+
+# Main rules. {{{1
+
+CSOURCES = $(filter %.c, $(SOURCES))
+SSOURCES = $(filter %.s, $(SOURCES))
+OBJECTS = $(CSOURCES:%.c=%.o) $(SSOURCES:%.s=%.o)
+ELFS = $(PROGS:%=%.elf)
+
+all: elf lst hex
+
+.PHONY: all clean elf lst doc text hex srec bin eeprom ehex esrec ebin
+
+# Rules for modules. {{{1
+
+MODULESFILES = $(MODULES:%=modules/%/Makefile.module)
+
+include $(MODULESFILES)
+
+SOURCES += $(MODULESSOURCES)
+
+$(MODULESFILES):
+ mkdir -p modules
+ cd modules && cvs co $(@:modules/%/Makefile.module=%)
+ test -f $@
+
+# General rules. {{{1
+
+elf: $(PROGS:%=%.elf)
+lst: $(PROGS:%=%.lst)
+
+$(PROGS:%=%.elf): $(OBJECTS)
+ $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS)
+
+%.lst: %.elf
+ $(OBJDUMP) -h -S $< > $@
+
+clean:
+ rm -f *.o $(OBJECTS) $(PROGS:%=%.elf) *.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/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h
new file mode 100644
index 0000000..cf92dd8
--- /dev/null
+++ b/n/asserv/src/avrconfig.h
@@ -0,0 +1,61 @@
+#ifndef avrconfig_h
+#define avrconfig_h
+/* avrconfig.h - config file for avr projects. */
+/* n.asserv - Asservissement moteur continu. {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+
+/* 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 32
+/** 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 3
+/** Protocol arguments type. */
+#define AC_PROTO_ARG_TYPE uint8_t
+
+#endif /* avrconfig_h */
diff --git a/n/asserv/src/counter.c b/n/asserv/src/counter.c
new file mode 100644
index 0000000..a9b5760
--- /dev/null
+++ b/n/asserv/src/counter.c
@@ -0,0 +1,97 @@
+/* counter.c */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "counter.h"
+#include <n/avr/utils/utils.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+/** Forward and reverse counter value. */
+static uint8_t counter_l_frw, counter_l_rev, counter_r_frw, counter_r_rev;
+/** Last TCNT value. */
+static uint8_t counter_l_old, counter_r_old;
+
+/** Initialize the counters. */
+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_reset ();
+}
+
+/* Update counter, should be called as often as possible. */
+inline void
+counter_update (void)
+{
+ uint8_t c;
+ /* Update left counter. */
+ c = TCNT2;
+ if (PINE & _BV (4))
+ {
+ PORTD &= ~0x40;
+ counter_l_frw += c - counter_l_old;
+ }
+ else
+ {
+ PORTD |= 0x40;
+ counter_l_rev += c - counter_l_old;
+ }
+ counter_l_old = c;
+ /* Update right counter. */
+ c = TCNT3L;
+ if (PINE & _BV (5))
+ {
+ PORTD &= ~0x20;
+ counter_r_frw += c - counter_r_old;
+ }
+ else
+ {
+ PORTD |= 0x20;
+ counter_r_rev += c - counter_r_old;
+ }
+ counter_r_old = c;
+}
+
+/* Reset the counters. */
+inline void
+counter_reset (void)
+{
+ counter_l_frw = 0;
+ counter_l_rev = 0;
+ counter_l_old = TCNT2;
+ counter_r_frw = 0;
+ counter_r_rev = 0;
+ counter_r_old = TCNT3L;
+}
+
diff --git a/n/asserv/src/counter.h b/n/asserv/src/counter.h
new file mode 100644
index 0000000..1f8ed88
--- /dev/null
+++ b/n/asserv/src/counter.h
@@ -0,0 +1,43 @@
+#ifndef counter_h
+#define counter_h
+/* counter.h */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+
+/* +AutoDec */
+
+/** Initialize the counters. */
+inline void
+counter_init (void);
+
+/* Update counter, should be called as often as possible. */
+inline void
+counter_update (void);
+
+/* Reset the counters. */
+inline void
+counter_reset (void);
+
+/* -AutoDec */
+
+#endif /* counter_h */
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
new file mode 100644
index 0000000..71317a5
--- /dev/null
+++ b/n/asserv/src/dsp.c
@@ -0,0 +1,55 @@
+/* dsp.c */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "dsp.h"
+
+/* +AutoDec */
+/* -AutoDec */
+
+/* Add two signed words and saturate. */
+inline int16_t
+dsp_i16i16_add_sat (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"
+ "1f" "\n\t"
+ : "=r" a : "0" a, "r" b);
+ return a;
+}
+
+/* Multiply and saturate. */
+inline int16_t
+dsp_i16u8_mul_sat (int16_t a, uint8_t b)
+{
+ asm ("mul
+}
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
new file mode 100644
index 0000000..2a25fd7
--- /dev/null
+++ b/n/asserv/src/main.c
@@ -0,0 +1,36 @@
+/* main.c */
+/* APBTasserv - asservissement Robot 2005 {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "motor.h"
+
+#include <avr/interrupt.h>
+
+int
+main (void)
+{
+ DDRD = 0x60;
+ motor_init ();
+ motor_main ();
+ sei ();
+ return 0;
+}
diff --git a/n/asserv/src/motor.c b/n/asserv/src/motor.c
new file mode 100644
index 0000000..f54d989
--- /dev/null
+++ b/n/asserv/src/motor.c
@@ -0,0 +1,402 @@
+/* motor.c */
+/* APBTasserv - asservissement Robot 2005 {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "motor.h"
+#include "pwm.h"
+#include "timer.h"
+#include "counter.h"
+#include <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+/* Variables globales. */
+uint8_t motor_asservi; /* Asservissement activé ? */
+uint8_t motor_pos_asserv; /* Si vrai, la carte d'asservissement est en
+ mode d'asservissement en position. C'est à
+ dire, l'ordinateur connecté à la carte
+ d'asservissement gère lui même la vitesse
+ afin d'asservir la position. En pratique,
+ - pas d'acquitement lors d'un !v
+ - arrét automatique si aucune commande !v
+ reçue aprés n cycles. Voir motor_ttl. */
+uint8_t motor_ttl; /* Time to live : arrète le robot si arrive à
+ 0 en mode asservissement position. */
+uint8_t motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */
+uint8_t motor_a; /* Acceleration. */
+uint8_t motor_a_cpt; /* Compteur d'acceleration. */
+int16_t motor_int_max; /* Terme intégral maximum. */
+uint8_t motor_pid_int; /* Compteur d'interruptions timer entre deux
+ calculs de PID. */
+
+/* Statistiques, etc... */
+uint8_t motor_stat_delay; /* Delais entre les stats. */
+uint8_t motor_stat_delay_cpt; /* Compteur de stats. */
+uint8_t motor_cpt_delay; /* Delais entre rapport codeurs. */
+uint8_t motor_cpt_delay_cpt; /* Compteur entre les rapports. */
+
+/* Entrées. */
+uint8_t motor_gpi_delay; /* Delais entre deux envois. */
+uint8_t motor_gpi_delay_cpt; /* Compteur. */
+
+/* Moteurs. */
+int8_t motor_g_vdes, motor_g_vacc; /* Vitesse désirée, actuelle. */
+int8_t motor_d_vdes, motor_d_vacc;
+int16_t motor_g_cpt, motor_d_cpt; /* Compteurs. */
+int16_t motor_g_e_old, motor_d_e_old; /* Dernière erreur, pour le
+ calcul de la dérivée. */
+int16_t motor_g_pwm_old, motor_d_pwm_old; /* Dernière pwm, pour les
+ stats. */
+int16_t motor_g_int, motor_d_int; /* Valeur integrale. */
+int16_t motor_g_der, motor_d_der; /* Valeur dérivée. */
+
+/* On fait les cradingues... */
+#include "pwm.c"
+#include "timer.c"
+#include "counter.c"
+
+/* Initialise le module moteur. */
+void
+motor_init (void)
+{
+ motor_asservi = 0;
+ motor_pos_asserv = 0;
+ motor_ttl = 10;
+ motor_kp = 10; motor_ki = 5; motor_kd = 0;
+ motor_a = 8;
+ motor_a_cpt = 8;
+ motor_int_max = 1000;
+ motor_pid_int = 0;
+ motor_stat_delay = 0; motor_stat_delay_cpt = 0;
+ motor_cpt_delay = 0; motor_cpt_delay_cpt = 0;
+ motor_gpi_delay = 0; motor_gpi_delay_cpt = 0;
+ motor_g_vdes = 0; motor_g_vacc = 0;
+ motor_d_vdes = 0; motor_d_vacc = 0;
+ motor_g_cpt = 0; motor_d_cpt = 0;
+ motor_g_e_old = 0; motor_d_e_old = 0;
+ motor_g_pwm_old = 0; motor_d_pwm_old = 0;
+ motor_g_int = 0; motor_d_int = 0;
+ motor_g_der = 0; motor_d_der = 0;
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ rs232_init ();
+ proto_init (motor_serial_callback, rs232_putc);
+ proto_send0 ('z');
+}
+
+/* Limite un entier entre -MAX et MAX. */
+inline int16_t
+boundary (int16_t n, int16_t max)
+{
+ if (n > max)
+ return max;
+ if (n < -max)
+ return -max;
+ return n;
+}
+
+/* Met à jour la vitesse du moteur gauche. */
+void
+motor_update_left_speed (void)
+{
+ if (motor_g_vacc != motor_g_vdes)
+ {
+ if (motor_g_vacc < motor_g_vdes)
+ {
+ /* Accélère. */
+ motor_g_vacc++;
+ }
+ else
+ {
+ /* Freine. */
+ motor_g_vacc--;
+ }
+ }
+}
+
+/* Met à jour la vitesse du moteur droit. */
+void
+motor_update_right_speed (void)
+{
+ if (motor_d_vacc != motor_d_vdes)
+ {
+ if (motor_d_vacc < motor_d_vdes)
+ {
+ /* Accélère. */
+ motor_d_vacc++;
+ }
+ else
+ {
+ /* Freine. */
+ motor_d_vacc--;
+ }
+ }
+}
+
+/* Calcule le PID associé au moteur gauche. */
+void
+motor_compute_left_pid (void)
+{
+ int16_t e;
+ int16_t diff;
+ int16_t pwm;
+ int16_t temp;
+ /* Récupère les compteurs et calcule l'erreur. */
+ diff = counter_l_frw;
+ counter_l_frw = 0;
+ diff -= counter_l_rev;
+ counter_l_rev = 0;
+ motor_g_cpt += diff;
+ e = motor_g_vacc - diff; /* 10b = 8b + 9b */
+ /* Calcul de l'integrale. */
+ motor_g_int = motor_g_int + e; /* 12b = 11b + 10b */
+ motor_g_int = boundary (motor_g_int, motor_int_max); /* 11b */
+ /* Calcul de la dérivée. */
+ //motor_g_der = safe_sub_long (e, motor_g_e_old);
+ /* Calcul du PID. P... */
+ pwm = e * motor_kp; /* 15b = 10b * 5b */
+ /* I... */
+ temp = motor_g_int * motor_ki; /* 15b = 11b * 4b */
+ pwm = pwm + temp; /* 16b = 15b + 15b */
+ /* D... */
+ //temp = safe_mul_long (motor_g_der, motor_kd);
+ //pwm = safe_add_long (pwm, temp);
+ /* Conserve l'ancienne erreur pour le terme dérivé. */
+ motor_g_e_old = e;
+ /* Conserve l'ancienne pwm. */
+ motor_g_pwm_old = pwm;
+}
+
+/* Calcule le PID associé au moteur droit. */
+void
+motor_compute_right_pid (void)
+{
+ int16_t e;
+ int16_t diff;
+ int16_t pwm;
+ int16_t temp;
+ /* Récupère les compteurs et calcule l'erreur. */
+ diff = counter_r_frw;
+ counter_r_frw = 0;
+ diff -= counter_r_rev;
+ counter_r_rev = 0;
+ motor_d_cpt += diff;
+ e = motor_d_vacc - diff;
+ /* Calcul de l'integrale. */
+ motor_d_int = motor_d_int + e;
+ motor_d_int = boundary (motor_d_int, motor_int_max);
+ /* Calcul de la dérivée. */
+ //motor_d_der = safe_sub_long (e, motor_d_e_old);
+ /* Calcul du PID. P... */
+ pwm = e * motor_kp;
+ /* I... */
+ temp = motor_d_int * motor_ki;
+ pwm = pwm + temp;
+ /* D... */
+ //temp = safe_mul_long (motor_d_der, motor_kd);
+ //pwm = safe_add_long (pwm, temp);
+ /* Conserve l'ancienne erreur pour le terme dérivé. */
+ motor_d_e_old = e;
+ /* Conserve l'ancienne pwm. */
+ motor_d_pwm_old = pwm;
+}
+
+/* Boucle principale. */
+void
+motor_main (void)
+{
+ while (1)
+ {
+ /* Ne fait le traitement que s'il y a eu une interruption. */
+ while (!timer_pending ())
+ counter_update ();
+ /* Calcul du PID. */
+ if (1)
+ {
+ motor_compute_left_pid ();
+ motor_compute_right_pid ();
+ /* Applique les nouvelles valeurs au même moment. */
+ if (motor_asservi)
+ {
+ pwm_set_left (motor_g_pwm_old);
+ pwm_set_right (motor_d_pwm_old);
+ }
+ else
+ {
+ pwm_set_left (0);
+ pwm_set_right (0);
+ }
+ /* Information de PWM. */
+ if (motor_stat_delay)
+ {
+ if (!--motor_stat_delay_cpt)
+ {
+ proto_send4 ('l', motor_g_vacc, motor_g_e_old,
+ motor_g_pwm_old >> 8, motor_g_pwm_old & 0xff);
+ proto_send4 ('r', motor_d_vacc, motor_d_e_old,
+ motor_d_pwm_old >> 8, motor_d_pwm_old & 0xff);
+ motor_stat_delay_cpt = motor_stat_delay;
+ }
+ }
+ /* Rapport des codeurs. */
+ if (motor_cpt_delay)
+ {
+ if (!--motor_cpt_delay_cpt)
+ {
+ proto_send4 ('C', motor_g_cpt >> 8, motor_g_cpt & 0xff,
+ motor_d_cpt >> 8, motor_d_cpt & 0xff);
+ motor_cpt_delay_cpt = motor_cpt_delay;
+ }
+ }
+ /* Accélère. */
+ if (motor_a)
+ {
+ if (!--motor_a_cpt)
+ {
+ motor_update_left_speed ();
+ motor_update_right_speed ();
+ motor_a_cpt = motor_a;
+ }
+ }
+ else
+ {
+ motor_g_vacc = motor_g_vdes;
+ motor_d_vacc = motor_d_vdes;
+ }
+ }
+ /* Le reste. */
+ if ((motor_pid_int & 7) == 0)
+ {
+ /* Gestion du ttl. */
+ if (motor_pos_asserv && (motor_g_vdes || motor_d_vdes) &&
+ --motor_ttl == 0)
+ {
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ //serial_send_motor_ttl ();
+ }
+ if (rs232_poll ())
+ proto_accept (rs232_getc ());
+ /* GPI. */
+ if (motor_gpi_delay)
+ {
+ if (!--motor_gpi_delay_cpt)
+ {
+ //serial_send_gpi (input_d ());
+ motor_gpi_delay_cpt = motor_gpi_delay;
+ }
+ }
+ }
+ motor_pid_int++;
+ }
+}
+
+/* Traite une entrée série. */
+void
+motor_serial_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 ('v', 2):
+ motor_g_vdes = argv[0];
+ motor_d_vdes = argv[1];
+ if (motor_pos_asserv)
+ {
+ motor_ttl = 10;
+ return;
+ }
+ break;
+ case c ('V', 1):
+ motor_pos_asserv = argv[0];
+ break;
+ case c ('s', 0):
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ break;
+ case c ('a', 1):
+ motor_a = argv[0];
+ motor_a_cpt = motor_a;
+ break;
+ case c ('p', 1):
+ motor_kp = argv[0];
+ break;
+ case c ('i', 1):
+ motor_ki = argv[0];
+ break;
+ case c ('d', 1):
+ motor_kd = argv[0];
+ break;
+ case c ('m', 1):
+ motor_stat_delay = argv[0];
+ motor_stat_delay_cpt = motor_stat_delay;
+ break;
+ case c ('c', 1):
+ motor_cpt_delay = argv[0];
+ motor_cpt_delay_cpt = motor_cpt_delay;
+ break;
+ case c ('g', 1):
+ motor_asservi = argv[0];
+ motor_toggle_asservi ();
+ break;
+ case c ('h', 1):
+ motor_gpi_delay = argv[0];
+ motor_gpi_delay_cpt = motor_gpi_delay;
+ break;
+ case c ('k', 1):
+ //temp = argv[0];
+ //output_b (temp);
+ break;
+ case c ('D', 1):
+ //temp = argv[0];
+ //if (temp == 0x42)
+ //enable_interrupts (INT_EXT);
+ break;
+ default:
+ proto_send0 ('e');
+ return;
+ }
+ proto_send (cmd, argc, argv);
+#undef c
+}
+
+/* Démarre l'asservissement. */
+void
+motor_toggle_asservi (void)
+{
+ if (motor_asservi)
+ {
+ counter_reset ();
+ motor_g_vacc = 0;
+ motor_d_vdes = 0;
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ motor_g_int = 0;
+ motor_d_int = 0;
+ }
+}
diff --git a/n/asserv/src/motor.h b/n/asserv/src/motor.h
new file mode 100644
index 0000000..5412712
--- /dev/null
+++ b/n/asserv/src/motor.h
@@ -0,0 +1,74 @@
+#ifndef motor_h
+#define motor_h
+/* motor.h */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+
+#include <n/avr/proto/proto.h>
+#include <avr/signal.h>
+#include <inttypes.h>
+
+/* +AutoDec */
+
+/* Initialise le module moteur. */
+void
+motor_init (void);
+
+/* Limite un entier entre -MAX et MAX. */
+inline int16_t
+boundary (int16_t n, int16_t max);
+
+/* Met à jour la vitesse du moteur gauche. */
+void
+motor_update_left_speed (void);
+
+/* Met à jour la vitesse du moteur droit. */
+void
+motor_update_right_speed (void);
+
+/* Calcule le PID associé au moteur gauche. */
+void
+motor_compute_left_pid (void);
+
+/* Calcule le PID associé au moteur droit. */
+void
+motor_compute_right_pid (void);
+
+/* Boucle principale. */
+void
+motor_main (void);
+
+/* Traite une entrée série. */
+void
+motor_serial_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]);
+
+short
+motor_parse (void);
+
+/* Démarre l'asservissement. */
+void
+motor_toggle_asservi (void);
+
+/* -AutoDec */
+
+#endif /* motor_h */
diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c
new file mode 100644
index 0000000..84791d4
--- /dev/null
+++ b/n/asserv/src/pwm.c
@@ -0,0 +1,92 @@
+/* pwm.c */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "pwm.h"
+#include <avr/io.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+inline void
+pwm_init (void)
+{
+ /* No timer/counter interrupt. */
+ //TIMSK = 0;
+ //ETIMSK = 0;
+ /* Phase correct PWM, TOP = 0xff, OC1B & OC1C with positive logic.
+ f_IO without prescaler.
+ Fpwm = f_IO / (2 * prescaler * TOP) = 28912 Hz. */
+ TCCR1A = _BV (COM1B1) | _BV (COM1C1) | _BV (WGM10);
+ TCCR1B = _BV (CS10);
+ /* Enable pwm and direction outputs in DDRB. */
+ DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2);
+}
+
+static inline int16_t
+pwm_preproc (int16_t v)
+{
+ v = (v >> 2) + ((v >> 1) & 1);
+ /*if (v > 0)
+ v += 0x0f;
+ else if (v < 0)
+ v -= 0x0f;*/
+ if (v > 255)
+ return 255;
+ else if (v < -255)
+ return -255;
+ else
+ return v;
+}
+
+inline void
+pwm_set_left (int16_t v)
+{
+ v = pwm_preproc (v);
+ if (v < 0)
+ {
+ PORTB &= ~_BV (2);
+ OCR1B = -v;
+ }
+ else
+ {
+ PORTB |= _BV (2);
+ OCR1B = v;
+ }
+}
+
+inline void
+pwm_set_right (int16_t v)
+{
+ v = pwm_preproc (v);
+ if (v < 0)
+ {
+ PORTB |= _BV (3);
+ OCR1C = -v;
+ }
+ else
+ {
+ PORTB &= ~_BV (3);
+ OCR1C = v;
+ }
+}
+
diff --git a/n/asserv/src/pwm.h b/n/asserv/src/pwm.h
new file mode 100644
index 0000000..7ae4902
--- /dev/null
+++ b/n/asserv/src/pwm.h
@@ -0,0 +1,42 @@
+#ifndef pwm_h
+#define pwm_h
+/* pwm.h */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+
+#include <inttypes.h>
+
+/* +AutoDec */
+
+inline void
+pwm_init (void);
+
+inline void
+pwm_set_left (int16_t v);
+
+inline void
+pwm_set_right (int16_t v);
+
+/* -AutoDec */
+
+#endif /* pwm_h */
diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c
new file mode 100644
index 0000000..ee1be2a
--- /dev/null
+++ b/n/asserv/src/test_pwm.c
@@ -0,0 +1,66 @@
+/* test_pwm.c */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "pwm.h"
+
+#include <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+#include <avr/io.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+void
+proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
+{
+ switch (c | argc << 8)
+ {
+ case 'l' | 2 << 8:
+ pwm_set_left (argv[0] << 8 | argv[1]);
+ break;
+ case 'r' | 2 << 8:
+ pwm_set_right (argv[0] << 8 | argv[1]);
+ break;
+ default:
+ proto_send0 ('e');
+ return;
+ }
+ 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/asserv/src/timer.c b/n/asserv/src/timer.c
new file mode 100644
index 0000000..b5d0b4d
--- /dev/null
+++ b/n/asserv/src/timer.c
@@ -0,0 +1,55 @@
+/* timer.c */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "timer.h"
+#include <avr/io.h>
+#include <n/avr/utils/utils.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+/** Initialise the timer. */
+inline void
+timer_init (void)
+{
+ /* 1024 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))
+ * Tov = 1 / Fov = 4.44 ms */
+}
+
+/* Test if a overflow occured and reset the flag. */
+inline int
+timer_pending (void)
+{
+ if (TIFR & _BV (TOV0))
+ {
+ /* Write 1 to clear. */
+ TIFR |= _BV (TOV0);
+ return 1;
+ }
+ else
+ return 0;
+}
+
diff --git a/n/asserv/src/timer.h b/n/asserv/src/timer.h
new file mode 100644
index 0000000..8f6ae1e
--- /dev/null
+++ b/n/asserv/src/timer.h
@@ -0,0 +1,39 @@
+#ifndef timer_h
+#define timer_h
+/* timer.h */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+
+/* +AutoDec */
+
+/** Initialise the timer. */
+inline void
+timer_init (void);
+
+/* Test if a overflow occured and reset the flag. */
+inline int
+timer_pending (void);
+
+/* -AutoDec */
+
+#endif /* timer_h */