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 --- .../modules/motor/encoder/ext/encoder_ext.avr.c | 190 +++++++++++++++++++++ 1 file changed, 190 insertions(+) create mode 100644 digital/avr/modules/motor/encoder/ext/encoder_ext.avr.c (limited to 'digital/avr/modules/motor/encoder/ext/encoder_ext.avr.c') 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; + } +} + -- cgit v1.2.3