From 5361663b43859260d59dbddfa28212b1e7597936 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 4 May 2010 02:25:30 +0200 Subject: digital/asserv/src/asserv: poll counter more often, add shift, closes #109 --- digital/asserv/src/asserv/counter.h | 3 ++ digital/asserv/src/asserv/counter_ext.avr.c | 74 ++++++++++++++++++++++------- digital/asserv/src/asserv/counter_tcc.avr.c | 6 +++ digital/asserv/src/asserv/timer.avr.c | 18 +++++++ 4 files changed, 85 insertions(+), 16 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/counter.h b/digital/asserv/src/asserv/counter.h index 45b7392f..198660e4 100644 --- a/digital/asserv/src/asserv/counter.h +++ b/digital/asserv/src/asserv/counter.h @@ -34,6 +34,9 @@ extern int16_t counter_left_diff, counter_right_diff, void counter_init (void); +void +counter_update_step (void); + void counter_update (void); diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c index 61ee6911..68bb0584 100644 --- a/digital/asserv/src/asserv/counter_ext.avr.c +++ b/digital/asserv/src/asserv/counter_ext.avr.c @@ -55,13 +55,28 @@ /** Define to 1 to reverse the second auxiliary counter. */ #define COUNTER_AUX1_REVERSE 0 +/** Left counter shift. */ +#define COUNTER_LEFT_SHIFT 0 +/** Right counter shift. */ +#define COUNTER_RIGHT_SHIFT 0 +/** First auxiliary counter shift. */ +#define COUNTER_AUX0_SHIFT 0 +/** Second auxiliary counter shift. */ +#define COUNTER_AUX1_SHIFT 0 + /** Define to 1 to use the AVR External Memory system, or 0 to use hand made * signals. */ #define COUNTER_USE_XMEM 1 /** Last values. */ -static uint8_t counter_left_old, counter_right_old, - counter_aux_old[AC_ASSERV_AUX_NB]; +static uint16_t counter_left_old, counter_right_old, + counter_aux_old[AC_ASSERV_AUX_NB]; +/** New values, being updated by step update. */ +static uint16_t counter_left_new_step, counter_right_new_step, + counter_aux_new_step[AC_ASSERV_AUX_NB]; +/** Last raw step values */ +static uint8_t counter_left_old_step, counter_right_old_step, + counter_aux_old_step[AC_ASSERV_AUX_NB]; /** Overall counter values. */ uint16_t counter_left, counter_right, counter_aux[AC_ASSERV_AUX_NB]; @@ -122,35 +137,60 @@ counter_init (void) DDRA = 0xff; #endif /* Begin with safe values. */ - counter_left_old = counter_read (COUNTER_LEFT); - counter_right_old = counter_read (COUNTER_RIGHT); - counter_aux_old[0] = counter_read (COUNTER_AUX0); - counter_aux_old[1] = counter_read (COUNTER_AUX1); + counter_left_old_step = counter_read (COUNTER_LEFT); + counter_right_old_step = counter_read (COUNTER_RIGHT); + counter_aux_old_step[0] = counter_read (COUNTER_AUX0); + counter_aux_old_step[1] = counter_read (COUNTER_AUX1); } -/** Update overall counter values and compute diffs. */ +/** Update one step. If counters are not read fast enough, they could + * overflow, call this function often to update step counters. */ void -counter_update (void) +counter_update_step (void) { uint8_t left, right, aux0, aux1; + int8_t diff; /* Sample counters. */ left = counter_read (COUNTER_LEFT); right = counter_read (COUNTER_RIGHT); aux0 = counter_read (COUNTER_AUX0); aux1 = counter_read (COUNTER_AUX1); + /* 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; + diff = (int8_t) (aux0 - counter_aux_old_step[0]); + counter_aux_old_step[0] = aux0; + counter_aux_new_step[0] += diff; + diff = (int8_t) (aux1 - counter_aux_old_step[1]); + counter_aux_old_step[1] = aux1; + counter_aux_new_step[1] += 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 >> COUNTER_LEFT_SHIFT; #if !COUNTER_LEFT_REVERSE - counter_left_diff = (int8_t) (left - counter_left_old); + counter_left_diff = (int16_t) (left - counter_left_old); #else - counter_left_diff = (int8_t) (counter_left_old - left); + counter_left_diff = (int16_t) (counter_left_old - left); #endif counter_left_old = left; counter_left += counter_left_diff; /* Right counter. */ + uint16_t right = counter_right_new_step >> COUNTER_RIGHT_SHIFT; #if !COUNTER_RIGHT_REVERSE - counter_right_diff = (int8_t) (right - counter_right_old); + counter_right_diff = (int16_t) (right - counter_right_old); #else - counter_right_diff = (int8_t) (counter_right_old - right); + counter_right_diff = (int16_t) (counter_right_old - right); #endif counter_right_old = right; /* Fix right counter. */ @@ -160,18 +200,20 @@ counter_update (void) counter_right_diff = (int16_t) (right_new - counter_right); counter_right = right_new; /* First auxiliary counter. */ + uint16_t aux0 = counter_aux_new_step[0] >> COUNTER_AUX0_SHIFT; #if !COUNTER_AUX0_REVERSE - counter_aux_diff[0] = (int8_t) (aux0 - counter_aux_old[0]); + counter_aux_diff[0] = (int16_t) (aux0 - counter_aux_old[0]); #else - counter_aux_diff[0] = (int8_t) (counter_aux_old[0] - aux0); + counter_aux_diff[0] = (int16_t) (counter_aux_old[0] - aux0); #endif counter_aux_old[0] = aux0; counter_aux[0] += counter_aux_diff[0]; /* Second auxiliary counter. */ + uint16_t aux1 = counter_aux_new_step[1] >> COUNTER_AUX1_SHIFT; #if !COUNTER_AUX1_REVERSE - counter_aux_diff[1] = (int8_t) (aux1 - counter_aux_old[1]); + counter_aux_diff[1] = (int16_t) (aux1 - counter_aux_old[1]); #else - counter_aux_diff[1] = (int8_t) (counter_aux_old[1] - aux1); + counter_aux_diff[1] = (int16_t) (counter_aux_old[1] - aux1); #endif counter_aux_old[1] = aux1; counter_aux[1] += counter_aux_diff[1]; diff --git a/digital/asserv/src/asserv/counter_tcc.avr.c b/digital/asserv/src/asserv/counter_tcc.avr.c index a4bd04ec..419e764f 100644 --- a/digital/asserv/src/asserv/counter_tcc.avr.c +++ b/digital/asserv/src/asserv/counter_tcc.avr.c @@ -115,6 +115,12 @@ SIGNAL (SIG_INTERRUPT5) counter_right_old = c; } +void +counter_update_step (void) +{ + /* No support for update steps. */ +} + /** Update overall counter values and compute diffs. */ void counter_update (void) diff --git a/digital/asserv/src/asserv/timer.avr.c b/digital/asserv/src/asserv/timer.avr.c index 2aaecc71..eec4414a 100644 --- a/digital/asserv/src/asserv/timer.avr.c +++ b/digital/asserv/src/asserv/timer.avr.c @@ -27,6 +27,15 @@ #include "modules/utils/utils.h" #include "io.h" +#include "counter.h" + +/** Top timer value. */ +#define TIMER_TOP 255 +/** Number of steps during wait. */ +#define TIMER_STEPS 4 +/** Size of step. */ +#define TIMER_STEP ((TIMER_TOP + 1) / TIMER_STEPS) + /** Initialise the timer. */ void timer_init (void) @@ -43,6 +52,15 @@ timer_init (void) void timer_wait (void) { + uint8_t i; + /* Make small steps with counter updates. */ + for (i = 1; i < TIMER_STEPS; i++) + { + while (TCNT0 < i * TIMER_STEP) + ; + counter_update_step (); + } + /* Wait overflow. */ while (!(TIFR & _BV (TOV0))) ; /* Write 1 to clear. */ -- cgit v1.2.3