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/eeprom.avr.c | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) (limited to 'digital/asserv/src/asserv/eeprom.avr.c') 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); -- cgit v1.2.3