From cf8fbdbfb994576b6f3278602c25e84795deac11 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 Apr 2012 20:18:16 +0200 Subject: digital: use 16bit speed --- digital/mimot/src/dirty/aux.c | 6 ++--- digital/mimot/src/dirty/aux.h | 6 ++--- digital/mimot/src/dirty/eeprom.avr.c | 10 +++---- digital/mimot/src/dirty/eeprom.h | 2 +- digital/mimot/src/dirty/main.c | 51 ++++++++++++++++++------------------ digital/mimot/src/dirty/twi_proto.c | 34 +++++++++++++----------- digital/mimot/tools/mimot/mimot.py | 10 +++---- 7 files changed, 61 insertions(+), 58 deletions(-) (limited to 'digital/mimot') diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c index 50bebc3a..d9110e11 100644 --- a/digital/mimot/src/dirty/aux.c +++ b/digital/mimot/src/dirty/aux.c @@ -162,7 +162,7 @@ aux_traj_clamp (struct aux_t *aux) } void -aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm) +aux_traj_clamp_start (struct aux_t *aux, int16_t speed, int16_t clampin_pwm) { aux->traj_mode = AUX_TRAJ_CLAMP; aux->clampin_pwm = clampin_pwm; @@ -196,7 +196,7 @@ aux_traj_find_zero (struct aux_t *aux) /** Start find zero mode. */ void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos) +aux_traj_find_zero_start (struct aux_t *aux, int16_t speed, int16_t reset_pos) { aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT; aux->reset_pos = reset_pos; @@ -236,7 +236,7 @@ aux_traj_find_limit (struct aux_t *aux) /** Start find limit mode. */ void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos) +aux_traj_find_limit_start (struct aux_t *aux, int16_t speed, int16_t reset_pos) { aux->traj_mode = AUX_TRAJ_FIND_LIMIT; speed_control_set_speed (&aux->cs->speed, speed); diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h index 133b7d51..2716f438 100644 --- a/digital/mimot/src/dirty/aux.h +++ b/digital/mimot/src/dirty/aux.h @@ -66,13 +66,13 @@ void aux_traj_goto_start (struct aux_t *aux, uint16_t pos); void -aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm); +aux_traj_clamp_start (struct aux_t *aux, int16_t speed, int16_t clampin_pwm); void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos); +aux_traj_find_zero_start (struct aux_t *aux, int16_t speed, int16_t reset_pos); void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, +aux_traj_find_limit_start (struct aux_t *aux, int16_t speed, int16_t reset_pos); void diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c index faf2c282..8a49a27d 100644 --- a/digital/mimot/src/dirty/eeprom.avr.c +++ b/digital/mimot/src/dirty/eeprom.avr.c @@ -45,9 +45,9 @@ struct eeprom_t uint8_t key; /** Saved parameters. */ struct { - uint8_t max; - uint8_t slow; - uint16_t acc; + uint16_t max; + uint16_t slow; + uint16_t acc_f; } speed[EEPROM_INDEX_NB]; struct { uint16_t kp; @@ -92,7 +92,7 @@ eeprom_read_params_helper (struct eeprom_t *loaded, uint8_t index, { speed->max = loaded->speed[index].max; speed->slow = loaded->speed[index].slow; - speed->acc = loaded->speed[index].acc; + speed->acc_f = loaded->speed[index].acc_f; pos->kp = loaded->pos[index].kp; pos->ki = loaded->pos[index].ki; pos->kd = loaded->pos[index].kd; @@ -154,7 +154,7 @@ eeprom_write_params_helper (struct eeprom_t *param, uint8_t index, { param->speed[index].max = speed->max; param->speed[index].slow = speed->slow; - param->speed[index].acc = speed->acc; + param->speed[index].acc_f = speed->acc_f; param->pos[index].kp = pos->kp; param->pos[index].ki = pos->ki; param->pos[index].kd = pos->kd; diff --git a/digital/mimot/src/dirty/eeprom.h b/digital/mimot/src/dirty/eeprom.h index 6196cd99..0e9dc122 100644 --- a/digital/mimot/src/dirty/eeprom.h +++ b/digital/mimot/src/dirty/eeprom.h @@ -26,7 +26,7 @@ * }}} */ /** Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x03 +#define EEPROM_KEY 0x04 extern int8_t eeprom_loaded; diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c index af61a5a3..03bae08a 100644 --- a/digital/mimot/src/dirty/main.c +++ b/digital/mimot/src/dirty/main.c @@ -147,8 +147,8 @@ main_loop (void) } if (main_stat_speed && !--main_stat_speed_cpt) { - proto_send2b ('S', cs_aux[0].speed.cur >> 8, - cs_aux[1].speed.cur >> 8); + proto_send2w ('S', cs_aux[0].speed.cur_f >> 8, + cs_aux[1].speed.cur_f >> 8); main_stat_speed_cpt = main_stat_speed; } if (main_stat_pos && !--main_stat_pos_cpt) @@ -248,12 +248,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_control_set_speed (speed, 0); control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0); break; - case c ('s', 2): + case c ('s', 3): /* Set auxiliary speed. * - b: aux index. - * - b: speed. */ + * - w: speed. */ if (!auxp) { proto_send0 ('?'); return; } - speed_control_set_speed (speed, args[1]); + speed_control_set_speed (speed, v8_to_v16 (args[1], args[2])); control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0); break; case c ('s', 6): @@ -279,33 +279,34 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2])); break; - case c ('y', 5): + case c ('y', 6): /* Auxiliary clamp. * - b: aux index. - * - b: speed. + * - w: speed. * - w: clamping PWM. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (!seq_start (seq, args[4])) + if (!seq_start (seq, args[5])) break; - aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3])); + aux_traj_clamp_start (auxp, v8_to_v16 (args[1], args[2]), + v8_to_v16 (args[3], args[4])); break; - case c ('y', 6): + case c ('y', 7): /* Auxiliary find zero. * - b: aux index. - * - b: speed. + * - w: speed. * - b: use switch. * - w: reset position. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (!seq_start (seq, args[5])) + if (!seq_start (seq, args[6])) break; - if (args[2]) - aux_traj_find_zero_start (auxp, args[1], - v8_to_v16 (args[3], args[4])); + if (args[3]) + aux_traj_find_zero_start (auxp, v8_to_v16 (args[1], args[2]), + v8_to_v16 (args[4], args[5])); else - aux_traj_find_limit_start (auxp, args[1], - v8_to_v16 (args[3], args[4])); + aux_traj_find_limit_start (auxp, v8_to_v16 (args[1], args[2]), + v8_to_v16 (args[4], args[5])); break; case c ('a', 2): /* Set all acknoledge. @@ -374,16 +375,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: index. * - w: acceleration. */ if (!speed) { proto_send0 ('?'); return; } - speed->acc = v8_to_v16 (args[2], args[3]); + speed->acc_f = v8_to_v16 (args[2], args[3]); break; - case c ('s', 4): + case c ('s', 6): /* Set maximum and slow speed. * - b: index. - * - b: max speed. - * - b: slow speed. */ + * - w: max speed. + * - w: slow speed. */ if (!speed) { proto_send0 ('?'); return; } - speed->max = args[2]; - speed->slow = args[3]; + speed->max = v8_to_v16 (args[2], args[3]); + speed->slow = v8_to_v16 (args[4], args[5]); break; case c ('p', 4): /* Set proportional coefficient. @@ -457,8 +458,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: index. */ if (!pos) { proto_send0 ('?'); return; } proto_send2b ('E', EEPROM_KEY, eeprom_loaded); - proto_send1w ('a', speed->acc); - proto_send2b ('s', speed->max, speed->slow); + proto_send1w ('a', speed->acc_f); + proto_send2w ('s', speed->max, speed->slow); proto_send3w ('b', bd->error_limit, bd->speed_limit, bd->counter_limit); proto_send1w ('p', pos->kp); diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index 5398815f..a61d3d6c 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -110,46 +110,48 @@ twi_proto_callback (u8 *buf, u8 size) /* Reset. */ utils_reset (); break; - case c ('b', 3): + case c ('b', 4): /* Move the aux0. * - w: new position. - * - b: speed. */ - cs_aux[0].speed.max = buf[4]; + * - w: speed. */ + cs_aux[0].speed.max = v8_to_v16 (buf[4], buf[5]); aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3])); break; case c ('c', 3): /* Move the aux1. * - w: new position. - * - b: speed. */ - cs_aux[1].speed.max = buf[4]; + * - w: speed. */ + cs_aux[1].speed.max = v8_to_v16 (buf[4], buf[5]); aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3])); break; - case c ('B', 5): + case c ('B', 6): /* Find the zero position. * - b: aux index. - * - b: speed. + * - w: speed. * - b: use switch. * - w: reset position. */ if (buf[2] < AC_ASSERV_AUX_NB) { - if (buf[4]) - aux_traj_find_zero_start (&aux[buf[2]], buf[3], - v8_to_v16 (buf[5], buf[6])); + if (buf[5]) + aux_traj_find_zero_start (&aux[buf[2]], + v8_to_v16 (buf[3], buf[4]), + v8_to_v16 (buf[6], buf[7])); else - aux_traj_find_limit_start (&aux[buf[2]], buf[3], - v8_to_v16 (buf[5], buf[6])); + aux_traj_find_limit_start (&aux[buf[2]], + v8_to_v16 (buf[3], buf[4]), + v8_to_v16 (buf[6], buf[7])); } else buf[0] = 0; break; - case c ('l', 4): + case c ('l', 5): /* Clamp. * - b: aux index. - * - b: speed. + * - w: speed. * - w: claming PWM. */ if (buf[2] < AC_ASSERV_AUX_NB) - aux_traj_clamp_start (&aux[buf[2]], buf[3], - v8_to_v16 (buf[4], buf[5])); + aux_traj_clamp_start (&aux[buf[2]], v8_to_v16 (buf[3], buf[4]), + v8_to_v16 (buf[5], buf[6])); else buf[0] = 0; break; diff --git a/digital/mimot/tools/mimot/mimot.py b/digital/mimot/tools/mimot/mimot.py index 836479fb..cb4caace 100644 --- a/digital/mimot/tools/mimot/mimot.py +++ b/digital/mimot/tools/mimot/mimot.py @@ -36,7 +36,7 @@ class Proto: stats_format = { 'C': 'HH', - 'S': 'bb', + 'S': 'hh', 'P': 'hhhh', 'W': 'hh', } @@ -140,7 +140,7 @@ class Proto: def speed (self, w, s): """Speed consign.""" - self.proto.send ('s', 'Bb', self._index[w], s) + self.proto.send ('s', 'Bh', self._index[w], s) def speed_pos (self, w, offset): """Speed controlled position consign.""" @@ -160,14 +160,14 @@ class Proto: """Clamp (speed control, then open loop PWM).""" i = self._index[w] self.aseq[i] += 1 - self.proto.send ('y', 'BBhB', i, s, pwm, self.aseq[i]) + self.proto.send ('y', 'BHhB', i, s, pwm, self.aseq[i]) self.wait (self.finished, auto = True) def find_zero (self, w, s, use_switch = True, reset_pos = 0): """Find zero position.""" i = self._index[w] self.aseq[i] += 1 - self.proto.send ('y', 'BBBhB', i, s, 1 if use_switch else 0, + self.proto.send ('y', 'BHBhB', i, s, 1 if use_switch else 0, reset_pos, self.aseq[i]) self.wait (self.finished, auto = True) @@ -182,7 +182,7 @@ class Proto: self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + '_ki'])) self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + '_kd'])) self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + '_acc'])) - self.proto.send ('p', 'cBBB', 's', index, p[m + '_speed_max'], + self.proto.send ('p', 'cBHH', 's', index, p[m + '_speed_max'], p[m + '_speed_slow']) self.proto.send ('p', 'cBHHB', 'b', index, p[m + '_bd_error_limit'], p[m + '_bd_speed_limit'], -- cgit v1.2.3