From 730ec2b971458a86c7a2e90901159eaf7b57601d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 31 Jan 2008 01:07:51 +0100 Subject: * digital/asserv/src/asserv: - added external counter support. --- digital/asserv/README | 25 ++++ digital/asserv/src/asserv/avrconfig.h | 4 + digital/asserv/src/asserv/counter.avr.c | 173 --------------------------- digital/asserv/src/asserv/counter_ext.avr.c | 95 +++++++++++++++ digital/asserv/src/asserv/counter_tcc.avr.c | 179 ++++++++++++++++++++++++++++ digital/asserv/src/asserv/main.c | 6 +- 6 files changed, 308 insertions(+), 174 deletions(-) create mode 100644 digital/asserv/README delete mode 100644 digital/asserv/src/asserv/counter.avr.c create mode 100644 digital/asserv/src/asserv/counter_ext.avr.c create mode 100644 digital/asserv/src/asserv/counter_tcc.avr.c diff --git a/digital/asserv/README b/digital/asserv/README new file mode 100644 index 00000000..37d78bf0 --- /dev/null +++ b/digital/asserv/README @@ -0,0 +1,25 @@ +asserv - Position & speed motor control on AVR. + +This board use a AVR to control up to 4 motors. It can compute a two-wheeled +robot position by small moves integration. + + +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. diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h index 5d316a25..bd46c273 100644 --- a/digital/asserv/src/asserv/avrconfig.h +++ b/digital/asserv/src/asserv/avrconfig.h @@ -83,4 +83,8 @@ /** Support for quote parameter. */ #define AC_PROTO_QUOTE 1 +/* asserv. */ +/** Use external counters. */ +#define AC_ASSERV_COUNTER_EXTERNAL 1 + #endif /* avrconfig_h */ diff --git a/digital/asserv/src/asserv/counter.avr.c b/digital/asserv/src/asserv/counter.avr.c deleted file mode 100644 index c2b58781..00000000 --- a/digital/asserv/src/asserv/counter.avr.c +++ /dev/null @@ -1,173 +0,0 @@ -/* 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/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c new file mode 100644 index 00000000..c717fdc6 --- /dev/null +++ b/digital/asserv/src/asserv/counter_ext.avr.c @@ -0,0 +1,95 @@ +/* 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. + * + * }}} */ + +/** + * This file add support for an external counter like the hdlcounter or + * avrcounter project. This can be better in order not to loose steps. + */ + +/** Last 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 7 significant bits, sign included. */ +static int16_t counter_left_diff, counter_right_diff; + +#define COUNTER_ALE _BV (2) +#define COUNTER_RD _BV (1) +#define COUNTER_WR _BV (0) + +static inline void +counter_restart (void); + +/** Initialize the counters. */ +static inline void +counter_init (void) +{ + PORTG |= COUNTER_ALE | COUNTER_RD | COUNTER_WR; + DDRG |= COUNTER_ALE | COUNTER_RD | COUNTER_WR; + PORTA = 0; + DDRA = 0xff; + /* Begin with safe values. */ + counter_restart (); +} + +/** Read an external counter. */ +static inline uint8_t +counter_read (uint8_t n) +{ + uint8_t v; + PORTA = n; + PORTG &= ~COUNTER_ALE; + PORTA = 0; + DDRA = 0; + PORTG &= ~COUNTER_RD; + v = PINA; + PORTG |= COUNTER_RD; + PORTG |= COUNTER_ALE; + DDRA = 0xff; + return v; +} + +/** Update overall counter values and compute diffs. */ +static inline void +counter_update (void) +{ + uint8_t left, right; + left = counter_read (0); + counter_left_diff = (int8_t) (left - counter_left_old); + counter_left += counter_left_diff; + right = counter_read (1); + counter_right_diff = (int8_t) (right - counter_right_old); + counter_right += counter_right_diff; +} + +/** Restart counting. */ +static inline void +counter_restart (void) +{ + counter_left_old = counter_read (0); + counter_right_old = counter_read (1); +} + diff --git a/digital/asserv/src/asserv/counter_tcc.avr.c b/digital/asserv/src/asserv/counter_tcc.avr.c new file mode 100644 index 00000000..5ce4058c --- /dev/null +++ b/digital/asserv/src/asserv/counter_tcc.avr.c @@ -0,0 +1,179 @@ +/* counter_tcc.avr.c - Internal counter support. */ +/* 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 add support for an AVR internal counter. This uses one TCC + * hardware per counter, and an external circuitry to compute rotation + * direction. + */ + +/** 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/main.c b/digital/asserv/src/asserv/main.c index 22a282cd..b3caa6ed 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -38,7 +38,11 @@ uint8_t main_sequence, main_sequence_ack, main_sequence_finish; /* This is implementation include. */ #ifndef HOST # include "timer.avr.c" -# include "counter.avr.c" +# if AC_ASSERV_COUNTER_EXTERNAL +# include "counter_ext.avr.c" +# else +# include "counter_tcc.avr.c" +# endif # include "pwm.avr.c" #else # include "simu.host.h" -- cgit v1.2.3