/* counter_ext.avr.c - External counter support. */ /* asserv - Position & speed motor control on AVR. {{{ * * Copyright (C) 2008 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 "counter.h" #include "modules/utils/utils.h" #include "modules/math/fixed/fixed.h" #include "io.h" /** * This file add support for an external counter like the hdlcounter or * avrcounter project. This can be better in order not to loose steps and * support more counters. * * There is additionnal support for error correction on the right counter. */ /** Define the left counter address. */ #define COUNTER_LEFT 0 /** Define the right counter address. */ #define COUNTER_RIGHT 1 /** Define to 1 to reverse the left counter. */ #define COUNTER_LEFT_REVERSE 0 /** Define to 1 to reverse the right counter. */ #define COUNTER_RIGHT_REVERSE 0 /** Left counter shift. */ #define COUNTER_LEFT_SHIFT 0 /** Right counter shift. */ #define COUNTER_RIGHT_SHIFT 0 /** Define to 1 to use the AVR External Memory system, or 0 to use hand made * signals. */ #define COUNTER_USE_XMEM 0 /** Last values. */ static uint16_t counter_left_old, counter_right_old; /** New values, being updated by step update. */ static int16_t counter_left_new_step, counter_right_new_step; /** Last raw step values */ static uint8_t counter_left_old_step, counter_right_old_step; /** Overall counter values. */ uint16_t counter_left, counter_right; /** Overall uncorrected counter values. */ static int32_t counter_right_raw; /** Correction factor (f8.24). */ uint32_t counter_right_correction = 1L << 24; /** Counter differences since last update. * Maximum of 9 significant bits, sign included. */ int16_t counter_left_diff, counter_right_diff; #if !COUNTER_USE_XMEM # define COUNTER_ALE_IO B, 4 # define COUNTER_RD_IO D, 6 #endif /** Read an external counter. */ static inline uint8_t counter_read (uint8_t n) { #if COUNTER_USE_XMEM uint8_t * const ext = (void *) 0x1100; return ext[n]; #else uint8_t v; PORTA = (PORTA & 0xf0) | (n & 0x0f); IO_CLR (COUNTER_ALE_IO); PORTA &= 0xf0; DDRA &= 0xf0; DDRB &= 0xf0; IO_CLR (COUNTER_RD_IO); utils_nop (); utils_nop (); v = (PINA & 0x0f) | (PINB & 0x0f) << 4; IO_SET (COUNTER_RD_IO); IO_SET (COUNTER_ALE_IO); DDRA |= 0x0f; DDRB |= 0x0f; return v; #endif } /** Initialize the counters. */ void counter_init (void) { #if COUNTER_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 (COUNTER_ALE_IO); IO_SET (COUNTER_RD_IO); IO_OUTPUT (COUNTER_ALE_IO); IO_OUTPUT (COUNTER_RD_IO); PORTA &= 0xf0; PORTB &= 0xf0; DDRA |= 0x0f; DDRB |= 0x0f; #endif /* Begin with safe values. */ counter_left_old_step = counter_read (COUNTER_LEFT); counter_right_old_step = counter_read (COUNTER_RIGHT); } /** Update one step. If counters are not read fast enough, they could * overflow, call this function often to update step counters. */ void counter_update_step (void) { uint8_t left, right; int8_t diff; /* Sample counters. */ left = counter_read (COUNTER_LEFT); right = counter_read (COUNTER_RIGHT); /* Update step counters. */ diff = (int8_t) (left - counter_left_old_step); counter_left_old_step = left; counter_left_new_step += diff; diff = (int8_t) (right - counter_right_old_step); counter_right_old_step = right; counter_right_new_step += diff; } /** Update overall counter values and compute diffs. */ void counter_update (void) { /* Wants fresh data. */ counter_update_step (); /* Left counter. */ uint16_t left = counter_left_new_step; left &= 0xffff << COUNTER_LEFT_SHIFT; /* Reset unused bits. */ #if !COUNTER_LEFT_REVERSE counter_left_diff = (int16_t) (left - counter_left_old); #else counter_left_diff = (int16_t) (counter_left_old - left); #endif counter_left_diff >>= COUNTER_LEFT_SHIFT; counter_left_old = left; counter_left += counter_left_diff; /* Right counter. */ uint16_t right = counter_right_new_step; right &= 0xffff << COUNTER_RIGHT_SHIFT; /* Reset unused bits. */ #if !COUNTER_RIGHT_REVERSE counter_right_diff = (int16_t) (right - counter_right_old); #else counter_right_diff = (int16_t) (counter_right_old - right); #endif counter_right_diff >>= COUNTER_RIGHT_SHIFT; counter_right_old = right; /* Fix right counter. */ counter_right_raw += counter_right_diff; uint16_t right_new = fixed_mul_f824 (counter_right_raw, counter_right_correction); counter_right_diff = (int16_t) (right_new - counter_right); counter_right = right_new; }