From 853fb5b5d899eb55f1f7d4044bff0a8d5d84b694 Mon Sep 17 00:00:00 2001 From: prot Date: Thu, 9 Dec 2004 23:57:42 +0000 Subject: Importing files from n/asserv --- n/line-follower/src/counter.c | 135 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 135 insertions(+) create mode 100644 n/line-follower/src/counter.c (limited to 'n/line-follower/src/counter.c') diff --git a/n/line-follower/src/counter.c b/n/line-follower/src/counter.c new file mode 100644 index 0000000..46dc05a --- /dev/null +++ b/n/line-follower/src/counter.c @@ -0,0 +1,135 @@ +/* counter.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 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. + * + * }}} */ + +/** 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); + +/** Poll counters, should be called as often as possible. */ +static inline void +counter_poll (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 (); +} + +/** Poll counters, should be called as often as possible. */ +static inline void +counter_poll (void) +{ + uint8_t c; + /* Read left counter. */ + c = TCNT2; + if (PINE & _BV (4)) + { + PORTD &= ~0x40; + counter_left_frw += c - counter_left_old; + } + else + { + PORTD |= 0x40; + counter_left_rev += c - counter_left_old; + } + counter_left_old = c; + /* Read right counter. */ + c = TCNT3L; + if (PINE & _BV (5)) + { + PORTD &= ~0x20; + counter_right_frw += c - counter_right_old; + } + else + { + PORTD |= 0x20; + counter_right_rev += c - counter_right_old; + } + counter_right_old = c; +} + +/** Update overall counter values and compute diffs. */ +static inline void +counter_update (void) +{ + counter_left_diff = counter_left_frw; + counter_left_frw = 0; + counter_left_diff -= counter_left_rev; + counter_left_rev = 0; + counter_left += counter_left_diff; + counter_right_diff = counter_right_frw; + counter_right_frw = 0; + counter_right_diff -= counter_right_rev; + counter_right_rev = 0; + counter_right += counter_right_diff; +} + +/** 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; +} + -- cgit v1.2.3