From 203401d8e656aaf704953288ed5efcd5ca60ffe4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 19 May 2009 08:52:25 +0200 Subject: digital/asserv, digital/mimot: use new eeprom code --- digital/mimot/src/dirty/Makefile | 4 + digital/mimot/src/dirty/eeprom.avr.c | 212 ++++++++++++++++++++---------- digital/mimot/src/dirty/eeprom.h | 4 +- digital/mimot/src/dirty/eeprom.mak | 9 ++ digital/mimot/src/dirty/main.c | 4 +- digital/mimot/src/dirty/simu.host.c | 3 + digital/mimot/tools/gen_eeprom_default.py | 99 ++++++++++++++ 7 files changed, 266 insertions(+), 69 deletions(-) create mode 100644 digital/mimot/src/dirty/eeprom.mak create mode 100644 digital/mimot/tools/gen_eeprom_default.py (limited to 'digital/mimot') diff --git a/digital/mimot/src/dirty/Makefile b/digital/mimot/src/dirty/Makefile index 55c3e894..e6788902 100644 --- a/digital/mimot/src/dirty/Makefile +++ b/digital/mimot/src/dirty/Makefile @@ -16,4 +16,8 @@ AVR_MCU = atmega32 OPTIMIZE = -Os HOST_LIBS = -lm +AVR_DEFS = $(EEPROM_DEFAULTS:%=-DEEPROM_DEFAULTS=\"%\") + include $(BASE)/make/Makefile.gen + +include eeprom.mak diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c index 409ab190..faf2c282 100644 --- a/digital/mimot/src/dirty/eeprom.avr.c +++ b/digital/mimot/src/dirty/eeprom.avr.c @@ -25,97 +25,177 @@ #include "common.h" #include "eeprom.h" -#include "modules/utils/byte.h" +#include "modules/utils/utils.h" +#include "modules/utils/crc.h" #include +#include #include "cs.h" -#define EEPROM_START 0 +#define EEPROM_INDEX_NB AC_ASSERV_AUX_NB /* WARNING: * If you change EEPROM format, be sure to change the EEPROM_KEY in header if - * your new format is not compatible with the old one or you will load + * your new format is not compatible with the old one or you may load * garbages in parameters. */ +struct eeprom_t +{ + /** Identify parameters version. */ + uint8_t key; + /** Saved parameters. */ + struct { + uint8_t max; + uint8_t slow; + uint16_t acc; + } speed[EEPROM_INDEX_NB]; + struct { + uint16_t kp; + uint16_t ki; + uint16_t kd; + uint16_t e_sat; + uint16_t i_sat; + uint16_t d_sat; + } pos[EEPROM_INDEX_NB]; + struct { + uint16_t error_limit; + uint16_t speed_limit; + uint8_t counter_limit; + } bd[EEPROM_INDEX_NB]; + struct { + uint8_t reverse; + } output[EEPROM_INDEX_NB]; + /** CRC of the whole structure. */ + uint8_t crc; +}; + +/* If EEPROM_DEFAULTS is defined, it should be the name of a file to include + * which defines the structure eeprom_defaults in PROGMEM and + * EEPROM_DEFAULTS_KEY which should match EEPROM_KEY. Those defaults values + * are used when no EEPROM set is good. */ +#ifdef EEPROM_DEFAULTS +# include EEPROM_DEFAULTS +# if EEPROM_DEFAULTS_KEY != EEPROM_KEY +# error "EEPROM defaults are not compatible (key mismatch)" +# endif +#endif + +EEMEM struct eeprom_t eeprom_params[4]; + +/** Index of loaded eeprom block. */ +int8_t eeprom_loaded; + +static void +eeprom_read_params_helper (struct eeprom_t *loaded, uint8_t index, + speed_control_t *speed, pos_control_t *pos, + blocking_detection_t *bd, output_t *output) +{ + speed->max = loaded->speed[index].max; + speed->slow = loaded->speed[index].slow; + speed->acc = loaded->speed[index].acc; + pos->kp = loaded->pos[index].kp; + pos->ki = loaded->pos[index].ki; + pos->kd = loaded->pos[index].kd; + pos->e_sat = loaded->pos[index].e_sat; + pos->i_sat = loaded->pos[index].i_sat; + pos->d_sat = loaded->pos[index].d_sat; + bd->error_limit = loaded->bd[index].error_limit; + bd->speed_limit = loaded->bd[index].speed_limit; + bd->counter_limit = loaded->bd[index].counter_limit; + output_set_reverse (output, loaded->output[index].reverse); +} /* Read parameters from eeprom. */ void eeprom_read_params (void) { - uint8_t *p8 = (uint8_t *) EEPROM_START; - uint16_t *p16; - if (eeprom_read_byte (p8++) != EEPROM_KEY) - return; - cs_aux[0].speed.max = eeprom_read_byte (p8++); - cs_aux[1].speed.max = eeprom_read_byte (p8++); - cs_aux[0].speed.slow = eeprom_read_byte (p8++); - cs_aux[1].speed.slow = eeprom_read_byte (p8++); - output_set_reverse (&output_aux[0], eeprom_read_byte (p8++)); - output_set_reverse (&output_aux[1], eeprom_read_byte (p8++)); - p16 = (uint16_t *) p8; - cs_aux[0].speed.acc = eeprom_read_word (p16++); - cs_aux[1].speed.acc = eeprom_read_word (p16++); - cs_aux[0].pos.kp = eeprom_read_word (p16++); - cs_aux[1].pos.kp = eeprom_read_word (p16++); - cs_aux[0].pos.ki = eeprom_read_word (p16++); - cs_aux[1].pos.ki = eeprom_read_word (p16++); - cs_aux[0].pos.kd = eeprom_read_word (p16++); - cs_aux[1].pos.kd = eeprom_read_word (p16++); - cs_aux[0].blocking_detection.error_limit = eeprom_read_word (p16++); - cs_aux[0].blocking_detection.speed_limit = eeprom_read_word (p16++); - cs_aux[0].blocking_detection.counter_limit = eeprom_read_word (p16++); - cs_aux[1].blocking_detection.error_limit = eeprom_read_word (p16++); - cs_aux[1].blocking_detection.speed_limit = eeprom_read_word (p16++); - cs_aux[1].blocking_detection.counter_limit = eeprom_read_word (p16++); - cs_aux[0].pos.e_sat = eeprom_read_word (p16++); - cs_aux[0].pos.i_sat = eeprom_read_word (p16++); - cs_aux[0].pos.d_sat = eeprom_read_word (p16++); - cs_aux[1].pos.e_sat = eeprom_read_word (p16++); - cs_aux[1].pos.i_sat = eeprom_read_word (p16++); - cs_aux[1].pos.d_sat = eeprom_read_word (p16++); + uint8_t i; + struct eeprom_t loaded; + eeprom_loaded = -1; + /* Load first good set. */ + for (i = 0; i < UTILS_COUNT (eeprom_params); i++) + { + /* Read EEPROM. */ + eeprom_read_block (&loaded, &eeprom_params[i], + sizeof (struct eeprom_t)); + /* Check CRC. */ + if (loaded.key == EEPROM_KEY + && crc_compute ((uint8_t *) &loaded, + sizeof (struct eeprom_t)) == 0) + { + /* Ok. */ + eeprom_loaded = i; + break; + } + } + /* Load defaults if no set is good. */ +#ifdef EEPROM_DEFAULTS + if (eeprom_loaded == -1) + { + memcpy_P (&loaded, &eeprom_defaults, sizeof (struct eeprom_t)); + eeprom_loaded = 0xDF; /* DeFaults. */ + } +#endif + if (eeprom_loaded != -1) + { + /* Ok, load parameters. */ + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + eeprom_read_params_helper (&loaded, i, &cs_aux[i].speed, + &cs_aux[i].pos, + &cs_aux[i].blocking_detection, + &output_aux[i]); + } +} + +static void +eeprom_write_params_helper (struct eeprom_t *param, uint8_t index, + speed_control_t *speed, pos_control_t *pos, + blocking_detection_t *bd, output_t *output) +{ + param->speed[index].max = speed->max; + param->speed[index].slow = speed->slow; + param->speed[index].acc = speed->acc; + param->pos[index].kp = pos->kp; + param->pos[index].ki = pos->ki; + param->pos[index].kd = pos->kd; + param->pos[index].e_sat = pos->e_sat; + param->pos[index].i_sat = pos->i_sat; + param->pos[index].d_sat = pos->d_sat; + param->bd[index].error_limit = bd->error_limit; + param->bd[index].speed_limit = bd->speed_limit; + param->bd[index].counter_limit = bd->counter_limit; + param->output[index].reverse = output->reverse; } /* Write parameters to eeprom. */ void eeprom_write_params (void) { - uint8_t *p8 = (uint8_t *) EEPROM_START; - uint16_t *p16; - eeprom_write_byte (p8++, EEPROM_KEY); - eeprom_write_byte (p8++, cs_aux[0].speed.max); - eeprom_write_byte (p8++, cs_aux[1].speed.max); - eeprom_write_byte (p8++, cs_aux[0].speed.slow); - eeprom_write_byte (p8++, cs_aux[1].speed.slow); - eeprom_write_byte (p8++, output_aux[0].reverse); - eeprom_write_byte (p8++, output_aux[1].reverse); - p16 = (uint16_t *) p8; - eeprom_write_word (p16++, cs_aux[0].speed.acc); - eeprom_write_word (p16++, cs_aux[1].speed.acc); - eeprom_write_word (p16++, cs_aux[0].pos.kp); - eeprom_write_word (p16++, cs_aux[1].pos.kp); - eeprom_write_word (p16++, cs_aux[0].pos.ki); - eeprom_write_word (p16++, cs_aux[1].pos.ki); - eeprom_write_word (p16++, cs_aux[0].pos.kd); - eeprom_write_word (p16++, cs_aux[1].pos.kd); - eeprom_write_word (p16++, cs_aux[0].blocking_detection.error_limit); - eeprom_write_word (p16++, cs_aux[0].blocking_detection.speed_limit); - eeprom_write_word (p16++, cs_aux[0].blocking_detection.counter_limit); - eeprom_write_word (p16++, cs_aux[1].blocking_detection.error_limit); - eeprom_write_word (p16++, cs_aux[1].blocking_detection.speed_limit); - eeprom_write_word (p16++, cs_aux[1].blocking_detection.counter_limit); - eeprom_write_word (p16++, cs_aux[0].pos.e_sat); - eeprom_write_word (p16++, cs_aux[0].pos.i_sat); - eeprom_write_word (p16++, cs_aux[0].pos.d_sat); - eeprom_write_word (p16++, cs_aux[1].pos.e_sat); - eeprom_write_word (p16++, cs_aux[1].pos.i_sat); - eeprom_write_word (p16++, cs_aux[1].pos.d_sat); + uint8_t i; + struct eeprom_t p; + /* Prepare parameters. */ + p.key = EEPROM_KEY; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + eeprom_write_params_helper (&p, i, &cs_aux[i].speed, + &cs_aux[i].pos, + &cs_aux[i].blocking_detection, + &output_aux[i]); + p.crc = crc_compute ((uint8_t *) &p, sizeof (p) - 1); + /* Write every sets. */ + for (i = 0; i < UTILS_COUNT (eeprom_params); i++) + { + eeprom_write_block (&p, &eeprom_params[i], + sizeof (struct eeprom_t)); + } } /* Clear eeprom parameters. */ void eeprom_clear_params (void) { - uint8_t *p = (uint8_t *) EEPROM_START; - eeprom_write_byte (p, 0xff); + uint8_t i; + /* Clear every sets. */ + for (i = 0; i < UTILS_COUNT (eeprom_params); i++) + eeprom_write_byte (&eeprom_params[i].key, 0); } diff --git a/digital/mimot/src/dirty/eeprom.h b/digital/mimot/src/dirty/eeprom.h index 9eb7c18e..6196cd99 100644 --- a/digital/mimot/src/dirty/eeprom.h +++ b/digital/mimot/src/dirty/eeprom.h @@ -26,7 +26,9 @@ * }}} */ /** Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x02 +#define EEPROM_KEY 0x03 + +extern int8_t eeprom_loaded; void eeprom_read_params (void); diff --git a/digital/mimot/src/dirty/eeprom.mak b/digital/mimot/src/dirty/eeprom.mak new file mode 100644 index 00000000..b41d5537 --- /dev/null +++ b/digital/mimot/src/dirty/eeprom.mak @@ -0,0 +1,9 @@ +EEPROM_DEFAULTS_new := $(EEPROM_DEFAULTS) +-include obj/eeprom_defaults +EXTRA_CLEAN_FILES += obj/eeprom_defaults +ifneq (was $(EEPROM_DEFAULTS_new),$(EEPROM_DEFAULTS_old)) +obj/eeprom_defaults: | $(OBJDIR) + echo "EEPROM_DEFAULTS_old = was $(EEPROM_DEFAULTS_new)" > $@ +.PHONY: obj/eeprom_defaults +endif +obj/eeprom.avr.avr.o: obj/eeprom_defaults diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c index ed703945..36688669 100644 --- a/digital/mimot/src/dirty/main.c +++ b/digital/mimot/src/dirty/main.c @@ -439,14 +439,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('P', 1): /* Print current settings. */ - proto_send1b ('E', EEPROM_KEY); + proto_send2b ('E', EEPROM_KEY, eeprom_loaded); proto_send1b ('w', (output_aux[0].reverse ? 1 : 0) | (output_aux[1].reverse ? 2 : 0)); break; case c ('P', 2): /* Print current settings for selected control. * - b: index. */ - proto_send1b ('E', EEPROM_KEY); + proto_send2b ('E', EEPROM_KEY, eeprom_loaded); proto_send1w ('a', speed->acc); proto_send2b ('s', speed->max, speed->slow); proto_send3w ('b', bd->error_limit, bd->speed_limit, diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index c4ac5026..7ee2c544 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -48,6 +48,9 @@ /** Simulate some AVR regs. */ uint8_t PORTB, PORTC, PORTD, PINC; +/** Index of loaded eeprom block. */ +int8_t eeprom_loaded = -1; + /* Robot model. */ const struct robot_t *simu_robot; diff --git a/digital/mimot/tools/gen_eeprom_default.py b/digital/mimot/tools/gen_eeprom_default.py new file mode 100644 index 00000000..10aa84d2 --- /dev/null +++ b/digital/mimot/tools/gen_eeprom_default.py @@ -0,0 +1,99 @@ +# mimot - Mini motor control, with motor driver. {{{ +# +# Copyright (C) 2012 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT org +# +# 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. +# +# }}} +from mimot import Proto +import mimot.init +import sys + +if len (sys.argv) != 3 or sys.argv[1] not in ('host', 'target'): + print >> sys.stderr, "usage: %s " % sys.argv[0] + sys.exit (1) + +target, robot = sys.argv[1:] + +init = dict (host = mimot.init.host, + target = mimot.init.target)[target] +if robot not in init: + print >> sys.stderr, "unknown robot" + sys.exit (1) + +proto = Proto (None, **init[robot]) + +params = proto.param + +template = """\ +/* Autogenerated file, do not edit! */ + +#ifdef EEPROM_DEFAULTS_KEY +# error "duplicated definition" +#endif + +#define EEPROM_DEFAULTS_KEY {key:#x} + +struct eeprom_t PROGMEM eeprom_defaults = +{defaults}; +""" + +def param (name, offset = 0, m = None): + if m: + mname = m + '_' + name + if mname in params: + name = mname + value = params[name] + if offset: + value = int (round (value * (1 << offset))) + return '0x%x /* %s */' % (value, name) + +def j (*params): + return '{\n ' + ',\n '.join (p.replace ('\n', '\n ') + for p in params) + '\n}' + +indexes = [ 'a%d' % i for i in xrange (2) ] + +key = 0x03 +defaults = j ( + 'EEPROM_DEFAULTS_KEY', + j (*[j ( + param ('speed_max', m = m), + param ('speed_slow', m = m), + param ('acc', offset = 8, m = m)) + for m in indexes]), + j (*[j ( + param ('kp', offset = 8, m = m), + param ('ki', offset = 8, m = m), + param ('kd', offset = 8, m = m), + param ('e_sat', m = m), + param ('i_sat', m = m), + param ('d_sat', m = m)) + for m in indexes]), + j (*[j ( + param ('bd_error_limit', m = m), + param ('bd_speed_limit', m = m), + param ('bd_counter_limit', m = m)) + for m in indexes]), + j (*[j ( + param ('reverse', m = m)) + for m in indexes]), + '0') + +print template.format (key = key, defaults = defaults) -- cgit v1.2.3