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/asserv/src/asserv/Makefile | 4 + digital/asserv/src/asserv/eeprom.avr.c | 317 ++++++++++++++++------------- digital/asserv/src/asserv/eeprom.h | 4 +- digital/asserv/src/asserv/eeprom.mak | 9 + digital/asserv/src/asserv/main.c | 4 +- digital/asserv/src/asserv/simu.host.c | 3 + digital/asserv/tools/gen_eeprom_default.py | 105 ++++++++++ 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 +++++++++ 14 files changed, 565 insertions(+), 216 deletions(-) create mode 100644 digital/asserv/src/asserv/eeprom.mak create mode 100644 digital/asserv/tools/gen_eeprom_default.py create mode 100644 digital/mimot/src/dirty/eeprom.mak create mode 100644 digital/mimot/tools/gen_eeprom_default.py diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile index d19d9909..80159026 100644 --- a/digital/asserv/src/asserv/Makefile +++ b/digital/asserv/src/asserv/Makefile @@ -16,4 +16,8 @@ AVR_MCU = atmega128 OPTIMIZE = -O2 HOST_LIBS = -lm +AVR_DEFS = $(EEPROM_DEFAULTS:%=-DEEPROM_DEFAULTS=\"%\") + include $(BASE)/make/Makefile.gen + +include eeprom.mak diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 004a2ecb..70e45dca 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -25,182 +25,211 @@ #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" #include "postrack.h" #include "traj.h" -#define EEPROM_START 0 +#define EEPROM_INDEX_NB (2 + 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. */ - -static uint32_t -eeprom_read_32 (uint8_t *addr) +struct eeprom_t { - 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]); -} + /** Identify parameters version. */ + uint8_t key; + /** Saved parameters. */ + uint32_t encoder_right_correction; + 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]; + uint16_t postrack_footing; + uint16_t traj_eps; + uint16_t traj_aeps; + uint16_t traj_angle_limit; + /** 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_write_32 (uint8_t *addr, uint32_t dw) +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) { - 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)); + 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_main.speed_theta.max = eeprom_read_byte (p8++); - cs_main.speed_alpha.max = eeprom_read_byte (p8++); - cs_aux[0].speed.max = eeprom_read_byte (p8++); - cs_aux[1].speed.max = eeprom_read_byte (p8++); - cs_main.speed_theta.slow = eeprom_read_byte (p8++); - cs_main.speed_alpha.slow = 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_left, eeprom_read_byte (p8++)); - output_set_reverse (&output_right, eeprom_read_byte (p8++)); - output_set_reverse (&output_aux[0], eeprom_read_byte (p8++)); - output_set_reverse (&output_aux[1], eeprom_read_byte (p8++)); - encoder_corrector_set_correction (&encoder_right_corrector, - eeprom_read_32 (p8)); p8 += 4; - p16 = (uint16_t *) p8; - postrack_set_footing (eeprom_read_word (p16++)); - cs_main.speed_theta.acc = eeprom_read_word (p16++); - cs_main.speed_alpha.acc = eeprom_read_word (p16++); - cs_aux[0].speed.acc = eeprom_read_word (p16++); - cs_aux[1].speed.acc = eeprom_read_word (p16++); - cs_main.pos_theta.kp = eeprom_read_word (p16++); - cs_main.pos_alpha.kp = eeprom_read_word (p16++); - cs_aux[0].pos.kp = eeprom_read_word (p16++); - cs_aux[1].pos.kp = eeprom_read_word (p16++); - cs_main.pos_theta.ki = eeprom_read_word (p16++); - cs_main.pos_alpha.ki = eeprom_read_word (p16++); - cs_aux[0].pos.ki = eeprom_read_word (p16++); - cs_aux[1].pos.ki = eeprom_read_word (p16++); - cs_main.pos_theta.kd = eeprom_read_word (p16++); - cs_main.pos_alpha.kd = eeprom_read_word (p16++); - cs_aux[0].pos.kd = eeprom_read_word (p16++); - cs_aux[1].pos.kd = eeprom_read_word (p16++); - cs_main.blocking_detection_theta.error_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_theta.speed_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_theta.counter_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_alpha.error_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_alpha.speed_limit = eeprom_read_word (p16++); - cs_main.blocking_detection_alpha.counter_limit = 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_main.pos_theta.e_sat = eeprom_read_word (p16++); - cs_main.pos_theta.i_sat = eeprom_read_word (p16++); - cs_main.pos_theta.d_sat = eeprom_read_word (p16++); - cs_main.pos_alpha.e_sat = eeprom_read_word (p16++); - cs_main.pos_alpha.i_sat = eeprom_read_word (p16++); - cs_main.pos_alpha.d_sat = 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++); - traj_eps = eeprom_read_word (p16++); - traj_aeps = eeprom_read_word (p16++); - traj_set_angle_limit (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. */ + encoder_corrector_set_correction (&encoder_right_corrector, + loaded.encoder_right_correction); + eeprom_read_params_helper (&loaded, 0, &cs_main.speed_theta, + &cs_main.pos_theta, + &cs_main.blocking_detection_theta, + &output_left); + eeprom_read_params_helper (&loaded, 1, &cs_main.speed_alpha, + &cs_main.pos_alpha, + &cs_main.blocking_detection_alpha, + &output_right); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + eeprom_read_params_helper (&loaded, 2 + i, &cs_aux[i].speed, + &cs_aux[i].pos, + &cs_aux[i].blocking_detection, + &output_aux[i]); + postrack_set_footing (loaded.postrack_footing); + traj_eps = loaded.traj_eps; + traj_aeps = loaded.traj_aeps; + traj_set_angle_limit (loaded.traj_angle_limit); + } +} + +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_main.speed_theta.max); - eeprom_write_byte (p8++, cs_main.speed_alpha.max); - eeprom_write_byte (p8++, cs_aux[0].speed.max); - eeprom_write_byte (p8++, cs_aux[1].speed.max); - eeprom_write_byte (p8++, cs_main.speed_theta.slow); - eeprom_write_byte (p8++, cs_main.speed_alpha.slow); - eeprom_write_byte (p8++, cs_aux[0].speed.slow); - eeprom_write_byte (p8++, cs_aux[1].speed.slow); - eeprom_write_byte (p8++, output_left.reverse); - eeprom_write_byte (p8++, output_right.reverse); - eeprom_write_byte (p8++, output_aux[0].reverse); - eeprom_write_byte (p8++, output_aux[1].reverse); - eeprom_write_32 (p8, encoder_right_corrector.correction); p8 += 4; - p16 = (uint16_t *) p8; - eeprom_write_word (p16++, postrack_footing); - eeprom_write_word (p16++, cs_main.speed_theta.acc); - eeprom_write_word (p16++, cs_main.speed_alpha.acc); - eeprom_write_word (p16++, cs_aux[0].speed.acc); - eeprom_write_word (p16++, cs_aux[1].speed.acc); - eeprom_write_word (p16++, cs_main.pos_theta.kp); - eeprom_write_word (p16++, cs_main.pos_alpha.kp); - eeprom_write_word (p16++, cs_aux[0].pos.kp); - eeprom_write_word (p16++, cs_aux[1].pos.kp); - eeprom_write_word (p16++, cs_main.pos_theta.ki); - eeprom_write_word (p16++, cs_main.pos_alpha.ki); - eeprom_write_word (p16++, cs_aux[0].pos.ki); - eeprom_write_word (p16++, cs_aux[1].pos.ki); - eeprom_write_word (p16++, cs_main.pos_theta.kd); - eeprom_write_word (p16++, cs_main.pos_alpha.kd); - eeprom_write_word (p16++, cs_aux[0].pos.kd); - eeprom_write_word (p16++, cs_aux[1].pos.kd); - eeprom_write_word (p16++, cs_main.blocking_detection_theta.error_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_theta.speed_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_theta.counter_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_alpha.error_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_alpha.speed_limit); - eeprom_write_word (p16++, cs_main.blocking_detection_alpha.counter_limit); - 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_main.pos_theta.e_sat); - eeprom_write_word (p16++, cs_main.pos_theta.i_sat); - eeprom_write_word (p16++, cs_main.pos_theta.d_sat); - eeprom_write_word (p16++, cs_main.pos_alpha.e_sat); - eeprom_write_word (p16++, cs_main.pos_alpha.i_sat); - eeprom_write_word (p16++, cs_main.pos_alpha.d_sat); - 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); - eeprom_write_word (p16++, traj_eps); - eeprom_write_word (p16++, traj_aeps); - eeprom_write_word (p16++, traj_angle_limit); + uint8_t i; + struct eeprom_t p; + /* Prepare parameters. */ + p.key = EEPROM_KEY; + p.encoder_right_correction = encoder_right_corrector.correction; + eeprom_write_params_helper (&p, 0, &cs_main.speed_theta, + &cs_main.pos_theta, + &cs_main.blocking_detection_theta, + &output_left); + eeprom_write_params_helper (&p, 1, &cs_main.speed_alpha, + &cs_main.pos_alpha, + &cs_main.blocking_detection_alpha, + &output_right); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + eeprom_write_params_helper (&p, 2 + i, &cs_aux[i].speed, + &cs_aux[i].pos, + &cs_aux[i].blocking_detection, + &output_aux[i]); + p.postrack_footing = postrack_footing; + p.traj_eps = traj_eps; + p.traj_aeps = traj_aeps; + p.traj_angle_limit = traj_angle_limit; + 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/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index c4cec931..6da2a083 100644 --- a/digital/asserv/src/asserv/eeprom.h +++ b/digital/asserv/src/asserv/eeprom.h @@ -26,7 +26,9 @@ * }}} */ /** Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x4f +#define EEPROM_KEY 0x50 + +extern int8_t eeprom_loaded; void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/eeprom.mak b/digital/asserv/src/asserv/eeprom.mak new file mode 100644 index 00000000..b41d5537 --- /dev/null +++ b/digital/asserv/src/asserv/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/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 5d91d90e..5fbd3a60 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -699,7 +699,7 @@ 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_send1d ('c', encoder_right_corrector.correction); proto_send1w ('f', postrack_footing); proto_send2w ('e', traj_eps, traj_aeps); @@ -712,7 +712,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) 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/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 1c609cc6..82f628ca 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -49,6 +49,9 @@ /** Simulate some AVR regs. */ uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; +/** Index of loaded eeprom block. */ +int8_t eeprom_loaded = -1; + /* Robot model. */ const struct robot_t *simu_robot; diff --git a/digital/asserv/tools/gen_eeprom_default.py b/digital/asserv/tools/gen_eeprom_default.py new file mode 100644 index 00000000..d24950f2 --- /dev/null +++ b/digital/asserv/tools/gen_eeprom_default.py @@ -0,0 +1,105 @@ +# asserv - Position & speed motor control on AVR. {{{ +# +# 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 asserv import Proto +import asserv.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 = asserv.init.host, + target = asserv.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 = [ 't', 'a' ] + [ 'a%d' % i for i in xrange (proto.aux_nb) ] +outputs = [ 'l', 'r' ] + [ 'a%d' % i for i in xrange (proto.aux_nb) ] + +key = 0x50 +defaults = j ( + 'EEPROM_DEFAULTS_KEY', + param ('encoder_right_correction', offset = 24), + 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 outputs]), + param ('footing'), + param ('eps'), + param ('aeps'), + param ('angle_limit'), + '0') + +print template.format (key = key, defaults = defaults) 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