summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv
diff options
context:
space:
mode:
authorschodet2005-11-06 18:01:08 +0000
committerschodet2005-11-06 18:01:08 +0000
commit0cc8897a51d07baaf42db53f70c9c9aac5272b86 (patch)
treecfe1442247d4af71cb71e8299ec3f4f6cd48cc5c /n/asserv/src/asserv
parent170c552ebdc180e3a231231d5851909fd29f6f39 (diff)
Première version du programme asserv.
Diffstat (limited to 'n/asserv/src/asserv')
-rw-r--r--n/asserv/src/asserv/Makefile17
-rw-r--r--n/asserv/src/asserv/avrconfig.h86
-rw-r--r--n/asserv/src/asserv/counter.c173
-rw-r--r--n/asserv/src/asserv/eeprom.c78
-rw-r--r--n/asserv/src/asserv/main.c260
-rw-r--r--n/asserv/src/asserv/pos.c104
-rw-r--r--n/asserv/src/asserv/pwm.c125
-rw-r--r--n/asserv/src/asserv/timer.c57
8 files changed, 900 insertions, 0 deletions
diff --git a/n/asserv/src/asserv/Makefile b/n/asserv/src/asserv/Makefile
new file mode 100644
index 0000000..4e8f054
--- /dev/null
+++ b/n/asserv/src/asserv/Makefile
@@ -0,0 +1,17 @@
+BASE = ../../../avr
+PROGS = asserv
+asserv_SOURCES = main.c
+DOC =
+EXTRACTDOC =
+MODULES = proto uart utils
+CONFIGFILE = avrconfig.h
+# atmega8, atmega8535, atmega128...
+AVR_MCU = atmega128
+# -O2 : speed
+# -Os : size
+OPTIMIZE = -O2
+
+DEFS =
+LIBS =
+
+include $(BASE)/make/Makefile.gen
diff --git a/n/asserv/src/asserv/avrconfig.h b/n/asserv/src/asserv/avrconfig.h
new file mode 100644
index 0000000..461818a
--- /dev/null
+++ b/n/asserv/src/asserv/avrconfig.h
@@ -0,0 +1,86 @@
+#ifndef avrconfig_h
+#define avrconfig_h
+/* avrconfig.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * 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
+
+/* uart - UART module. */
+/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */
+#define AC_UART0_PORT 0
+/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800,
+ * 115200, 230400, 250000, 500000, 1000000. */
+#define AC_UART0_BAUDRATE 115200
+/** Send mode:
+ * - POLLING: no interrupts.
+ * - RING: interrupts, ring buffer. */
+#define AC_UART0_SEND_MODE RING
+/** Recv mode, same as send mode. */
+#define AC_UART0_RECV_MODE RING
+/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */
+#define AC_UART0_CHAR_SIZE 8
+/** Parity : ODD, EVEN, NONE. */
+#define AC_UART0_PARITY EVEN
+/** Stop bits : 1, 2. */
+#define AC_UART0_STOP_BITS 1
+/** Send buffer size, should be power of 2 for RING mode. */
+#define AC_UART0_SEND_BUFFER_SIZE 32
+/** Recv buffer size, should be power of 2 for RING mode. */
+#define AC_UART0_RECV_BUFFER_SIZE 32
+/** If the send buffer is full when putc:
+ * - DROP: drop the new byte.
+ * - WAIT: wait until there is room in the send buffer. */
+#define AC_UART0_SEND_BUFFER_FULL DROP
+/** In HOST compilation:
+ * - STDIO: use stdin/out.
+ * - PTS: use pseudo terminal. */
+#define AC_UART0_HOST_DRIVER PTS
+/** Same thing for secondary port. */
+#define AC_UART1_PORT -1
+#define AC_UART1_BAUDRATE 115200
+#define AC_UART1_SEND_MODE RING
+#define AC_UART1_RECV_MODE RING
+#define AC_UART1_CHAR_SIZE 8
+#define AC_UART1_PARITY EVEN
+#define AC_UART1_STOP_BITS 1
+#define AC_UART1_SEND_BUFFER_SIZE 32
+#define AC_UART1_RECV_BUFFER_SIZE 32
+#define AC_UART1_SEND_BUFFER_FULL WAIT
+#define AC_UART1_HOST_DRIVER PTS
+
+/* proto - Protocol module. */
+/** Maximum argument size. */
+#define AC_PROTO_ARGS_MAX_SIZE 8
+/** Callback function name. */
+#define AC_PROTO_CALLBACK proto_callback
+/** Putchar function name. */
+#define AC_PROTO_PUTC uart0_putc
+/** Support for quote parameter. */
+#define AC_PROTO_QUOTE 1
+
+#endif /* avrconfig_h */
diff --git a/n/asserv/src/asserv/counter.c b/n/asserv/src/asserv/counter.c
new file mode 100644
index 0000000..6ac2cea
--- /dev/null
+++ b/n/asserv/src/asserv/counter.c
@@ -0,0 +1,173 @@
+/* counter.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * 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.
+ *
+ * }}} */
+
+/** Define to 1 to reverse the left counter. */
+#define COUNTER_REVERSE_LEFT 1
+/** Define to 1 to reverse the right counter. */
+#define COUNTER_REVERSE_RIGHT 0
+
+/** 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);
+
+/** 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 ();
+ /* Interrupts for direction. */
+ EICRB = 0x05;
+ EIFR = _BV (4) | _BV (5);
+ EIMSK = _BV (4) | _BV (5);
+}
+
+/** Left direction change. */
+SIGNAL (SIG_INTERRUPT4)
+{
+ uint8_t c;
+ c = TCNT2;
+ if (PINE & _BV (4))
+ {
+ counter_left_rev += c - counter_left_old;
+ PORTD |= 0x40;
+ }
+ else
+ {
+ counter_left_frw += c - counter_left_old;
+ PORTD &= ~0x40;
+ }
+ counter_left_old = c;
+}
+
+/** Right direction change. */
+SIGNAL (SIG_INTERRUPT5)
+{
+ uint8_t c;
+ c = TCNT3L;
+ if (PINE & _BV (5))
+ {
+ counter_right_rev += c - counter_right_old;
+ PORTD |= 0x20;
+ }
+ else
+ {
+ counter_right_frw += c - counter_right_old;
+ PORTD &= ~0x20;
+ }
+ counter_right_old = c;
+}
+
+/** Update overall counter values and compute diffs. */
+static inline void
+counter_update (void)
+{
+ uint8_t c;
+ /* Disable ints. */
+ EIMSK &= ~(_BV (4) | _BV (5));
+ /* Read left counter. */
+ c = TCNT2;
+ if (PINE & _BV (4))
+ counter_left_frw += c - counter_left_old;
+ else
+ counter_left_rev += c - counter_left_old;
+ counter_left_old = c;
+ /* Read right counter. */
+ c = TCNT3L;
+ if (PINE & _BV (5))
+ counter_right_frw += c - counter_right_old;
+ else
+ counter_right_rev += c - counter_right_old;
+ counter_right_old = c;
+ /* Update counter values and diffs. */
+#if COUNTER_REVERSE_LEFT == 0
+ counter_left_diff = counter_left_frw;
+ counter_left_diff -= counter_left_rev;
+#else
+ counter_left_diff = counter_left_rev;
+ counter_left_diff -= counter_left_frw;
+#endif
+ counter_left_frw = 0;
+ counter_left_rev = 0;
+ counter_left += counter_left_diff;
+#if COUNTER_REVERSE_RIGHT == 0
+ counter_right_diff = counter_right_frw;
+ counter_right_diff -= counter_right_rev;
+#else
+ counter_right_diff = counter_right_rev;
+ counter_right_diff -= counter_right_frw;
+#endif
+ counter_right_frw = 0;
+ counter_right_rev = 0;
+ counter_right += counter_right_diff;
+ /* Enable ints. */
+ EIMSK |= _BV (4) | _BV (5);
+}
+
+/** 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/asserv/src/asserv/eeprom.c b/n/asserv/src/asserv/eeprom.c
new file mode 100644
index 0000000..44f6ec2
--- /dev/null
+++ b/n/asserv/src/asserv/eeprom.c
@@ -0,0 +1,78 @@
+/* eeprom.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * 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 <avr/eeprom.h>
+
+#define EEPROM_KEY 0xa9
+
+/* +AutoDec */
+/* -AutoDec */
+
+/* Read parameters from eeprom. */
+static void
+eeprom_read_params (void)
+{
+ uint8_t *p8 = 0;
+ uint16_t *p16;
+ if (eeprom_read_byte (p8++) != EEPROM_KEY)
+ return;
+ pwm_dir = eeprom_read_byte (p8++);
+ p16 = (uint16_t *) p8;
+ pos_theta_kp = eeprom_read_word (p16++);
+ pos_alpha_kp = eeprom_read_word (p16++);
+ pos_theta_ki = eeprom_read_word (p16++);
+ pos_alpha_ki = eeprom_read_word (p16++);
+ pos_theta_kd = eeprom_read_word (p16++);
+ pos_alpha_kd = eeprom_read_word (p16++);
+ pos_e_sat = eeprom_read_word (p16++);
+ pos_int_sat = eeprom_read_word (p16++);
+}
+
+/* Write parameters to eeprom. */
+static void
+eeprom_write_params (void)
+{
+ uint8_t *p8 = 0;
+ uint16_t *p16;
+ eeprom_write_byte (p8++, EEPROM_KEY);
+ eeprom_write_byte (p8++, pwm_dir);
+ p16 = (uint16_t *) p8;
+ eeprom_write_word (p16++, pos_theta_kp);
+ eeprom_write_word (p16++, pos_alpha_kp);
+ eeprom_write_word (p16++, pos_theta_ki);
+ eeprom_write_word (p16++, pos_alpha_ki);
+ eeprom_write_word (p16++, pos_theta_kd);
+ eeprom_write_word (p16++, pos_alpha_kd);
+ eeprom_write_word (p16++, pos_e_sat);
+ eeprom_write_word (p16++, pos_int_sat);
+}
+
+/* Clear eeprom parameters. */
+static void
+eeprom_clear_params (void)
+{
+ uint8_t *p = 0;
+ eeprom_write_byte (p, 0xff);
+}
+
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
new file mode 100644
index 0000000..2f9c77b
--- /dev/null
+++ b/n/asserv/src/asserv/main.c
@@ -0,0 +1,260 @@
+/* main.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * 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 "common.h"
+#include "modules/uart/uart.h"
+#include "modules/proto/proto.h"
+#include "modules/utils/utils.h"
+#include "modules/utils/byte.h"
+#include "io.h"
+
+/* This is implementation include. */
+#include "timer.c"
+#include "counter.c"
+#include "pwm.c"
+#include "pos.c"
+#include "eeprom.c"
+
+/** Motor control mode:
+ * 0: pwm setup.
+ * 1: shaft position control.
+ * 2: speed control.
+ * 3: trajectory control. */
+int8_t main_mode;
+
+/** Report of counters. */
+uint8_t main_stat_counter, main_stat_counter_cpt;
+
+/** Statistics about shaft position control. */
+uint8_t main_stat_pos, main_stat_pos_cpt;
+
+/** Statistics about pwm values. */
+uint8_t main_stat_pwm, main_stat_pwm_cpt;
+
+/** Report of timer. */
+uint8_t main_stat_timer, main_stat_timer_cpt;
+
+/** Print input port. */
+uint8_t main_print_pin, main_print_pin_cpt;
+
+/** Record timer value at different stage of computing. Used for performance
+ * analisys. */
+uint8_t main_timer_0, main_timer_1, main_timer_2, main_timer_3, main_timer_4;
+
+/* +AutoDec */
+
+/** Main loop. */
+static void
+main_loop (void);
+
+/* -AutoDec */
+
+/** Entry point. */
+int
+main (void)
+{
+ DDRD = 0x60;
+ /* Pull-ups. */
+ PORTA = _BV (7) | _BV (1) | _BV (0);
+ eeprom_read_params ();
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ uart0_init ();
+ proto_send0 ('z');
+ sei ();
+ while (1)
+ main_loop ();
+ return 0;
+}
+
+/** Main loop. */
+static void
+main_loop (void)
+{
+ timer_wait ();
+ /* Counter update. */
+ counter_update ();
+ /* Postion control. */
+ if (main_mode >= 1)
+ pos_update ();
+ /* Pwm setup. */
+ pwm_update ();
+ /* Stats. */
+ if (main_stat_counter && !--main_stat_counter_cpt)
+ {
+ proto_send2w ('C', counter_left, counter_right);
+ main_stat_counter_cpt = main_stat_counter;
+ }
+ if (main_stat_pos && !--main_stat_pos_cpt)
+ {
+ proto_send4w ('P', pos_theta_e_old, pos_theta_int,
+ pos_alpha_e_old, pos_alpha_int);
+ main_stat_pos_cpt = main_stat_pos;
+ }
+ if (main_stat_pwm && !--main_stat_pwm_cpt)
+ {
+ proto_send2w ('W', pwm_left, pwm_right);
+ main_stat_pwm_cpt = main_stat_pwm;
+ }
+ if (main_stat_timer && !--main_stat_timer_cpt)
+ {
+ proto_send5b ('T', main_timer_4, main_timer_3, main_timer_2,
+ main_timer_1, main_timer_0);
+ main_stat_timer_cpt = main_stat_timer;
+ }
+ if (main_print_pin && !--main_print_pin_cpt)
+ {
+ proto_send1b ('I', PINA);
+ main_print_pin_cpt = main_print_pin;
+ }
+ /* Misc. */
+ while (uart0_poll ())
+ proto_accept (uart0_getc ());
+}
+
+/** Handle incoming messages. */
+void
+proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
+{
+#define c(cmd, size) (cmd << 8 | size)
+ switch (c (cmd, size))
+ {
+ case c ('z', 0):
+ utils_reset ();
+ break;
+ /* Commands. */
+ case c ('w', 0):
+ /* Set zero pwm. */
+ pos_reset ();
+ main_mode = 0;
+ pwm_left = 0;
+ pwm_right = 0;
+ break;
+ case c ('w', 4):
+ /* Set pwm.
+ * - w: left pwm.
+ * - w: right pwm. */
+ pos_reset ();
+ main_mode = 0;
+ pwm_left = v8_to_v16 (args[0], args[1]);
+ pwm_right = v8_to_v16 (args[2], args[3]);
+ break;
+ /* Stats.
+ * - b: interval between stats. */
+ case c ('C', 1):
+ /* Counter stats. */
+ main_stat_counter_cpt = main_stat_counter = args[0];
+ break;
+ case c ('P', 1):
+ /* Motor position control stats. */
+ main_stat_pos_cpt = main_stat_pos = args[0];
+ break;
+ case c ('W', 1):
+ /* Pwm stats. */
+ main_stat_pwm_cpt = main_stat_pwm = args[0];
+ break;
+ case c ('T', 1):
+ /* Timing stats. */
+ main_stat_timer_cpt = main_stat_timer = args[0];
+ break;
+ case c ('I', 1):
+ /* Input port stats. */
+ main_print_pin_cpt = main_print_pin = args[0];
+ break;
+ default:
+ /* Params. */
+ if (cmd == 'p')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('p', 3):
+ pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('p', 5):
+ pos_theta_kp = v8_to_v16 (args[1], args[2]);
+ pos_alpha_kp = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('i', 3):
+ pos_theta_ki = pos_alpha_ki = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('i', 5):
+ pos_theta_ki = v8_to_v16 (args[1], args[2]);
+ pos_alpha_ki = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('d', 3):
+ pos_theta_kd = pos_alpha_kd = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('d', 5):
+ pos_theta_kd = v8_to_v16 (args[1], args[2]);
+ pos_alpha_kd = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('E', 3):
+ pos_e_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('I', 3):
+ pos_int_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('w', 3):
+ /* Set PWM direction.
+ * - b: inverse left direction.
+ * - b: inverse right direction. */
+ pwm_dir = 0;
+ if (args[1]) pwm_dir |= _BV (PWM_LEFT_DIR);
+ if (args[2]) pwm_dir |= _BV (PWM_RIGHT_DIR);
+ break;
+ case c ('E', 2):
+ /* Write to eeprom.
+ * - b: 00: clear config, 01: write config. */
+ if (args[1])
+ eeprom_write_params ();
+ else
+ eeprom_clear_params ();
+ break;
+ case c ('P', 1):
+ /* Print current settings. */
+ proto_send1b ('E', EEPROM_KEY);
+ proto_send2w ('p', pos_theta_kp, pos_alpha_kp);
+ proto_send2w ('i', pos_theta_ki, pos_alpha_ki);
+ proto_send2w ('d', pos_theta_kd, pos_alpha_kd);
+ proto_send1w ('E', pos_e_sat);
+ proto_send1w ('I', pos_int_sat);
+ proto_send1b ('w', pwm_dir);
+ break;
+ default:
+ proto_send0 ('?');
+ return;
+ }
+ }
+ else
+ {
+ proto_send0 ('?');
+ return;
+ }
+ break;
+ }
+ proto_send (cmd, size, args);
+#undef c
+}
+
diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c
new file mode 100644
index 0000000..0a1af23
--- /dev/null
+++ b/n/asserv/src/asserv/pos.c
@@ -0,0 +1,104 @@
+/* pos.c - Position motor control. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * 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.
+ *
+ * }}} */
+
+/**
+ * This file is responsible for position motor control. The consign is a
+ * position of the motor shafts, as theta/alpha. Theta is the sum of right
+ * and left position, alpha is the difference between the right and the left
+ * position.
+ * 16 bits are enough as long as there is no long blocking (see 2005 cup!).
+ */
+
+/** Current theta/alpha. */
+uint16_t pos_theta_cur, pos_alpha_cur;
+/** Consign theta/alpha. */
+uint16_t pos_theta_cons, pos_alpha_cons;
+
+/** Error saturation. */
+int16_t pos_e_sat = 1023;
+/** Integral saturation. */
+int16_t pos_int_sat = 8191;
+/** P coefficients. */
+uint16_t pos_theta_kp = 0, pos_alpha_kp = 0;
+/** I coefficients. */
+uint16_t pos_theta_ki = 0, pos_alpha_ki = 0;
+/** D coefficients. */
+uint16_t pos_theta_kd = 0, pos_alpha_kd = 0;
+/** Current integral values. */
+int16_t pos_theta_int, pos_alpha_int;
+/** Last error values. */
+int16_t pos_theta_e_old, pos_alpha_e_old;
+
+/* +AutoDec */
+/* -AutoDec */
+
+/** Compute a PID. */
+static inline int16_t
+pos_compute_pid (int16_t e, int16_t *i, int16_t *e_old,
+ uint16_t kp, uint16_t ki, uint16_t kd)
+{
+ int16_t diff, pid;
+ /* Saturate error. */
+ UTILS_BOUND (e, -pos_e_sat, pos_e_sat);
+ /* Integral update. */
+ *i += e;
+ UTILS_BOUND (*i, -pos_int_sat, pos_int_sat);
+ /* Differential value. */
+ diff = e - *e_old;
+ /* Compute PID. */
+ pid = e * kp + *i * ki + diff * kd;
+ /* Save result. */
+ *e_old = e;
+ return pid;
+}
+
+/** Update PWM according to consign. */
+static void
+pos_update (void)
+{
+ int16_t pid_theta, pid_alpha;
+ /* Update current shaft positions. */
+ pos_theta_cur += counter_left_diff + counter_right_diff;
+ pos_alpha_cur += counter_right_diff - counter_left_diff;
+ /* Compute PID. */
+ pid_theta = pos_compute_pid (pos_theta_cons - pos_theta_cur,
+ &pos_theta_int, &pos_theta_e_old,
+ pos_theta_kp, pos_theta_ki, pos_theta_kd);
+ pid_alpha = pos_compute_pid (pos_alpha_cons - pos_alpha_cur,
+ &pos_alpha_int, &pos_alpha_e_old,
+ pos_alpha_kp, pos_alpha_ki, pos_alpha_kd);
+ /* Update PWM. */
+ pwm_left = pid_theta - pid_alpha;
+ UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
+ pwm_right = pid_theta + pid_alpha;
+ UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX);
+}
+
+/** Reset position control internal state. */
+static void
+pos_reset (void)
+{
+ pos_theta_int = pos_alpha_int = 0;
+}
diff --git a/n/asserv/src/asserv/pwm.c b/n/asserv/src/asserv/pwm.c
new file mode 100644
index 0000000..4b5abc5
--- /dev/null
+++ b/n/asserv/src/asserv/pwm.c
@@ -0,0 +1,125 @@
+/* pwm.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * 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.
+ *
+ * }}} */
+
+/** Define the PWM output used for left motor. */
+#define PWM_LEFT_OCR OCR1C
+/** Define the PWM output used for right motor. */
+#define PWM_RIGHT_OCR OCR1B
+/** Define the direction output for left motor. */
+#define PWM_LEFT_DIR 3
+/** Define the direction output for right motor. */
+#define PWM_RIGHT_DIR 2
+
+/** Define the absolute maximum PWM value. */
+#define PWM_MAX 0x1ff
+
+/** PWM values, this is an error if absolute value is greater than the
+ * maximum. */
+int16_t pwm_left, pwm_right;
+/** PWM reverse direction, only set pwm dir bits or you will get weird results
+ * on port B. */
+uint8_t pwm_dir = _BV (PWM_LEFT_DIR);
+
+/* +AutoDec */
+
+/** Initialise PWM generator. */
+static inline void
+pwm_init (void);
+
+/** Update the hardware PWM values. */
+static inline void
+pwm_update (void);
+
+/* -AutoDec */
+
+/** Initialise PWM generator. */
+static inline void
+pwm_init (void)
+{
+ /* Fast PWM, TOP = 0x1ff, OC1B & OC1C with positive logic.
+ f_IO without prescaler.
+ Fpwm = f_IO / (prescaler * (1 + TOP)) = 28912 Hz. */
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 1, 0, 1, 0, 1, 0);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 1, 0, 0, 1);
+ /* Enable pwm and direction outputs in DDRB. */
+ DDRB |= _BV (7) | _BV (6) | _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR);
+}
+
+/** Update the hardware PWM values. */
+static inline void
+pwm_update (void)
+{
+ uint16_t left, right;
+ uint8_t dir;
+ /* Some assumption checks. */
+ assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX);
+ assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX);
+ assert ((pwm_dir & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR))) == 0);
+ /* Sample port B. */
+ dir = PORTB & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR));
+ /* Set left PWM. */
+ if (pwm_left == 0)
+ {
+ left = 0;
+ }
+ else if (pwm_left < 0)
+ {
+ left = -pwm_left;
+ }
+ else
+ {
+ dir |= _BV (PWM_LEFT_DIR);
+ left = pwm_left;
+ }
+ /* Set right PWM. */
+ if (pwm_right == 0)
+ {
+ right = 0;
+ }
+ else if (pwm_right < 0)
+ {
+ right = -pwm_right;
+ }
+ else
+ {
+ dir |= _BV (PWM_RIGHT_DIR);
+ right = pwm_right;
+ }
+ /* Setup registers. */
+ /* Here, there could be a problem because OCRx are double buffered, not
+ * PORTB! */
+ /* Another problem arise if the OCR sampling is done between left and
+ * right OCR: the right PWM is one cycle late. */
+ /* A solution could be to use interrupts to update PWM or to synchronise
+ * general timer with PWM. */
+ dir ^= pwm_dir;
+ PORTB = dir;
+ PWM_LEFT_OCR = left;
+ PWM_RIGHT_OCR = right;
+}
+
diff --git a/n/asserv/src/asserv/timer.c b/n/asserv/src/asserv/timer.c
new file mode 100644
index 0000000..c065317
--- /dev/null
+++ b/n/asserv/src/asserv/timer.c
@@ -0,0 +1,57 @@
+/* timer.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * 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 */
+/* -AutoDec */
+
+/** Initialise the timer. */
+static inline void
+timer_init (void)
+{
+ /* 64 prescaler. */
+ TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00,
+ 0, 0, 0, 0, 0, 1, 0, 0);
+ /* Fov = F_io / (prescaler * (TOP + 1))
+ * TOP = 0xff
+ * Tov = 1 / Fov = 1.111 ms */
+}
+
+/** Wait for timer overflow. */
+static inline void
+timer_wait (void)
+{
+ while (!(TIFR & _BV (TOV0)))
+ ;
+ /* Write 1 to clear. */
+ TIFR = _BV (TOV0);
+}
+
+/** Read timer value. Used for performance analysis. */
+static inline uint8_t
+timer_read (void)
+{
+ return TCNT0;
+}
+