summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2008-01-28 21:12:12 +0100
committerNicolas Schodet2008-01-28 21:12:12 +0100
commitbc38b408c53281ee985f6f9d67accc5733d99e37 (patch)
treeeb4626232c03e3c25f5b508e3b8f57a539971fb4 /digital
parent837b9fe53240e8cbbd76bd9ba9b6a9a3cd9bc891 (diff)
* digital/asserv/src/asserv:
- imported old sources. - removed holes support (rule 2006).
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/Makefile16
-rw-r--r--digital/asserv/src/asserv/avrconfig.h86
-rw-r--r--digital/asserv/src/asserv/counter.avr.c173
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c96
-rw-r--r--digital/asserv/src/asserv/main.c471
-rw-r--r--digital/asserv/src/asserv/misc.h38
-rw-r--r--digital/asserv/src/asserv/models.host.c154
-rw-r--r--digital/asserv/src/asserv/models.host.h38
-rw-r--r--digital/asserv/src/asserv/motor_model.host.c88
-rw-r--r--digital/asserv/src/asserv/motor_model.host.h58
-rw-r--r--digital/asserv/src/asserv/pos.c144
-rw-r--r--digital/asserv/src/asserv/postrack.c116
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c125
-rw-r--r--digital/asserv/src/asserv/simu.host.c185
-rw-r--r--digital/asserv/src/asserv/simu.host.h94
-rw-r--r--digital/asserv/src/asserv/speed.c123
-rw-r--r--digital/asserv/src/asserv/test_motor_model.c70
-rw-r--r--digital/asserv/src/asserv/timer.avr.c70
-rw-r--r--digital/asserv/src/asserv/traj.c76
19 files changed, 2221 insertions, 0 deletions
diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile
new file mode 100644
index 00000000..023a8fd1
--- /dev/null
+++ b/digital/asserv/src/asserv/Makefile
@@ -0,0 +1,16 @@
+BASE = ../../../avr
+PROGS = asserv
+HOST_PROGS = test_motor_model
+asserv_SOURCES = main.c simu.host.c motor_model.host.c models.host.c
+test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c
+MODULES = proto uart utils math/fixed
+test_motor_model_MODULES =
+CONFIGFILE = avrconfig.h
+# atmega8, atmega8535, atmega128...
+AVR_MCU = atmega128
+# -O2 : speed
+# -Os : size
+OPTIMIZE = -O2
+HOST_LIBS = -lm
+
+include $(BASE)/make/Makefile.gen
diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h
new file mode 100644
index 00000000..5d316a25
--- /dev/null
+++ b/digital/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 1
+/** 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 WAIT
+/** 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 12
+/** 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/digital/asserv/src/asserv/counter.avr.c b/digital/asserv/src/asserv/counter.avr.c
new file mode 100644
index 00000000..c2b58781
--- /dev/null
+++ b/digital/asserv/src/asserv/counter.avr.c
@@ -0,0 +1,173 @@
+/* counter.avr.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 0
+/** 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;
+ GREEN_LED (!COUNTER_REVERSE_LEFT);
+ }
+ else
+ {
+ counter_left_frw += c - counter_left_old;
+ GREEN_LED (COUNTER_REVERSE_LEFT);
+ }
+ 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;
+ RED_LED (!COUNTER_REVERSE_RIGHT);
+ }
+ else
+ {
+ counter_right_frw += c - counter_right_old;
+ RED_LED (COUNTER_REVERSE_RIGHT);
+ }
+ 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/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
new file mode 100644
index 00000000..26e5c4b6
--- /dev/null
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -0,0 +1,96 @@
+/* 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>
+
+/* Change the eeprom key each time you change eeprom format. */
+#define EEPROM_KEY 0x45
+#define EEPROM_START 256
+
+/* +AutoDec */
+/* -AutoDec */
+
+/* Read parameters from eeprom. */
+static void
+eeprom_read_params (void)
+{
+ uint8_t *p8 = (uint8_t *) EEPROM_START;
+ uint16_t *p16;
+ if (eeprom_read_byte (p8++) != EEPROM_KEY)
+ return;
+ speed_theta_max = eeprom_read_byte (p8++);
+ speed_alpha_max = eeprom_read_byte (p8++);
+ speed_theta_slow = eeprom_read_byte (p8++);
+ speed_alpha_slow = eeprom_read_byte (p8++);
+ pwm_dir = eeprom_read_byte (p8++);
+ p16 = (uint16_t *) p8;
+ postrack_set_footing (eeprom_read_word (p16++));
+ speed_theta_acc = eeprom_read_word (p16++);
+ speed_alpha_acc = eeprom_read_word (p16++);
+ 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++);
+ pos_blocked = eeprom_read_word (p16++);
+}
+
+/* Write parameters to eeprom. */
+static void
+eeprom_write_params (void)
+{
+ uint8_t *p8 = (uint8_t *) EEPROM_START;
+ uint16_t *p16;
+ eeprom_write_byte (p8++, EEPROM_KEY);
+ eeprom_write_byte (p8++, speed_theta_max);
+ eeprom_write_byte (p8++, speed_alpha_max);
+ eeprom_write_byte (p8++, speed_theta_slow);
+ eeprom_write_byte (p8++, speed_alpha_slow);
+ eeprom_write_byte (p8++, pwm_dir);
+ p16 = (uint16_t *) p8;
+ eeprom_write_word (p16++, postrack_footing);
+ eeprom_write_word (p16++, speed_theta_acc);
+ eeprom_write_word (p16++, speed_alpha_acc);
+ 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);
+ eeprom_write_word (p16++, pos_blocked);
+}
+
+/* Clear eeprom parameters. */
+static void
+eeprom_clear_params (void)
+{
+ uint8_t *p = (uint8_t *) EEPROM_START;
+ eeprom_write_byte (p, 0xff);
+}
+
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
new file mode 100644
index 00000000..22a282cd
--- /dev/null
+++ b/digital/asserv/src/asserv/main.c
@@ -0,0 +1,471 @@
+/* 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 "modules/math/fixed/fixed.h"
+#include "io.h"
+
+#include "misc.h"
+
+/** Motor command sequence, do not use values above 127, do not use zero. */
+uint8_t main_sequence, main_sequence_ack, main_sequence_finish;
+
+/* This is implementation include. */
+#ifndef HOST
+# include "timer.avr.c"
+# include "counter.avr.c"
+# include "pwm.avr.c"
+#else
+# include "simu.host.h"
+#endif
+#include "pos.c"
+#include "speed.c"
+#include "postrack.c"
+#include "traj.c"
+#ifndef HOST
+# include "eeprom.avr.c"
+#endif
+
+/** Motor control mode:
+ * 0: pwm setup.
+ * 1: shaft position control.
+ * 2: speed control.
+ * 3: trajectory control. */
+int8_t main_mode;
+
+/** Report trajectory end. */
+uint8_t main_sequence_ack_cpt = 2;
+
+/** Report of counters. */
+uint8_t main_stat_counter, main_stat_counter_cpt;
+
+/** Report of position. */
+uint8_t main_stat_postrack, main_stat_postrack_cpt;
+
+/** Statistics about speed control. */
+uint8_t main_stat_speed, main_stat_speed_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;
+
+#ifdef HOST
+/** Simulation data. */
+uint8_t main_simu, main_simu_cpt;
+#endif /* HOST */
+
+/** Record timer value at different stage of computing. Used for performance
+ * analisys. */
+uint8_t main_timer[6];
+
+/* +AutoDec */
+
+/** Main loop. */
+static void
+main_loop (void);
+
+/* -AutoDec */
+
+/** Entry point. */
+int
+main (int argc, char **argv)
+{
+ avr_init (argc, argv);
+ DDRD = 0x60;
+ /* Pull-ups. */
+ PORTA = 0xff;
+ PORTC = 0xff;
+ eeprom_read_params ();
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ uart0_init ();
+ postrack_init ();
+ proto_send0 ('z');
+ sei ();
+ while (1)
+ main_loop ();
+ return 0;
+}
+
+/** Main loop. */
+static void
+main_loop (void)
+{
+ main_timer[5] = timer_read ();
+ timer_wait ();
+ /* Counter update. */
+ counter_update ();
+ main_timer[0] = timer_read ();
+ /* Postion control. */
+ if (main_mode >= 1)
+ pos_update ();
+ main_timer[1] = timer_read ();
+ /* Pwm setup. */
+ pwm_update ();
+ main_timer[2] = timer_read ();
+ /* Compute absolute position. */
+ postrack_update ();
+ /* Compute trajectory. */
+ if (main_mode >= 3)
+ traj_update ();
+ /* Speed control. */
+ if (main_mode >= 2)
+ speed_update ();
+ main_timer[3] = timer_read ();
+ /* Stats. */
+ if (main_sequence_ack != main_sequence_finish && !--main_sequence_ack_cpt)
+ {
+ proto_send1b ('A', main_sequence_finish);
+ main_sequence_ack_cpt = 4;
+ }
+ 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_postrack && !--main_stat_postrack_cpt)
+ {
+ proto_send3d ('X', postrack_x, postrack_y, postrack_a);
+ main_stat_postrack_cpt = main_stat_postrack;
+ }
+ if (main_stat_speed && !--main_stat_speed_cpt)
+ {
+ proto_send2b ('S', speed_theta_cur >> 8, speed_alpha_cur >> 8);
+ main_stat_speed_cpt = main_stat_speed;
+ }
+ 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;
+ }
+#ifdef HOST
+ if (main_simu && !--main_simu_cpt)
+ {
+ proto_send3w ('Y', (uint16_t) simu_pos_x,
+ (uint16_t) simu_pos_y,
+ (uint16_t) (simu_pos_a * 1024));
+ proto_send3b ('Z', traj_mode, 0, 0);
+ main_simu_cpt = main_simu;
+ }
+#endif /* HOST */
+ 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_send6b ('T', main_timer[0], main_timer[2], main_timer[3],
+ main_timer[4], main_timer[4], main_timer[5]);
+ main_stat_timer_cpt = main_stat_timer;
+ }
+ if (main_print_pin && !--main_print_pin_cpt)
+ {
+ proto_send2b ('I', PINA, PINF & 0xf);
+ main_print_pin_cpt = main_print_pin;
+ }
+ /* Misc. */
+ while (uart0_poll ())
+ proto_accept (uart0_getc ());
+ main_timer[4] = timer_read ();
+}
+
+/** 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):
+ /* Reset. */
+ 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]);
+ UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
+ pwm_right = v8_to_v16 (args[2], args[3]);
+ UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX);
+ break;
+ case c ('c', 4):
+ /* Add to position consign.
+ * - w: theta consign offset.
+ * - w: alpha consign offset. */
+ main_mode = 1;
+ pos_theta_cons += v8_to_v16 (args[0], args[1]);
+ pos_alpha_cons += v8_to_v16 (args[2], args[3]);
+ break;
+ case c ('s', 0):
+ /* Stop (set zero speed). */
+ main_mode = 2;
+ speed_pos = 0;
+ speed_theta_cons = 0;
+ speed_alpha_cons = 0;
+ break;
+ case c ('s', 2):
+ /* Set speed.
+ * - b: theta speed.
+ * - b: alpha speed. */
+ main_mode = 2;
+ speed_pos = 0;
+ speed_theta_cons = args[0] << 8;
+ speed_alpha_cons = args[1] << 8;
+ break;
+ case c ('s', 9):
+ /* Set speed controlled position consign.
+ * - d: theta consign offset.
+ * - d: alpha consign offset.
+ * - b: sequence number. */
+ if (args[8] == main_sequence)
+ break;
+ main_mode = 2;
+ speed_pos = 1;
+ speed_theta_pos_cons = pos_theta_cons;
+ speed_theta_pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
+ speed_alpha_pos_cons = pos_alpha_cons;
+ speed_alpha_pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
+ main_sequence = args[8];
+ break;
+ case c ('f', 1):
+ /* Go to the wall.
+ * - b: sequence number. */
+ if (args[0] == main_sequence)
+ break;
+ main_mode = 3;
+ speed_pos = 0;
+ traj_mode = 10;
+ main_sequence = args[0];
+ break;
+ case c ('a', 1):
+ /* Set acknoledge.
+ * - b: ack sequence number. */
+ main_sequence_ack = args[0];
+ if (pos_blocked_state)
+ pos_reset ();
+ break;
+ /* Stats.
+ * - b: interval between stats. */
+ case c ('C', 1):
+ /* Counter stats. */
+ main_stat_counter_cpt = main_stat_counter = args[0];
+ break;
+ case c ('X', 1):
+ /* Position stats. */
+ main_stat_postrack_cpt = main_stat_postrack = args[0];
+ break;
+ case c ('S', 1):
+ /* Motor speed control stats. */
+ main_stat_speed_cpt = main_stat_speed = 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;
+#ifdef HOST
+ case c ('Y', 1):
+ /* Simulation data. */
+ main_simu_cpt = main_simu = args[0];
+ break;
+#endif /* HOST */
+ default:
+ /* Params. */
+ if (cmd == 'p')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('X', 1):
+ /* Reset position. */
+ postrack_x = 0;
+ postrack_y = 0;
+ postrack_a = 0;
+ break;
+ case c ('X', 5):
+ /* Set current x position.
+ * - d: x position. */
+ postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('Y', 5):
+ /* Set current y position.
+ * - d: y position. */
+ postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('A', 5):
+ /* Set current angle.
+ * - d: angle. */
+ postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('f', 3):
+ /* Set footing.
+ * - w: footing. */
+ postrack_set_footing (v8_to_v16 (args[1], args[2]));
+ break;
+ case c ('a', 5):
+ /* Set acceleration.
+ * - w: theta.
+ * - w: alpha. */
+ speed_theta_acc = v8_to_v16 (args[1], args[2]);
+ speed_alpha_acc = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('s', 5):
+ /* Set maximum and slow speed.
+ * - b: theta max.
+ * - b: alpha max.
+ * - b: theta slow.
+ * - b: alpha slow. */
+ speed_theta_max = args[1];
+ speed_alpha_max = args[2];
+ speed_theta_slow = args[3];
+ speed_alpha_slow = args[4];
+ break;
+ 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 ('b', 3):
+ pos_blocked = 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_send1w ('f', postrack_footing);
+ proto_send2w ('a', speed_theta_acc, speed_alpha_acc);
+ proto_send2b ('s', speed_theta_max, speed_alpha_max);
+ 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;
+ }
+ }
+#ifdef HOST
+ else if (cmd == 'y')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('X', 7):
+ /* Set simulated position.
+ * - w: x position.
+ * - w: y position.
+ * - w: angle (rad) * 1024. */
+ simu_pos_x = (double) v8_to_v16 (args[1], args[2]);
+ simu_pos_y = (double) v8_to_v16 (args[3], args[4]);
+ simu_pos_a = (double) v8_to_v16 (args[5], args[6]) / 1024;
+ break;
+ }
+ }
+#endif /* HOST */
+ else
+ {
+ proto_send0 ('?');
+ return;
+ }
+ break;
+ }
+ proto_send (cmd, size, args);
+#undef c
+}
+
diff --git a/digital/asserv/src/asserv/misc.h b/digital/asserv/src/asserv/misc.h
new file mode 100644
index 00000000..59f25135
--- /dev/null
+++ b/digital/asserv/src/asserv/misc.h
@@ -0,0 +1,38 @@
+#ifndef misc_h
+#define misc_h
+/* misc.h - Miscellaneous definitions. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 RED_LED(x) do { \
+ if (!(x)) PORTD |= 0x20; \
+ else PORTD &= ~0x20; \
+} while (0)
+
+#define GREEN_LED(x) do { \
+ if (!(x)) PORTD |= 0x40; \
+ else PORTD &= ~0x40; \
+} while (0)
+
+#endif /* misc_h */
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
new file mode 100644
index 00000000..976edd3e
--- /dev/null
+++ b/digital/asserv/src/asserv/models.host.c
@@ -0,0 +1,154 @@
+/* models.host.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 "motor_model.host.h"
+#include "models.host.h"
+
+#include <math.h>
+#include <string.h>
+
+/* Gloubi model. */
+static const struct motor_t gloubi_model =
+{
+ /* Motor caracteristics. */
+ 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
+ 23.4 / 1000, /* Torque constant (N.m/A). */
+ 0, /* Bearing friction (N.m/(rad/s)). */
+ 2.18, /* Terminal resistance (Ohm). */
+ 0.24 / 1000, /* Terminal inductance (H). */
+ /* Gearbox caracteristics. */
+ 10, /* Gearbox ratio. */
+ 0.75, /* Gearbox efficiency. */
+ /* Load caracteristics. */
+ 4 * 0.02 * 0.02, /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ 0.02, /* Wheel radius (m). */
+ /* Simulation parameters. */
+ 4.444444 / 1000, /* Simulation time step (s). */
+ 1000, /* Simulation time step division. */
+ /* Simulation current state. */
+ 0, /* Current time (not realy used) (s). */
+ 0, /* Current input voltage (V). */
+ 0, /* Current current (A). */
+ 0, /* Current angular speed (o for omega) (rad/s). */
+ 0 /* Current theta (th for theta) (rad). */
+};
+
+static const struct robot_t gloubi_robot =
+{
+ &gloubi_model,
+ 26.0, /* Distance between the wheels (m). */
+};
+
+/* Taz model. */
+static const struct motor_t taz_model =
+{
+ /* Motor caracteristics. */
+ 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
+ 23.4 / 1000, /* Torque constant (N.m/A). */
+ 0, /* Bearing friction (N.m/(rad/s)). */
+ 2.18, /* Terminal resistance (Ohm). */
+ 0.24 / 1000, /* Terminal inductance (H). */
+ /* Gearbox caracteristics. */
+ 10, /* Gearbox ratio. */
+ 0.75, /* Gearbox efficiency. */
+ /* Load caracteristics. */
+ 10 * 0.04 * 0.04, /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ 0.04, /* Wheel radius (m). */
+ /* Simulation parameters. */
+ 4.444444 / 1000, /* Simulation time step (s). */
+ 1000, /* Simulation time step division. */
+ /* Simulation current state. */
+ 0, /* Current time (not realy used) (s). */
+ 0, /* Current input voltage (V). */
+ 0, /* Current current (A). */
+ 0, /* Current angular speed (o for omega) (rad/s). */
+ 0 /* Current theta (th for theta) (rad). */
+};
+
+static const struct robot_t taz_robot =
+{
+ &taz_model,
+ 30.0, /* Distance between the wheels (m). */
+};
+
+/* Taz model, with a RE25G and a 1:20.25 ratio gearbox. */
+static const struct motor_t tazg_model =
+{
+ /* Motor caracteristics. */
+ 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
+ 40.2 / 1000, /* Torque constant (N.m/A). */
+ 0, /* Bearing friction (N.m/(rad/s)). */
+ 2.32, /* Terminal resistance (Ohm). */
+ 0.24 / 1000, /* Terminal inductance (H). */
+ /* Gearbox caracteristics. */
+ 20.25, /* Gearbox ratio. */
+ 0.75, /* Gearbox efficiency. */
+ /* Load caracteristics. */
+ 10 * 0.04 * 0.04, /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ 0.04, /* Wheel radius (m). */
+ /* Simulation parameters. */
+ 4.444444 / 1000, /* Simulation time step (s). */
+ 1000, /* Simulation time step division. */
+ /* Simulation current state. */
+ 0, /* Current time (not realy used) (s). */
+ 0, /* Current input voltage (V). */
+ 0, /* Current current (A). */
+ 0, /* Current angular speed (o for omega) (rad/s). */
+ 0 /* Current theta (th for theta) (rad). */
+};
+
+static const struct robot_t tazg_robot =
+{
+ &tazg_model,
+ 30.0, /* Distance between the wheels (m). */
+};
+
+/* Table of models. */
+static const struct
+{
+ const char *name;
+ const struct robot_t *robot;
+} models[] = {
+ { "gloubi", &gloubi_robot },
+ { "taz", &taz_robot },
+ { "tazg", &tazg_robot },
+ { 0, 0 }
+};
+
+/** Get a pointer to a model by name, or return 0. */
+const struct robot_t *
+models_get (const char *name)
+{
+ int i;
+ for (i = 0; models[i].name; i++)
+ {
+ if (strcmp (models[i].name, name) == 0)
+ return models[i].robot;
+ }
+ return 0;
+}
+
diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h
new file mode 100644
index 00000000..4ae55ad8
--- /dev/null
+++ b/digital/asserv/src/asserv/models.host.h
@@ -0,0 +1,38 @@
+#ifndef models_host_h
+#define models_host_h
+/* models.host.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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.
+ *
+ * }}} */
+
+struct robot_t
+{
+ const struct motor_t *motor;
+ double footing; /* Distance between the wheels (m). */
+};
+
+/** Get a pointer to a model by name, or return 0. */
+const struct robot_t *
+models_get (const char *name);
+
+#endif /* models_host_h */
diff --git a/digital/asserv/src/asserv/motor_model.host.c b/digital/asserv/src/asserv/motor_model.host.c
new file mode 100644
index 00000000..c8b4639c
--- /dev/null
+++ b/digital/asserv/src/asserv/motor_model.host.c
@@ -0,0 +1,88 @@
+/* motor_model.c - DC motor model. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 "motor_model.host.h"
+
+/**
+ * Switching to french for all thoses non english speaking people.
+ *
+ * Ce fichier permet de modéliser un moteur à courant continue. Il y a deux
+ * parties, la modélisation électrique et la modélisation mécanique.
+ *
+ * On peut trouver de l'aide sur :
+ * - le site de maxon : http://www.maxonmotor.com/
+ * - ici : http://www.mathworks.com/access/helpdesk/help/toolbox/control/getstart/buildmo4.html
+ * - là : http://iai.eivd.ch/users/mee/Regulation_automatique_anal.htm
+ *
+ * Voici ce qui en résulte, des belles équations différentielles :
+ * u(t) = R i(t) + L di(t)/dt + Ke o(t)
+ * J do(t)/dt = Kt i(t) - Rf o(t)
+ * dth(t)/dt = o(t)
+ *
+ * Les variables sont décrites dans la structure motor_t.
+ *
+ * À cela, il faut ajouter un coef pour le réducteur, je vous laisse ça en
+ * exercice.
+ *
+ * On va résoudre ces belles équadiff numériquement par la méthode d'Euler (il
+ * est partout celui là). Si vous voulez plus de détail, mailez moi. On arrive
+ * à :
+ *
+ * i(t+h) = i(t) + h (1/L u(t) - R/L i(t) - 1/(Ke L) o(t))
+ * o(t+h) = o(t) + h (i_G^2 ro_G)/J (Kt i(t) - Rf o(t))
+ * th(t+h) = th(t) + h o(t)
+ *
+ * C'est consternant de simplicité non ?
+ */
+
+/** Make a simulation step. */
+void motor_model_step (struct motor_t *m)
+{
+ double i_, o_, th_; /* New computed values. */
+ double h; /* Infinitesimal step... Well, not so infinite. */
+ int d;
+ d = m->d;
+ h = m->h / d;
+ /* Make several small steps to increase precision. */
+ for (; d; d--)
+ {
+ /* Ah, the mistical power of computation. */
+ i_ = m->i
+ + h * (1.0 / m->L * m->u
+ - m->R / m->L * m->i
+ - 1.0 / m->Ke / m->L * m->o);
+ o_ = m->o
+ + h * m->i_G * m->i_G * m->ro_G / m->J
+ * (m->Kt * m->i - m->Rf * m->o);
+ th_ = m->th + h * m->o;
+ /* Ok, now store this step. */
+ m->i = i_;
+ m->o = o_;
+ m->th = th_;
+ }
+ /* Damn! It's finished yet! */
+ m->t += m->h;
+}
+
diff --git a/digital/asserv/src/asserv/motor_model.host.h b/digital/asserv/src/asserv/motor_model.host.h
new file mode 100644
index 00000000..931890a7
--- /dev/null
+++ b/digital/asserv/src/asserv/motor_model.host.h
@@ -0,0 +1,58 @@
+#ifndef motor_model_host_h
+#define motor_model_host_h
+/* motor_model.host.h - DC motor model. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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.
+ *
+ * }}} */
+
+/** Motor and load caracteristics and current data. */
+struct motor_t
+{
+ /* Motor caracteristics. */
+ double Ke; /* Speed constant ((rad/s)/V). */
+ double Kt; /* Torque constant (N.m/A). */
+ double Rf; /* Bearing friction (N.m/(rad/s)). */
+ double R; /* Terminal resistance (Ohm). */
+ double L; /* Terminal inductance (H). */
+ /* Gearbox caracteristics. */
+ double i_G; /* Gearbox ratio. */
+ double ro_G;/* Gearbox efficiency. */
+ /* Load caracteristics. */
+ double J; /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ double w_r; /* Wheel radius (m). */
+ /* Simulation parameters. */
+ double h; /* Simulation time step (s). */
+ int d; /* Simulation time step division. */
+ /* Simulation current state. */
+ double t; /* Current time (not realy used) (s). */
+ double u; /* Current input voltage (V). */
+ double i; /* Current current (A). */
+ double o; /* Current angular speed (o for omega) (rad/s). */
+ double th; /* Current theta (th for theta) (rad). */
+};
+
+/** Make a simulation step. */
+void motor_model_step (struct motor_t *m);
+
+#endif /* motor_model_host_h */
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
new file mode 100644
index 00000000..eecd71f3
--- /dev/null
+++ b/digital/asserv/src/asserv/pos.c
@@ -0,0 +1,144 @@
+/* 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. */
+uint32_t pos_theta_cur, pos_alpha_cur;
+/** Consign theta/alpha. */
+uint32_t pos_theta_cons, pos_alpha_cons;
+
+/** Error saturation. */
+int32_t pos_e_sat = 1023;
+/** Integral saturation. */
+int32_t pos_int_sat = 1023;
+/** P coefficients. */
+uint16_t pos_theta_kp, pos_alpha_kp;
+/** I coefficients. */
+uint16_t pos_theta_ki, pos_alpha_ki;
+/** D coefficients. */
+uint16_t pos_theta_kd, pos_alpha_kd;
+/** Blocked value. If error is greater than this value, stop the robot and
+ * report blocked state. */
+int32_t pos_blocked = 15000L;
+
+/** Current integral values. */
+int32_t pos_theta_int, pos_alpha_int;
+/** Last error values. */
+int32_t pos_theta_e_old, pos_alpha_e_old;
+/** One if blocked. */
+uint8_t pos_blocked_state;
+
+/* +AutoDec */
+/* -AutoDec */
+
+/** Compute a PID.
+ * How to compute maximum numbers size:
+ * Result is 24 bits (16 bits kept after shift).
+ * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff
+ * max is 12 bits.
+ * If int_sat == 1023, i max is 11 bits.
+ * In the final addition, let's give 23 bits to the p part, and 22 bits to the
+ * i and d part (23b + 22b + 22b => 23b + 23b => 24b).
+ * Therefore, kp can be 23 - 11 = 12 bits (f4.8).
+ * ki can be 22 - 11 = 11 bits (f3.8).
+ * kd can be 22 - 12 = 10 bits (f2.8).
+ * How to increase this number:
+ * - lower the shift.
+ * - bound the value returned.
+ * - lower e & int saturation. */
+static inline int16_t
+pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old,
+ uint16_t kp, uint16_t ki, uint16_t kd)
+{
+ int32_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 >> 8;
+}
+
+/** Update PWM according to consign. */
+static void
+pos_update (void)
+{
+ int16_t pid_theta, pid_alpha;
+ int32_t diff_theta, diff_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. */
+ diff_theta = pos_theta_cons - pos_theta_cur;
+ diff_alpha = pos_alpha_cons - pos_alpha_cur;
+ if (pos_blocked_state
+ || diff_theta < -pos_blocked || pos_blocked < diff_theta
+ || diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
+ {
+ /* Blocked. */
+ pwm_left = 0;
+ pwm_right = 0;
+ pos_blocked_state = 1;
+ main_sequence_finish = main_sequence | 0x80;
+ }
+ else
+ {
+ pid_theta = pos_compute_pid (diff_theta, &pos_theta_int,
+ &pos_theta_e_old, pos_theta_kp,
+ pos_theta_ki, pos_theta_kd);
+ pid_alpha = pos_compute_pid (diff_alpha, &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;
+ pos_theta_cur = pos_alpha_cur = 0;
+ pos_theta_cons = pos_alpha_cons = 0;
+ pos_theta_e_old = pos_alpha_e_old = 0;
+ pos_blocked_state = 0;
+}
diff --git a/digital/asserv/src/asserv/postrack.c b/digital/asserv/src/asserv/postrack.c
new file mode 100644
index 00000000..6da77592
--- /dev/null
+++ b/digital/asserv/src/asserv/postrack.c
@@ -0,0 +1,116 @@
+/* postrack.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 = (2 * 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 f10.16 format, we multiply by 256 to have
+ * a angle in f8.24 format.
+ * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24).
+ * - there is a two factor because of the sum done in the motor control. */
+uint32_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)
+{
+ /* Prevent division by 0 by providing a default large value. */
+ postrack_set_footing (0x1000);
+}
+
+#define M_PI 3.14159265358979323846 /* pi */
+
+/** Update the current position. */
+static inline void
+postrack_update (void)
+{
+ int32_t d, dd, da, na, dsc;
+ d = counter_right_diff + counter_left_diff; /* 10b */
+ d <<= 16; /* 10.16b */
+ if (counter_right_diff == counter_left_diff)
+ {
+ /* Line. */
+ postrack_x += fixed_mul_f824 (d, fixed_cos_f824 (postrack_a)) >> 8;
+ postrack_y += fixed_mul_f824 (d, fixed_sin_f824 (postrack_a)) >> 8;
+ }
+ else
+ {
+ /* Arc. */
+ dd = counter_right_diff - counter_left_diff; /* 10b */
+ dd <<= 16; /* 10.16b */
+ da = fixed_mul_f824 (dd, postrack_footing_factor);/* 8.24b */
+ /* New angle. */
+ na = postrack_a + da;
+ /* Compute da in radians. */
+ da = fixed_mul_f824 (da, 2 * M_PI * (1L << 24));
+ /* X increment. */
+ dsc = fixed_sin_f824 (na) - fixed_sin_f824 (postrack_a); /* 8.24b */
+ dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */
+ postrack_x += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */
+ /* Y increment. */
+ dsc = fixed_cos_f824 (postrack_a) - fixed_cos_f824 (na); /* 8.24b */
+ dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */
+ postrack_y += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */
+ /* Angle update. */
+ 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 =
+ (uint32_t) (M_1_PI * (1L << 8) * (1L << 24)) / footing;
+}
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
new file mode 100644
index 00000000..773cfef0
--- /dev/null
+++ b/digital/asserv/src/asserv/pwm.avr.c
@@ -0,0 +1,125 @@
+/* pwm.avr.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 0x3ff
+
+/** 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 = 0x3ff, OC1B & OC1C with positive logic.
+ f_IO without prescaler.
+ Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 1, 0, 1, 0, 1, 1);
+ 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/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
new file mode 100644
index 00000000..beefafe2
--- /dev/null
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -0,0 +1,185 @@
+/* simu.host.c */
+/* {{{
+ *
+ * Copyright (C) 2006 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/
+ * }}} */
+#include "common.h"
+#include "modules/host/host.h"
+#include "modules/utils/utils.h"
+#include "simu.host.h"
+#include "motor_model.host.h"
+#include "models.host.h"
+
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+/** Simulate some AVR regs. */
+uint8_t DDRD, PORTD, PORTA, PORTC, PINA, PINF;
+
+/** Overall counter values. */
+uint16_t counter_left, counter_right;
+/** Counter differences since last update.
+ * Maximum of 9 significant bits, sign included. */
+int16_t counter_left_diff, counter_right_diff;
+
+/** 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;
+
+struct motor_t simu_left_model, simu_right_model;
+
+/** Computed simulated position (mm). */
+double simu_pos_x, simu_pos_y, simu_pos_a;
+
+/** Distance between wheels. */
+double simu_footing;
+
+/** Initialise simulation. */
+static void
+simu_init (void)
+{
+ int argc;
+ char **argv;
+ const struct robot_t *m;
+ host_get_program_arguments (&argc, &argv);
+ if (argc != 1)
+ {
+ fprintf (stderr, "need model name as first argument\n");
+ exit (1);
+ }
+ m = models_get (argv[0]);
+ if (!m)
+ {
+ fprintf (stderr, "unknown model name: %s\n", argv[0]);
+ exit (1);
+ }
+ simu_left_model = *m->motor;
+ simu_right_model = *m->motor;
+ simu_footing = m->footing;
+ simu_pos_x = simu_pos_y = 0;
+}
+
+/** Update simulation position. */
+static void
+simu_pos_update (double dl, double dr)
+{
+ double d = 0.5 * (dl + dr);
+ double da = (dr - dl) / simu_footing;
+ double na = simu_pos_a + da;
+ if (da < 0.0001 && da > -0.0001)
+ {
+ /* Avoid a division by zero when angle is too small. */
+ double a = simu_pos_a + da * 0.5;
+ simu_pos_x += d * cos (a);
+ simu_pos_y += d * sin (a);
+ }
+ else
+ {
+ /* Radius of turn is d / da. */
+ simu_pos_x += (sin (na) - sin (simu_pos_a)) * d / da;
+ simu_pos_y += (cos (simu_pos_a) - cos (na)) * d / da;
+ }
+ simu_pos_a = na;
+}
+
+/** Do a simulation step. */
+static void
+simu_step (void)
+{
+ double old_left_th, old_right_th;
+ /* Convert pwm value into voltage. */
+ assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX);
+ assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX);
+ simu_left_model.u = (double) (pwm_left + 1) / (PWM_MAX + 1);
+ simu_right_model.u = (double) (pwm_right + 1) / (PWM_MAX + 1);
+ /* Make one step. */
+ old_left_th = simu_left_model.th;
+ old_right_th = simu_right_model.th;
+ motor_model_step (&simu_left_model);
+ motor_model_step (&simu_right_model);
+ /* Modify counters. */
+ counter_left_diff = (simu_left_model.th - old_left_th) / (2*M_PI)
+ * 500 * simu_left_model.i_G;
+ counter_left += counter_left_diff;
+ counter_right_diff = (simu_right_model.th - old_right_th) / (2*M_PI)
+ * 500 * simu_right_model.i_G;
+ counter_right += counter_right_diff;
+ /* Update position */
+ simu_pos_update ((simu_left_model.th - old_left_th)
+ * simu_left_model.i_G * simu_left_model.w_r * 1000,
+ (simu_right_model.th - old_right_th)
+ * simu_right_model.i_G * simu_right_model.w_r * 1000);
+}
+
+/** Initialise the timer. */
+void
+timer_init (void)
+{
+ simu_init ();
+}
+
+/** Wait for timer overflow. */
+void
+timer_wait (void)
+{
+ simu_step ();
+}
+
+/** Read timer value. Used for performance analysis. */
+uint8_t
+timer_read (void)
+{
+ return 0;
+}
+
+/** Initialize the counters. */
+void
+counter_init (void)
+{
+}
+
+/** Update overall counter values and compute diffs. */
+void
+counter_update (void)
+{
+}
+
+/** Restart counting. */
+void
+counter_restart (void)
+{
+}
+
+/** Initialise PWM generator. */
+void
+pwm_init (void)
+{
+}
+
+/** Update the hardware PWM values. */
+void
+pwm_update (void)
+{
+}
+
diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h
new file mode 100644
index 00000000..2730a71f
--- /dev/null
+++ b/digital/asserv/src/asserv/simu.host.h
@@ -0,0 +1,94 @@
+#ifndef simu_host_h
+#define simu_host_h
+/* simu.host.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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/
+ * }}} */
+
+/** Simulate some AVR regs. */
+extern uint8_t DDRD, PORTD, PORTA, PORTC, PINA, PINF;
+
+/** Overall counter values. */
+extern uint16_t counter_left, counter_right;
+/** Counter differences since last update.
+ * Maximum of 9 significant bits, sign included. */
+extern int16_t counter_left_diff, counter_right_diff;
+
+/** 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 0x3ff
+
+/** PWM values, this is an error if absolute value is greater than the
+ * maximum. */
+extern int16_t pwm_left, pwm_right;
+/** PWM reverse direction, only set pwm dir bits or you will get weird results
+ * on port B. */
+extern uint8_t pwm_dir;
+
+/** Computed simulated position. */
+extern double simu_pos_x, simu_pos_y, simu_pos_a;
+
+#define EEPROM_KEY 0xa5
+#define eeprom_read_params() do { } while (0)
+#define eeprom_write_params() do { } while (0)
+#define eeprom_clear_params() do { } while (0)
+
+/* +AutoDec */
+
+/** Initialise the timer. */
+void
+timer_init (void);
+
+/** Wait for timer overflow. */
+void
+timer_wait (void);
+
+/** Read timer value. Used for performance analysis. */
+uint8_t
+timer_read (void);
+
+/** Initialize the counters. */
+void
+counter_init (void);
+
+/** Update overall counter values and compute diffs. */
+void
+counter_update (void);
+
+/** Restart counting. */
+void
+counter_restart (void);
+
+/** Initialise PWM generator. */
+void
+pwm_init (void);
+
+/** Update the hardware PWM values. */
+void
+pwm_update (void);
+
+/* -AutoDec */
+
+#endif /* simu_host_h */
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
new file mode 100644
index 00000000..7e8ed558
--- /dev/null
+++ b/digital/asserv/src/asserv/speed.c
@@ -0,0 +1,123 @@
+/* speed.c - Speed 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 speed control. It changes the current shafts
+ * positions using ramps. It can be controlled by a wanted speed or wanted
+ * shaft position.
+ */
+
+/** Current speed, f8.8. */
+int16_t speed_theta_cur, speed_alpha_cur;
+/** Consign speed, f8.8. */
+int16_t speed_theta_cons, speed_alpha_cons;
+/** Maximum speed for position consign, u8. */
+int8_t speed_theta_max, speed_alpha_max;
+/** Slow speed for position consign, u8. */
+int8_t speed_theta_slow, speed_alpha_slow;
+/** Consign position. */
+uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
+/** Weither to use the consign position (1) or not (0). */
+uint8_t speed_pos;
+
+/** Acceleration, uf8.8. */
+int16_t speed_theta_acc, speed_alpha_acc;
+
+/* +AutoDec */
+/* -AutoDec */
+
+/** Update shaft position consign according to a speed consign. */
+static void
+speed_update_by_speed (void)
+{
+ /* Update current speed. */
+ if (UTILS_ABS (speed_theta_cons - speed_theta_cur) < speed_theta_acc)
+ speed_theta_cur = speed_theta_cons;
+ else if (speed_theta_cons > speed_theta_cur)
+ speed_theta_cur += speed_theta_acc;
+ else
+ speed_theta_cur -= speed_theta_acc;
+ if (UTILS_ABS (speed_alpha_cons - speed_alpha_cur) < speed_alpha_acc)
+ speed_alpha_cur = speed_alpha_cons;
+ else if (speed_alpha_cons > speed_alpha_cur)
+ speed_alpha_cur += speed_alpha_acc;
+ else
+ speed_alpha_cur -= speed_alpha_acc;
+}
+
+/** Compute maximum allowed speed according to: distance left, maximum speed,
+ * current speed and acceleration. */
+static int16_t
+speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
+{
+ int16_t s;
+ /* Compute maximum speed in order to be able to brake in time.
+ * s = sqrt (2 * a * d) */
+ s = fixed_sqrt_ui32 (2 * (acc >> 8) * UTILS_ABS (d));
+ /* Apply consign. */
+ s = UTILS_MIN (max, s);
+ /* Apply sign. */
+ if (d < 0)
+ s = -s;
+ /* Convert to f8.8 and check acceleration. */
+ s = s << 8;
+ UTILS_BOUND (s, cur - acc, cur + acc);
+ return s;
+}
+
+/** Update shaft position consign according to a position consign. */
+static void
+speed_update_by_position (void)
+{
+ int32_t theta_d = speed_theta_pos_cons - pos_theta_cons;
+ int32_t alpha_d = speed_alpha_pos_cons - pos_alpha_cons;
+ if (theta_d >= -speed_theta_max && theta_d <= speed_theta_max)
+ speed_theta_cur = theta_d << 8;
+ else
+ speed_theta_cur = speed_compute_max_speed (theta_d, speed_theta_cur,
+ speed_theta_acc, speed_theta_max);
+ if (alpha_d >= -speed_alpha_max && alpha_d <= speed_alpha_max)
+ speed_alpha_cur = alpha_d << 8;
+ else
+ speed_alpha_cur = speed_compute_max_speed (alpha_d, speed_alpha_cur,
+ speed_alpha_acc, speed_alpha_max);
+ if (speed_theta_cur == 0 && speed_alpha_cur == 0)
+ main_sequence_finish = main_sequence;
+}
+
+/** Update shaft position consign according to consign. */
+static void
+speed_update (void)
+{
+ /* Update speed. */
+ if (speed_pos)
+ speed_update_by_position ();
+ else
+ speed_update_by_speed ();
+ /* Update shaft position. */
+ pos_theta_cons += speed_theta_cur >> 8;
+ pos_alpha_cons += speed_alpha_cur >> 8;
+}
+
diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c
new file mode 100644
index 00000000..f5fb575b
--- /dev/null
+++ b/digital/asserv/src/asserv/test_motor_model.c
@@ -0,0 +1,70 @@
+/* test_motor_model.c - Test DC motor model. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 "motor_model.host.h"
+#include "models.host.h"
+
+#include <stdio.h>
+
+void simu (struct motor_t *m, double t)
+{
+ int i, s;
+ s = t / m->h;
+ for (i = 0; i < s; i++)
+ {
+ motor_model_step (m);
+ printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th);
+ }
+}
+
+int
+main (int argc, char **argv)
+{
+ struct motor_t ms;
+ struct motor_t *m;
+ const struct robot_t *mr;
+ /* Check arguments. */
+ if (argc != 2)
+ {
+ fprintf (stderr, "syntax: %s MODEL\n", argv[0]);
+ return 1;
+ }
+ mr = models_get (argv[1]);
+ if (!mr)
+ {
+ fprintf (stderr, "model unknown\n");
+ return 1;
+ }
+ ms = *mr->motor;
+ m = &ms;
+ /* Make a step response simulation. */
+ printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta");
+ m->u = 3.0;
+ printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th);
+ simu (m, 1.0);
+ m->u = 0.0;
+ simu (m, 1.0);
+ return 0;
+}
diff --git a/digital/asserv/src/asserv/timer.avr.c b/digital/asserv/src/asserv/timer.avr.c
new file mode 100644
index 00000000..dcad74d0
--- /dev/null
+++ b/digital/asserv/src/asserv/timer.avr.c
@@ -0,0 +1,70 @@
+/* timer.avr.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 */
+
+/** Initialise the timer. */
+static inline void
+timer_init (void);
+
+/** Wait for timer overflow. */
+static inline void
+timer_wait (void);
+
+/** Read timer value. Used for performance analysis. */
+static inline uint8_t
+timer_read (void);
+
+/* -AutoDec */
+
+/** Initialise the timer. */
+static inline void
+timer_init (void)
+{
+ 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
+ * prescaler = 256
+ * Tov = 1 / Fov = 4.444 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;
+}
+
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
new file mode 100644
index 00000000..b80a7513
--- /dev/null
+++ b/digital/asserv/src/asserv/traj.c
@@ -0,0 +1,76 @@
+/* traj.c - Trajectories. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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.
+ *
+ * }}} */
+
+/** Traj mode:
+ * 10, 11: go to the wall.
+ */
+uint8_t traj_mode;
+
+/** Go to the wall mode. */
+void
+traj_ftw (void)
+{
+ int16_t speed;
+ speed = speed_theta_slow;
+ speed *= 256;
+ if (PINA & _BV (0) && PINA & _BV (7))
+ {
+ speed_theta_cons = -speed;
+ speed_alpha_cons = 0;
+ }
+ else if (PINA & _BV (0))
+ {
+ speed_theta_cons = -speed / 2;
+ speed_alpha_cons = speed / 2;
+ }
+ else if (PINA & _BV (7))
+ {
+ speed_theta_cons = -speed / 2;
+ speed_alpha_cons = -speed / 2;
+ }
+ else
+ {
+ speed_theta_cons = 0;
+ speed_alpha_cons = 0;
+ speed_theta_cur = 0;
+ speed_alpha_cur = 0;
+ main_sequence_finish = main_sequence;
+ traj_mode = 11;
+ }
+}
+
+/* Compute new speed according the defined trajectory. */
+static void
+traj_update (void)
+{
+ switch (traj_mode)
+ {
+ case 10:
+ traj_ftw ();
+ case 11:
+ break;
+ }
+}
+