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/asserv/src/asserv/aux.c | 8 ++--- digital/asserv/src/asserv/aux.h | 6 ++-- digital/asserv/src/asserv/eeprom.avr.c | 10 +++---- digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 53 ++++++++++++++++++---------------- digital/asserv/src/asserv/traj.c | 6 ++-- digital/asserv/src/asserv/twi_proto.c | 46 ++++++++++++++--------------- digital/asserv/tools/asserv/asserv.py | 10 +++---- 8 files changed, 72 insertions(+), 69 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c index 204f3fac..5b942bd9 100644 --- a/digital/asserv/src/asserv/aux.c +++ b/digital/asserv/src/asserv/aux.c @@ -159,7 +159,7 @@ aux_traj_find_zero (struct aux_t *aux) case AUX_TRAJ_FIND_ZERO_NOT_REVERSE: if (zero) { - aux->cs->speed.cons = -aux->cs->speed.cons; + aux->cs->speed.cons_f = -aux->cs->speed.cons_f; aux->traj_mode = AUX_TRAJ_FIND_ZERO; } break; @@ -177,7 +177,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) +aux_traj_find_zero_start (struct aux_t *aux, int16_t speed) { aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT; speed_control_set_speed (&aux->cs->speed, speed); @@ -186,7 +186,7 @@ aux_traj_find_zero_start (struct aux_t *aux, int8_t speed) /** Start find zero reverse mode. */ void -aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed) +aux_traj_find_zero_reverse_start (struct aux_t *aux, int16_t speed) { aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT_REVERSE; speed_control_set_speed (&aux->cs->speed, speed); @@ -210,7 +210,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) +aux_traj_find_limit_start (struct aux_t *aux, int16_t speed) { aux->traj_mode = AUX_TRAJ_FIND_LIMIT; speed_control_set_speed (&aux->cs->speed, speed); diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h index cf12aa7d..6d2c583c 100644 --- a/digital/asserv/src/asserv/aux.h +++ b/digital/asserv/src/asserv/aux.h @@ -67,13 +67,13 @@ void aux_traj_goto_start (struct aux_t *aux, uint16_t pos); void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed); +aux_traj_find_zero_start (struct aux_t *aux, int16_t speed); void -aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed); +aux_traj_find_zero_reverse_start (struct aux_t *aux, int16_t speed); 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); void aux_traj_update (void); diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 0d1753ee..c2a85050 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -48,9 +48,9 @@ struct eeprom_t /** Saved parameters. */ uint32_t encoder_right_correction; 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; @@ -99,7 +99,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; @@ -177,7 +177,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/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index 6da2a083..23744de1 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 0x50 +#define EEPROM_KEY 0x51 extern int8_t eeprom_loaded; diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 36cd2219..97d0b1c7 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -181,10 +181,10 @@ main_loop (void) #endif if (main_stat_speed && !--main_stat_speed_cpt) { - proto_sendb ('S', cs_main.speed_theta.cur >> 8, - cs_main.speed_alpha.cur >> 8 - AUX_IF (, cs_aux[0].speed.cur >> 8, - cs_aux[1].speed.cur >> 8)); + proto_sendw ('S', cs_main.speed_theta.cur_f >> 8, + cs_main.speed_alpha.cur_f >> 8 + AUX_IF (, 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) @@ -327,21 +327,23 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_control_set_speed (&cs_main.speed_alpha, 0); control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0); break; - case c ('s', 2): + case c ('s', 4): /* Set speed. - * - b: theta speed. - * - b: alpha speed. */ - speed_control_set_speed (&cs_main.speed_theta, args[0]); - speed_control_set_speed (&cs_main.speed_alpha, args[1]); + * - w: theta speed. + * - w: alpha speed. */ + speed_control_set_speed (&cs_main.speed_theta, + v8_to_v16 (args[0], args[1])); + speed_control_set_speed (&cs_main.speed_alpha, + v8_to_v16 (args[2], args[3])); control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0); break; #if AC_ASSERV_AUX_NB - 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; #endif @@ -484,18 +486,19 @@ 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', 3): + case c ('Y', 4): /* Auxiliary find zero. * - b: aux index. - * - b: speed. + * - w: speed. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (!seq_start (seq, args[2])) + if (!seq_start (seq, args[3])) break; if (args[0] == 0) - aux_traj_find_limit_start (auxp, args[1]); + aux_traj_find_limit_start (auxp, v8_to_v16 (args[1], args[2])); else - aux_traj_find_zero_reverse_start (auxp, args[1]); + aux_traj_find_zero_reverse_start (auxp, + v8_to_v16 (args[1], args[2])); break; case c ('a', 3): /* Set all acknoledge. @@ -634,16 +637,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. @@ -746,8 +749,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/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index abf6c730..48802fcb 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -150,7 +150,7 @@ static void traj_ftw (void) { uint8_t left, center, right; - int8_t speed; + int16_t speed; speed = cs_main.speed_theta.slow; if (!traj_backward) { @@ -260,7 +260,7 @@ void traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, int32_t init_a) { - int8_t speed; + int16_t speed; traj_mode = TRAJ_PTW; traj_init_x = init_x; traj_init_y = init_y; @@ -281,7 +281,7 @@ static void traj_gtd (void) { #ifdef CONTACT_CENTER_IO - int8_t speed; + int16_t speed; speed = cs_main.speed_theta.slow; if (IO_GET (CONTACT_CENTER_IO)) { diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index d466493f..190c3caf 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -79,8 +79,8 @@ twi_proto_update (void) | (control_state_is_blocked (&cs_aux[0].state) ? (1 << 5) : 0) | (control_state_is_finished (&cs_aux[0].state) ? (1 << 4) : 0) #endif - | (cs_main.speed_theta.cur < 0 ? (1 << 3) : 0) - | (cs_main.speed_theta.cur > 0 ? (1 << 2) : 0) + | (cs_main.speed_theta.cur_f < 0 ? (1 << 3) : 0) + | (cs_main.speed_theta.cur_f > 0 ? (1 << 2) : 0) | (control_state_is_blocked (&cs_main.state) ? (1 << 1) : 0) | (control_state_is_finished (&cs_main.state) ? (1 << 0) : 0); status[1] = PINC; @@ -216,29 +216,29 @@ twi_proto_callback (u8 *buf, u8 size) buf[10]); break; #if AC_ASSERV_AUX_NB - 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 ('B', 1): + case c ('B', 2): /* Find the aux0 zero position. - * - b: speed. */ - aux_traj_find_limit_start (&aux[0], buf[2]); + * - w: speed. */ + aux_traj_find_limit_start (&aux[0], v8_to_v16 (buf[2], buf[3])); break; - case c ('c', 3): + case c ('c', 4): /* 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 ('C', 1): + case c ('C', 2): /* Find the aux1 zero position. - * - b: speed. */ - aux_traj_find_zero_reverse_start (&aux[1], buf[2]); + * - w: speed. */ + aux_traj_find_zero_reverse_start (&aux[1], v8_to_v16 (buf[2], buf[3])); break; case c ('r', 1): /* Set aux zero pwm. @@ -302,16 +302,16 @@ twi_proto_params (u8 *buf, u8 size) break; case 's': /* Set maximum and slow speed. - * - b: theta max. - * - b: alpha max. - * - b: theta slow. - * - b: alpha slow. */ - if (size < 4) + * - w: theta max. + * - w: alpha max. + * - w: theta slow. + * - w: alpha slow. */ + if (size < 8) return 1; - cs_main.speed_theta.max = buf[0]; - cs_main.speed_alpha.max = buf[1]; - cs_main.speed_theta.slow = buf[2]; - cs_main.speed_alpha.slow = buf[3]; + cs_main.speed_theta.max = v8_to_v16 (buf[0], buf[1]); + cs_main.speed_alpha.max = v8_to_v16 (buf[2], buf[3]); + cs_main.speed_theta.slow = v8_to_v16 (buf[4], buf[5]); + cs_main.speed_alpha.slow = v8_to_v16 (buf[6], buf[7]); eat = 4; break; default: diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index b97174e0..fb45c633 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -36,7 +36,7 @@ class Proto: stats_format = { 'C': 'HHHH', - 'S': 'bbbb', + 'S': 'hhhh', 'P': 'hhhh', 'Q': 'hhhh', 'W': 'hhhh', @@ -189,11 +189,11 @@ class Proto: def speed (self, w, s): """Speed consign.""" if w == 't': - self.proto.send ('s', 'bb', s, 0) + self.proto.send ('s', 'hh', s, 0) elif w == 'a': - self.proto.send ('s', 'bb', 0, s) + self.proto.send ('s', 'hh', 0, s) else: - 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.""" @@ -310,7 +310,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