From 0db1380594157e8b15c62b46bdd75ebd010dc9dc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 17 Apr 2008 00:32:45 +0200 Subject: * digital/asserv/src/asserv: - added counter correction to handle wheels with different size. --- digital/asserv/src/asserv/counter.h | 1 + digital/asserv/src/asserv/counter_ext.avr.c | 16 ++++++++++++++-- digital/asserv/src/asserv/eeprom.avr.c | 25 +++++++++++++++++++++++++ digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 7 +++++++ digital/asserv/src/asserv/simu.host.c | 13 +++++++++++-- 6 files changed, 59 insertions(+), 5 deletions(-) diff --git a/digital/asserv/src/asserv/counter.h b/digital/asserv/src/asserv/counter.h index 2e17dbfb..c5eae18b 100644 --- a/digital/asserv/src/asserv/counter.h +++ b/digital/asserv/src/asserv/counter.h @@ -26,6 +26,7 @@ * }}} */ extern uint16_t counter_left, counter_right, counter_aux0; +extern uint32_t counter_right_correction; extern int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; void diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c index f0d1622d..4f8cad3b 100644 --- a/digital/asserv/src/asserv/counter_ext.avr.c +++ b/digital/asserv/src/asserv/counter_ext.avr.c @@ -26,12 +26,15 @@ #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. */ @@ -56,8 +59,12 @@ static uint8_t counter_left_old, counter_right_old, counter_aux0_old; /** Overall counter values. */ uint16_t counter_left, counter_right, counter_aux0; +/** 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 7 significant bits, sign included. */ + * Maximum of 9 significant bits, sign included. */ int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; #if !COUNTER_USE_XMEM @@ -137,7 +144,12 @@ counter_update (void) counter_right_diff = (int8_t) (counter_right_old - right); #endif counter_right_old = right; - counter_right += counter_right_diff; + /* 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; /* First auxiliary counter. */ #if !COUNTER_AUX0_REVERSE counter_aux0_diff = (int8_t) (aux0 - counter_aux0_old); diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 9f4920a4..4277ceac 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -25,8 +25,11 @@ #include "common.h" #include "eeprom.h" +#include "modules/utils/byte.h" + #include +#include "counter.h" #include "pwm.h" #include "pos.h" #include "speed.h" @@ -40,6 +43,26 @@ * your new format is not compatible with the old one or you will load * garbages in parameters. */ +static uint32_t +eeprom_read_dword (uint8_t *addr) +{ + uint8_t dw[4]; + dw[0] = eeprom_read_byte (addr++); + dw[1] = eeprom_read_byte (addr++); + dw[2] = eeprom_read_byte (addr++); + dw[3] = eeprom_read_byte (addr++); + return v8_to_v32 (dw[3], dw[2], dw[1], dw[0]); +} + +static void +eeprom_write_dword (uint8_t *addr, uint32_t dw) +{ + eeprom_write_byte (addr++, v32_to_v8 (dw, 0)); + eeprom_write_byte (addr++, v32_to_v8 (dw, 1)); + eeprom_write_byte (addr++, v32_to_v8 (dw, 2)); + eeprom_write_byte (addr++, v32_to_v8 (dw, 3)); +} + /* Read parameters from eeprom. */ void eeprom_read_params (void) @@ -55,6 +78,7 @@ eeprom_read_params (void) speed_alpha.slow = eeprom_read_byte (p8++); speed_aux0.slow = eeprom_read_byte (p8++); pwm_set_reverse (eeprom_read_byte (p8++)); + counter_right_correction = eeprom_read_dword (p8); p8 += 4; p16 = (uint16_t *) p8; postrack_set_footing (eeprom_read_word (p16++)); speed_theta.acc = eeprom_read_word (p16++); @@ -89,6 +113,7 @@ eeprom_write_params (void) eeprom_write_byte (p8++, speed_alpha.slow); eeprom_write_byte (p8++, speed_aux0.slow); eeprom_write_byte (p8++, pwm_reverse); + eeprom_write_dword (p8, counter_right_correction); p8 += 4; p16 = (uint16_t *) p8; eeprom_write_word (p16++, postrack_footing); eeprom_write_word (p16++, speed_theta.acc); diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index 2f3d5c74..c3a57075 100644 --- a/digital/asserv/src/asserv/eeprom.h +++ b/digital/asserv/src/asserv/eeprom.h @@ -26,7 +26,7 @@ * }}} */ /** Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x47 +#define EEPROM_KEY 0x48 void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 1e242a1d..5913c107 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -441,6 +441,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - d: angle. */ postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]); break; + case c ('c', 5): + /* Set right counter correction factor. + * - d: factor (f8.24). */ + counter_right_correction = v8_to_v32 (args[1], args[2], + args[3], args[4]); + break; case c ('f', 3): /* Set footing. * - w: footing. */ @@ -525,6 +531,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('P', 1): /* Print current settings. */ proto_send1b ('E', EEPROM_KEY); + proto_send1d ('c', counter_right_correction); proto_send1w ('f', postrack_footing); proto_send2w ('a', speed_theta.acc, speed_alpha.acc); proto_send4b ('s', speed_theta.max, speed_alpha.max, diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 803142dd..8b499123 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -28,6 +28,7 @@ #include "modules/host/host.h" #include "modules/host/mex.h" #include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" #include #include @@ -45,8 +46,12 @@ uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; /** Overall counter values. */ uint16_t counter_left, counter_right, counter_aux0; /** Counter differences since last update. - * Maximum of 7 significant bits, sign included. */ + * Maximum of 9 significant bits, sign included. */ int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; +/** Overall uncorrected counter values. */ +static int32_t counter_right_raw; +/** Correction factor (f8.24). */ +uint32_t counter_right_correction = 1L << 24; /** PWM values, this is an error if absolute value is greater than the * maximum. */ @@ -207,7 +212,11 @@ simu_step (void) counter_left += counter_left_diff; simu_counter_left = counter_left_new; counter_right_diff = counter_right_new - simu_counter_right; - counter_right += counter_right_diff; + 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; simu_counter_right = counter_right_new; /* Update auxiliary counter. */ if (simu_robot->aux0_motor) -- cgit v1.2.3