From f30e14673a84863fd51ec36fcf771221335161ba Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 30 Mar 2011 01:09:44 +0200 Subject: digital/avr/modules/motor: add new encoder module --- digital/avr/modules/motor/README | 25 +++ digital/avr/modules/motor/encoder/Makefile | 5 + digital/avr/modules/motor/encoder/Makefile.module | 1 + digital/avr/modules/motor/encoder/avrconfig.h | 32 ++++ digital/avr/modules/motor/encoder/encoder.h | 71 ++++++++ digital/avr/modules/motor/encoder/encoder.txt | 59 +++++++ .../avr/modules/motor/encoder/encoder_corrector.c | 55 ++++++ .../avr/modules/motor/encoder/encoder_corrector.h | 55 ++++++ digital/avr/modules/motor/encoder/encoder_host.h | 37 ++++ .../avr/modules/motor/encoder/encoder_host.host.c | 42 +++++ .../avr/modules/motor/encoder/ext/Makefile.module | 1 + digital/avr/modules/motor/encoder/ext/avrconfig.h | 48 ++++++ .../modules/motor/encoder/ext/encoder_ext.avr.c | 190 ++++++++++++++++++++ .../avr/modules/motor/encoder/ext/encoder_ext.h | 45 +++++ digital/avr/modules/motor/encoder/test/Makefile | 15 ++ .../avr/modules/motor/encoder/test/avrconfig_io.h | 110 ++++++++++++ .../modules/motor/encoder/test/avrconfig_xmem.h | 102 +++++++++++ .../avr/modules/motor/encoder/test/test_encoder.c | 192 +++++++++++++++++++++ 18 files changed, 1085 insertions(+) create mode 100644 digital/avr/modules/motor/README create mode 100644 digital/avr/modules/motor/encoder/Makefile create mode 100644 digital/avr/modules/motor/encoder/Makefile.module create mode 100644 digital/avr/modules/motor/encoder/avrconfig.h create mode 100644 digital/avr/modules/motor/encoder/encoder.h create mode 100644 digital/avr/modules/motor/encoder/encoder.txt create mode 100644 digital/avr/modules/motor/encoder/encoder_corrector.c create mode 100644 digital/avr/modules/motor/encoder/encoder_corrector.h create mode 100644 digital/avr/modules/motor/encoder/encoder_host.h create mode 100644 digital/avr/modules/motor/encoder/encoder_host.host.c create mode 100644 digital/avr/modules/motor/encoder/ext/Makefile.module create mode 100644 digital/avr/modules/motor/encoder/ext/avrconfig.h create mode 100644 digital/avr/modules/motor/encoder/ext/encoder_ext.avr.c create mode 100644 digital/avr/modules/motor/encoder/ext/encoder_ext.h create mode 100644 digital/avr/modules/motor/encoder/test/Makefile create mode 100644 digital/avr/modules/motor/encoder/test/avrconfig_io.h create mode 100644 digital/avr/modules/motor/encoder/test/avrconfig_xmem.h create mode 100644 digital/avr/modules/motor/encoder/test/test_encoder.c (limited to 'digital/avr') diff --git a/digital/avr/modules/motor/README b/digital/avr/modules/motor/README new file mode 100644 index 00000000..77008c10 --- /dev/null +++ b/digital/avr/modules/motor/README @@ -0,0 +1,25 @@ +motor - Motor control module. + +Provide tools to control motors, from encoder to pwm output, including +position and speed control. + + +Copyright (C) 2010 Nicolas Schodet + +APBTeam: + Web: http://apbteam.org/ + Email: team AT apbteam DOT org + +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. diff --git a/digital/avr/modules/motor/encoder/Makefile b/digital/avr/modules/motor/encoder/Makefile new file mode 100644 index 00000000..11b8d51b --- /dev/null +++ b/digital/avr/modules/motor/encoder/Makefile @@ -0,0 +1,5 @@ +BASE = ../../.. +DOC = encoder.html +EXTRACTDOC = encoder.h encoder_corrector.h avrconfig.h + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/motor/encoder/Makefile.module b/digital/avr/modules/motor/encoder/Makefile.module new file mode 100644 index 00000000..3b7856a1 --- /dev/null +++ b/digital/avr/modules/motor/encoder/Makefile.module @@ -0,0 +1 @@ +motor_encoder_SOURCES = encoder_corrector.c encoder_host.host.c diff --git a/digital/avr/modules/motor/encoder/avrconfig.h b/digital/avr/modules/motor/encoder/avrconfig.h new file mode 100644 index 00000000..c107fa80 --- /dev/null +++ b/digital/avr/modules/motor/encoder/avrconfig.h @@ -0,0 +1,32 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - motor/encoder configuration template. */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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/encoder - Encoder module. */ +/** Use external encoder module. */ +#define AC_ENCODER_USE_EXT 1 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/motor/encoder/encoder.h b/digital/avr/modules/motor/encoder/encoder.h new file mode 100644 index 00000000..5738dd7c --- /dev/null +++ b/digital/avr/modules/motor/encoder/encoder.h @@ -0,0 +1,71 @@ +#ifndef encoder_h +#define encoder_h +/* encoder.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 "preproc.h" + +/** Encoder state. */ +struct encoder_t +{ + /** Current position. */ + uint16_t cur; + /** Difference from last update. */ + int16_t diff; +}; +typedef struct encoder_t encoder_t; + +#ifdef HOST +# include "encoder_host.h" +# define ENCODER_MODULE host +#else +# if AC_ENCODER_USE_EXT +# include "ext/encoder_ext.h" +# define ENCODER_MODULE ext +# endif +#endif + +/** Initialize an encoder and attach it to provided encoder structure. */ +extern inline void +encoder_init (uint8_t index, encoder_t *encoder) +{ + PREPROC_PASTE (encoder_, ENCODER_MODULE, _init) (index, encoder); +} + +/** Update one step. If encoders are not read fast enough, they could + * overflow, call this function often for an update step. */ +extern inline void +encoder_update_step (void) +{ + PREPROC_PASTE (encoder_, ENCODER_MODULE, _update_step) (); +} + +/** Update overall encoder values and compute diffs. */ +extern inline void +encoder_update (void) +{ + PREPROC_PASTE (encoder_, ENCODER_MODULE, _update) (); +} + +#endif /* encoder_h */ diff --git a/digital/avr/modules/motor/encoder/encoder.txt b/digital/avr/modules/motor/encoder/encoder.txt new file mode 100644 index 00000000..fe5ce505 --- /dev/null +++ b/digital/avr/modules/motor/encoder/encoder.txt @@ -0,0 +1,59 @@ +====================== + motor/encoder module +====================== +:Author: Nicolas Schodet + +Introduction +============ + +The encoder module interfaces with hardware counters to feed back the motor +control system with the current state of the motors. These counters are +usually connected to an optical or magnetic encoder connected to the motor +shaft. + +Encoder reading +=============== + +In the `encoder_t` structure, the encoder module provides: + + - overall counter value since initialization (field `cur`), + - difference since last update (field `diff`). + +The `cur` field will roll over on overflow. + +Care should be taken about value amplitude in order not to overflow later +computations. + +Each hardware counter is assigned an index from the compile time +configuration. Code is responsible to call `encoder_init` to initialize +hardware and associate an `encoder_t` structure with each index. + +Then, `encoder_update` should be called to query hardware counter and update +the structure. + +If motor is running too fast for the hardware to avoid counter overflow, +`encoder_update_step` should be called at short interval so that no overflow +occurs. + +You should not write to the `encoder_t` structure. + +Encoder correction +================== + +When two parallel wheels are used with encoders, it is almost impossible that +both wheels have exactly the same radius. To handle the problem, the encoder +module provides a corrector which should be used with one of the wheels. + +It should be initialized using `encoder_corrector_init` and an +`encoder_corrector_t` structure. This structure does not contain any user +usable field but is used by the corrector to store internal state. Instead, +call `encoder_corrector_update` after each encoder update. This function will +modify the `encoder_t` structure to reflect the radius difference. + +Correction factor should be set using `encoder_corrector_set_correction` with +a fixed point f8.24 format. + +API +=== + +.. include:: encoder.exd diff --git a/digital/avr/modules/motor/encoder/encoder_corrector.c b/digital/avr/modules/motor/encoder/encoder_corrector.c new file mode 100644 index 00000000..50e11cd9 --- /dev/null +++ b/digital/avr/modules/motor/encoder/encoder_corrector.c @@ -0,0 +1,55 @@ +/* encoder_corrector.c */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 "encoder_corrector.h" + +#include "modules/math/fixed/fixed.h" + +/* + * The encoder corrector apply a correction factor to an encoder output to + * take into account a difference in wheel size. This should be used on the + * right encoder for example for a two wheeled robot. + */ + +void +encoder_corrector_init (encoder_corrector_t *corrector) +{ + /* Default value: no correction. */ + corrector->correction = 1L << 24; +} + +void +encoder_corrector_update (encoder_corrector_t *corrector, encoder_t *encoder) +{ + /* First cancel encoder update work. */ + encoder->cur -= encoder->diff; + /* Update our own raw encoder position and apply factor. */ + corrector->cur_raw += encoder->diff; + uint16_t new = fixed_mul_f824 (corrector->cur_raw, corrector->correction); + /* Now patch encoder state. */ + encoder->diff = (int16_t) (new - encoder->cur); + encoder->cur = new; +} + diff --git a/digital/avr/modules/motor/encoder/encoder_corrector.h b/digital/avr/modules/motor/encoder/encoder_corrector.h new file mode 100644 index 00000000..ebe4e3aa --- /dev/null +++ b/digital/avr/modules/motor/encoder/encoder_corrector.h @@ -0,0 +1,55 @@ +#ifndef encoder_corrector_h +#define encoder_corrector_h +/* encoder_corrector.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 "encoder.h" + +/** Encoder corrector state. */ +struct encoder_corrector_t +{ + /** Current uncorrected value. */ + int32_t cur_raw; + /** Correction factor (f8.24). */ + uint32_t correction; +}; +typedef struct encoder_corrector_t encoder_corrector_t; + +/** Set correction factor (f8.24). */ +extern inline void +encoder_corrector_set_correction (encoder_corrector_t *corrector, + uint32_t correction) +{ + corrector->correction = correction; +} + +/** Initialise corrector. */ +void +encoder_corrector_init (encoder_corrector_t *corrector); + +/** Update correction on a single encoder. Encoder state will be changed. */ +void +encoder_corrector_update (encoder_corrector_t *corrector, encoder_t *encoder); + +#endif /* encoder_corrector_h */ diff --git a/digital/avr/modules/motor/encoder/encoder_host.h b/digital/avr/modules/motor/encoder/encoder_host.h new file mode 100644 index 00000000..eccefa1a --- /dev/null +++ b/digital/avr/modules/motor/encoder/encoder_host.h @@ -0,0 +1,37 @@ +#ifndef encoder_host_h +#define encoder_host_h +/* encoder_host.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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. + * + * }}} */ + +void +encoder_host_init (uint8_t index, encoder_t *encoder); + +void +encoder_host_update_step (void); + +void +encoder_host_update (void); + +#endif /* encoder_host_h */ diff --git a/digital/avr/modules/motor/encoder/encoder_host.host.c b/digital/avr/modules/motor/encoder/encoder_host.host.c new file mode 100644 index 00000000..5557c61b --- /dev/null +++ b/digital/avr/modules/motor/encoder/encoder_host.host.c @@ -0,0 +1,42 @@ +/* encoder_host.host.c - Host stub. */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 "encoder.h" + +void +encoder_host_init (uint8_t index, encoder_t *encoder) +{ +} + +void +encoder_host_update_step (void) +{ +} + +void +encoder_host_update (void) +{ +} + diff --git a/digital/avr/modules/motor/encoder/ext/Makefile.module b/digital/avr/modules/motor/encoder/ext/Makefile.module new file mode 100644 index 00000000..b4f7da50 --- /dev/null +++ b/digital/avr/modules/motor/encoder/ext/Makefile.module @@ -0,0 +1 @@ +motor_encoder_ext_SOURCES = encoder_ext.avr.c diff --git a/digital/avr/modules/motor/encoder/ext/avrconfig.h b/digital/avr/modules/motor/encoder/ext/avrconfig.h new file mode 100644 index 00000000..76b23a0b --- /dev/null +++ b/digital/avr/modules/motor/encoder/ext/avrconfig.h @@ -0,0 +1,48 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - motor/encoder/ext configuration template. */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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/encoder/ext - External encoder module. */ +/** Number of encoders. */ +#define AC_ENCODER_EXT_NB 4 +/** Use external memory hardware interface. */ +#define AC_ENCODER_EXT_USE_XMEM 0 +/** If not using XMEM, address/data bus, see io_bus.h. */ +#define AC_ENCODER_EXT_AD_BUS A, 8, 0 +/** If not using XMEM, address latch enable IO. */ +#define AC_ENCODER_EXT_ALE_IO B, 0 +/** If not using XMEM, read enable IO, valid low. */ +#define AC_ENCODER_EXT_RD_IO B, 1 +/** If not using XMEM and write available, write enable IO, valid low. */ +#define AC_ENCODER_EXT_WR_IO B, 2 +/** Reverse flag for each encoder (1 to reverse direction). */ +#define AC_ENCODER_EXT_REVERSE 0, 0, 0, 0 +/** Right shift for all encoders to lower resolution. */ +#define AC_ENCODER_EXT_SHIFT 0 +/** For debug purpose only! */ +#define AC_ENCODER_EXT_EXPORT_READ 0 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/motor/encoder/ext/encoder_ext.avr.c b/digital/avr/modules/motor/encoder/ext/encoder_ext.avr.c new file mode 100644 index 00000000..39126b54 --- /dev/null +++ b/digital/avr/modules/motor/encoder/ext/encoder_ext.avr.c @@ -0,0 +1,190 @@ +/* encoder_ext.c */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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/motor/encoder/encoder.h" +#include "modules/utils/utils.h" +#include "encoder_ext.h" + +#include "io_bus.h" + +/** + * This file add support for an external counter like the hdlcounter or + * avrcounter project. They are connected using the external memory + * interface using the dedicated external memory hardware or classic IO. + */ + +#if AC_ENCODER_EXT_USE_XMEM +# if (defined (AC_ENCODER_EXT_AD_BUS) \ + || defined (AC_ENCODER_EXT_ALE_IO) \ + || defined (AC_ENCODER_EXT_RD_IO) \ + || defined (AC_ENCODER_EXT_WR_IO)) +# error "motor/encoder/ext: when using XMEM, io definition is not used" +# endif +# if defined (__AVR_ATmega64__) +# elif defined (__AVR_ATmega128__) +# else +# error "motor/encoder/ext: XMEM not tested on this chip" +# endif +#endif + +/** External encoder state. */ +struct encoder_ext_t +{ + /** Base encoder. */ + encoder_t *encoder; + /** Encoder value at previous update. */ + uint16_t old; + /** Encoder value accumulated during update steps. */ + uint16_t step_acc; + /** Encoder value at previous update step. */ + uint8_t step_old; +}; +typedef struct encoder_ext_t encoder_ext_t; + +/** Global encoder states. */ +encoder_ext_t encoder_ext[AC_ENCODER_EXT_NB]; + +/** Read an external encoder. */ +#if !AC_ENCODER_EXT_EXPORT_READ +static inline +#endif +uint8_t +encoder_ext_read (uint8_t n) +{ +#if AC_ENCODER_EXT_USE_XMEM + uint8_t * const ext = (void *) (RAMEND + 1); + return ext[n]; +#else + uint8_t v; + IO_BUS_SET (AC_ENCODER_EXT_AD_BUS, n); + IO_CLR (AC_ENCODER_EXT_ALE_IO); + IO_BUS_SET (AC_ENCODER_EXT_AD_BUS, 0); + IO_BUS_INPUT (AC_ENCODER_EXT_AD_BUS); + IO_CLR (AC_ENCODER_EXT_RD_IO); + utils_nop (); + utils_nop (); + v = IO_BUS_GET (AC_ENCODER_EXT_AD_BUS); + IO_SET (AC_ENCODER_EXT_RD_IO); + IO_SET (AC_ENCODER_EXT_ALE_IO); + IO_BUS_OUTPUT (AC_ENCODER_EXT_AD_BUS); + return v; +#endif +} + +/** Initialize encoder bus, to be done once. */ +static void +encoder_ext_init_bus (void) +{ + static uint8_t inited; + if (!inited) + { + /* Setup XMEM or regular IO bus. */ +#if AC_ENCODER_EXT_USE_XMEM + /* Long wait-states. */ + XMCRA = _BV (SRW11); + /* Do not use port C for address. */ + XMCRB = _BV (XMM2) | _BV (XMM1) | _BV (XMM0); + /* Long wait-states and enable. */ + MCUCR |= _BV (SRE) | _BV (SRW10); +#else + IO_SET (AC_ENCODER_EXT_ALE_IO); + IO_SET (AC_ENCODER_EXT_RD_IO); +#ifdef AC_ENCODER_EXT_WR_IO + IO_SET (AC_ENCODER_EXT_WR_IO); +#endif + IO_OUTPUT (AC_ENCODER_EXT_ALE_IO); + IO_OUTPUT (AC_ENCODER_EXT_RD_IO); +#ifdef AC_ENCODER_EXT_WR_IO + IO_OUTPUT (AC_ENCODER_EXT_WR_IO); +#endif + IO_BUS_SET (AC_ENCODER_EXT_AD_BUS, 0); + IO_BUS_OUTPUT (AC_ENCODER_EXT_AD_BUS); +#endif + /* Done. */ + inited = 1; + } +} + +void +encoder_ext_init (uint8_t index, encoder_t *encoder) +{ + /* Need a working bus. */ + encoder_ext_init_bus (); + /* Keep encoder structure for future usage. */ + encoder_ext[index].encoder = encoder; + /* Begin with safe values. */ + encoder_ext[index].step_old = encoder_ext_read (index); +} + +void +encoder_ext_update_step (void) +{ + uint8_t i; + uint8_t step_now[AC_ENCODER_EXT_NB]; + int8_t diff; + /* Sample encoders. */ + for (i = 0; i < AC_ENCODER_EXT_NB; i++) + step_now[i] = encoder_ext_read (i); + /* Update step. */ + for (i = 0; i < AC_ENCODER_EXT_NB; i++) + { + diff = (int8_t) (step_now[i] - encoder_ext[i].step_old); + encoder_ext[i].step_old = step_now[i]; + encoder_ext[i].step_acc += diff; + } +} + +void +encoder_ext_update (void) +{ + uint8_t i; + /* Wants fresh data. */ + encoder_ext_update_step (); + static const uint8_t reverse[AC_ENCODER_EXT_NB] = + { AC_ENCODER_EXT_REVERSE }; + /* Update each encoder. */ + for (i = 0; i < AC_ENCODER_EXT_NB; i++) + { + /* About shift: this needs to be done after the subtraction to handle + * input value under/overflow. The subtraction is done modulo 2^16 + * and the result is signed. + * + * However, there is two problems: + * - shifting rounds towards -infinity, so this is not fair, + * - shifting eliminates small differences, so 1 + 1 + 1 + 1 = 0. + * To fix these problems, unused bits must be cleared before the + * subtraction is done. */ + uint16_t now = encoder_ext[i].step_acc; + now &= 0xffff << AC_ENCODER_EXT_SHIFT; + int16_t diff = (int16_t) (now - encoder_ext[i].old) + >> AC_ENCODER_EXT_SHIFT; + if (reverse[i]) + diff = -diff; + encoder_ext[i].encoder->diff = diff; + encoder_ext[i].encoder->cur += diff; + encoder_ext[i].old = now; + } +} + diff --git a/digital/avr/modules/motor/encoder/ext/encoder_ext.h b/digital/avr/modules/motor/encoder/ext/encoder_ext.h new file mode 100644 index 00000000..46b51501 --- /dev/null +++ b/digital/avr/modules/motor/encoder/ext/encoder_ext.h @@ -0,0 +1,45 @@ +#ifndef encoder_ext_h +#define encoder_ext_h +/* encoder_ext.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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. + * + * }}} */ + +/** See encoder_init. */ +void +encoder_ext_init (uint8_t index, encoder_t *encoder); + +/** See encoder_update_step. */ +void +encoder_ext_update_step (void); + +/** See encoder_update. */ +void +encoder_ext_update (void); + +#if AC_ENCODER_EXT_EXPORT_READ +uint8_t +encoder_ext_read (uint8_t n); +#endif + +#endif /* encoder_ext_h */ diff --git a/digital/avr/modules/motor/encoder/test/Makefile b/digital/avr/modules/motor/encoder/test/Makefile new file mode 100644 index 00000000..0d15f5dc --- /dev/null +++ b/digital/avr/modules/motor/encoder/test/Makefile @@ -0,0 +1,15 @@ +BASE = ../../../.. +PROGS = test_encoder_ext +test_encoder_ext_SOURCES = test_encoder.c +MODULES = utils uart proto math/fixed motor/encoder motor/encoder/ext +CONFIGFILE = avrconfig_xmem.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -Os + +TEST_MCU = atmega128 +TEST_CONFIGFILES = avrconfig_xmem.h avrconfig_io.h + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/motor/encoder/test/avrconfig_io.h b/digital/avr/modules/motor/encoder/test/avrconfig_io.h new file mode 100644 index 00000000..5476afdc --- /dev/null +++ b/digital/avr/modules/motor/encoder/test/avrconfig_io.h @@ -0,0 +1,110 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 38400 +/** 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 STDIO +/** 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 + +/* motor/encoder - Encoder module. */ +/** Use external encoder module. */ +#define AC_ENCODER_USE_EXT 1 + +/* motor/encoder/ext - External encoder module. */ +/** Number of encoders. */ +#define AC_ENCODER_EXT_NB 4 +/** Use external memory hardware interface. */ +#define AC_ENCODER_EXT_USE_XMEM 0 +/** If not using XMEM, address/data bus, see io_bus.h. */ +#define AC_ENCODER_EXT_AD_BUS A, 8, 0 +/** If not using XMEM, address latch enable IO. */ +#define AC_ENCODER_EXT_ALE_IO B, 0 +/** If not using XMEM, read enable IO, valid low. */ +#define AC_ENCODER_EXT_RD_IO B, 1 +/** If not using XMEM and write available, write enable IO, valid low. */ +#define AC_ENCODER_EXT_WR_IO B, 2 +/** Reverse flag for each encoder (1 to reverse direction). */ +#define AC_ENCODER_EXT_REVERSE 1, 0, 1, 1 +/** Right shift for all encoders to lower resolution. */ +#define AC_ENCODER_EXT_SHIFT 1 +/** For debug purpose only! */ +#define AC_ENCODER_EXT_EXPORT_READ 1 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/motor/encoder/test/avrconfig_xmem.h b/digital/avr/modules/motor/encoder/test/avrconfig_xmem.h new file mode 100644 index 00000000..e96299fe --- /dev/null +++ b/digital/avr/modules/motor/encoder/test/avrconfig_xmem.h @@ -0,0 +1,102 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 38400 +/** 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 STDIO +/** 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 + +/* motor/encoder - Encoder module. */ +/** Use external encoder module. */ +#define AC_ENCODER_USE_EXT 1 + +/* motor/encoder/ext - External encoder module. */ +/** Number of encoders. */ +#define AC_ENCODER_EXT_NB 4 +/** Use external memory hardware interface. */ +#define AC_ENCODER_EXT_USE_XMEM 1 +/** Reverse flag for each encoder (1 to reverse direction). */ +#define AC_ENCODER_EXT_REVERSE 1, 0, 1, 1 +/** Right shift for all encoders to lower resolution. */ +#define AC_ENCODER_EXT_SHIFT 1 +/** For debug purpose only! */ +#define AC_ENCODER_EXT_EXPORT_READ 1 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/motor/encoder/test/test_encoder.c b/digital/avr/modules/motor/encoder/test/test_encoder.c new file mode 100644 index 00000000..efa4a787 --- /dev/null +++ b/digital/avr/modules/motor/encoder/test/test_encoder.c @@ -0,0 +1,192 @@ +/* test_encoder.c */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 "io.h" +#include "modules/utils/utils.h" +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" + +#include "modules/motor/encoder/encoder.h" +#include "modules/motor/encoder/encoder_corrector.h" + +uint8_t read, read_cpt, read_mode; +uint8_t ind, ind_cpt, ind_init; +uint8_t count, count_cpt; + +encoder_t encoder[AC_ENCODER_EXT_NB]; +encoder_corrector_t encoder_corrector_right; + +#define TIMER_TOP 255 +#define TIMER_STEPS 4 +#define TIMER_STEP ((TIMER_TOP + 1) / TIMER_STEPS) + +void +timer_init (void) +{ +#ifndef HOST + TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, + 0, 0, 0, 0, 0, 1, 1, 0); +#endif +} + +void +timer_wait (void) +{ +#ifndef HOST + uint8_t i; + /* Make small steps with counter updates. */ + for (i = 1; i < TIMER_STEPS; i++) + { + while (TCNT0 < i * TIMER_STEP) + ; + if (count) + encoder_update_step (); + } + /* Wait overflow. */ + while (!(TIFR & _BV (TOV0))) + ; + /* Write 1 to clear. */ + TIFR = _BV (TOV0); +#else + if (count) + encoder_update_step (); +#endif +} + +int +main (void) +{ + uint8_t i; +#ifndef HOST + uint8_t read_old = 0; + uint8_t old_ind = 0; + const int total = 5000; +#endif + timer_init (); + for (i = 0; i < AC_ENCODER_EXT_NB; i++) + encoder_init (i, &encoder[i]); + encoder_corrector_init (&encoder_corrector_right); + uart0_init (); + proto_send0 ('z'); + sei (); + while (1) + { + timer_wait (); + if (count) + { + encoder_update (); + encoder_corrector_update (&encoder_corrector_right, &encoder[1]); + } +#ifndef HOST + if (read && !--read_cpt) + { + uint8_t r0, r1, r2, r3; + r0 = encoder_ext_read (0); + r1 = encoder_ext_read (1); + r2 = encoder_ext_read (2); + r3 = encoder_ext_read (3); + if (read_mode == 0 || (read_mode == 1 && r3 != read_old) + || (read_mode == 2 + && (r0 == 0 || r1 == 0 || r2 == 0 || r3 == 0))) + { + proto_send4b ('r', r0, r1, r2, r3); + read_old = r3; + } + read_cpt = read; + } + if (ind && !--ind_cpt) + { + i = encoder_ext_read (3); + if (!ind_init && i != old_ind) + { + uint8_t eip = old_ind + total; + uint8_t eim = old_ind - total; + proto_send7b ('i', old_ind, i, eip, eim, i - eip, i - eim, + i == eip || i == eim); + } + old_ind = i; + ind_init = 0; + ind_cpt = ind; + } +#endif + if (count && !--count_cpt) + { + proto_send4w ('C', encoder[0].cur, encoder[1].cur, + encoder[2].cur, encoder[3].cur); + count_cpt = count; + } + 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): + /* Reset. */ + utils_reset (); + break; + case c ('r', 1): + /* Output encoders raw value after every read. */ + read_cpt = read = args[0]; + read_mode = 0; + break; + case c ('R', 1): + /* Output encoders raw value only if last encoder changed. */ + read_cpt = read = args[0]; + read_mode = 1; + break; + case c ('Z', 1): + /* Output encoders raw value if any encoder is null. */ + read_cpt = read = args[0]; + read_mode = 2; + break; + case c ('i', 1): + /* Index checking mode. Require counter_index_test. */ + ind_cpt = ind = args[0]; + ind_init = 1; + break; + case c ('C', 1): + /* Regular encoder output, use encoder module code. */ + count_cpt = count = args[0]; + break; + case c ('c', 4): + /* Set correction. */ + encoder_corrector_set_correction (&encoder_corrector_right, + v8_to_v32 (args[0], args[1], + args[2], args[3])); + break; + default: + proto_send0 ('?'); + return; + } + proto_send (cmd, size, args); +#undef c +} -- cgit v1.2.3