summaryrefslogtreecommitdiff
path: root/n/asserv/src/counter.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/counter.c')
-rw-r--r--n/asserv/src/counter.c103
1 files changed, 70 insertions, 33 deletions
diff --git a/n/asserv/src/counter.c b/n/asserv/src/counter.c
index a9b5760..cfdf2e6 100644
--- a/n/asserv/src/counter.c
+++ b/n/asserv/src/counter.c
@@ -1,8 +1,12 @@
/* 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
@@ -17,23 +21,40 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
-#include "counter.h"
-#include <n/avr/utils/utils.h>
+
+/** 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. */
+static int16_t counter_left_diff, counter_right_diff;
/* +AutoDec */
-/* -AutoDec */
-/** Forward and reverse counter value. */
-static uint8_t counter_l_frw, counter_l_rev, counter_r_frw, counter_r_rev;
-/** Last TCNT value. */
-static uint8_t counter_l_old, counter_r_old;
+/** 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. */
-inline void
+static inline void
counter_init (void)
{
/* Left counter. */
@@ -47,51 +68,67 @@ counter_init (void)
TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30,
0, 0, 0, 0, 0, 1, 1, 1);
/* Begin with safe values. */
- counter_reset ();
+ counter_restart ();
}
-/* Update counter, should be called as often as possible. */
-inline void
-counter_update (void)
+/** Poll counters, should be called as often as possible. */
+static inline void
+counter_poll (void)
{
uint8_t c;
- /* Update left counter. */
+ /* Read left counter. */
c = TCNT2;
if (PINE & _BV (4))
{
PORTD &= ~0x40;
- counter_l_frw += c - counter_l_old;
+ counter_left_frw += c - counter_left_old;
}
else
{
PORTD |= 0x40;
- counter_l_rev += c - counter_l_old;
+ counter_left_rev += c - counter_left_old;
}
- counter_l_old = c;
- /* Update right counter. */
+ counter_left_old = c;
+ /* Read right counter. */
c = TCNT3L;
if (PINE & _BV (5))
{
PORTD &= ~0x20;
- counter_r_frw += c - counter_r_old;
+ counter_right_frw += c - counter_right_old;
}
else
{
PORTD |= 0x20;
- counter_r_rev += c - counter_r_old;
+ counter_right_rev += c - counter_right_old;
}
- counter_r_old = c;
+ 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;
}
-/* Reset the counters. */
-inline void
-counter_reset (void)
+/** Restart counting. */
+static inline void
+counter_restart (void)
{
- counter_l_frw = 0;
- counter_l_rev = 0;
- counter_l_old = TCNT2;
- counter_r_frw = 0;
- counter_r_rev = 0;
- counter_r_old = TCNT3L;
+ counter_left_frw = 0;
+ counter_left_rev = 0;
+ counter_left_old = TCNT2;
+ counter_right_frw = 0;
+ counter_right_rev = 0;
+ counter_right_old = TCNT3L;
}