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/ai/src/twi_master/asserv.c | 46 ++++++++------- digital/ai/src/twi_master/asserv.h | 12 ++-- digital/ai/src/twi_master/mimot.c | 66 ++++++++++++---------- digital/ai/src/twi_master/mimot.h | 16 +++--- 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 ++-- .../modules/motor/speed_control/speed_control.c | 50 ++++++++-------- .../modules/motor/speed_control/speed_control.h | 20 +++---- .../modules/motor/speed_control/speed_control.txt | 2 +- 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 ++-- 22 files changed, 247 insertions(+), 225 deletions(-) diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index 76b2eb15..153997b6 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -339,25 +339,27 @@ asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, #if AC_ASSERV_AUX_NB void -asserv_move_motor0_absolute (uint16_t position, uint8_t speed) +asserv_move_motor0_absolute (uint16_t position, uint16_t speed) { uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); buffer[0] = 'b'; buffer[1] = v16_to_v8 (position, 1); buffer[2] = v16_to_v8 (position, 0); - buffer[3] = speed; - twi_master_send_buffer (4); + buffer[3] = v16_to_v8 (speed, 1); + buffer[4] = v16_to_v8 (speed, 0); + twi_master_send_buffer (5); } void -asserv_move_motor1_absolute (uint16_t position, uint8_t speed) +asserv_move_motor1_absolute (uint16_t position, uint16_t speed) { uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); buffer[0] = 'c'; buffer[1] = v16_to_v8 (position, 1); buffer[2] = v16_to_v8 (position, 0); - buffer[3] = speed; - twi_master_send_buffer (4); + buffer[3] = v16_to_v8 (speed, 1); + buffer[4] = v16_to_v8 (speed, 0); + twi_master_send_buffer (5); } #endif /* AC_ASSERV_AUX_NB */ @@ -400,17 +402,21 @@ asserv_set_angle_position (int16_t angle) } void -asserv_set_speed (uint8_t linear_high, uint8_t angular_high, - uint8_t linear_low, uint8_t angular_low) +asserv_set_speed (uint16_t linear_high, uint16_t angular_high, + uint16_t linear_low, uint16_t angular_low) { uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); buffer[0] = 'p'; buffer[1] = 's'; - buffer[2] = linear_high; - buffer[3] = angular_high; - buffer[4] = linear_low; - buffer[5] = angular_low; - twi_master_send_buffer (6); + buffer[2] = v16_to_v8 (linear_high, 1); + buffer[3] = v16_to_v8 (linear_high, 0); + buffer[4] = v16_to_v8 (angular_high, 1); + buffer[5] = v16_to_v8 (angular_high, 0); + buffer[6] = v16_to_v8 (linear_low, 1); + buffer[7] = v16_to_v8 (linear_low, 0); + buffer[8] = v16_to_v8 (angular_low, 1); + buffer[9] = v16_to_v8 (angular_low, 0); + twi_master_send_buffer (10); } void @@ -454,21 +460,23 @@ asserv_goto (uint32_t x, uint32_t y, uint8_t backward) #if AC_ASSERV_AUX_NB void -asserv_motor0_zero_position (int8_t speed) +asserv_motor0_zero_position (int16_t speed) { uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); buffer[0] = 'B'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[1] = v16_to_v8 (speed, 1); + buffer[2] = v16_to_v8 (speed, 0); + twi_master_send_buffer (3); } void -asserv_motor1_zero_position (int8_t speed) +asserv_motor1_zero_position (int16_t speed) { uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); buffer[0] = 'C'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[1] = v16_to_v8 (speed, 1); + buffer[2] = v16_to_v8 (speed, 0); + twi_master_send_buffer (3); } void diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 90e3b785..ed6ed046 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -223,7 +223,7 @@ asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, * @param speed speed of the movement. */ void -asserv_move_motor0_absolute (uint16_t position, uint8_t speed); +asserv_move_motor0_absolute (uint16_t position, uint16_t speed); /** * Move the motor1. @@ -234,7 +234,7 @@ asserv_move_motor0_absolute (uint16_t position, uint8_t speed); * @param speed speed of the movement. */ void -asserv_move_motor1_absolute (uint16_t position, uint8_t speed); +asserv_move_motor1_absolute (uint16_t position, uint16_t speed); #endif /* AC_ASSERV_AUX_NB */ @@ -271,8 +271,8 @@ asserv_set_angle_position (int16_t angle); * @param angular_low angular low speed */ void -asserv_set_speed (uint8_t linear_high, uint8_t angular_high, - uint8_t linear_low, uint8_t angular_low); +asserv_set_speed (uint16_t linear_high, uint16_t angular_high, + uint16_t linear_low, uint16_t angular_low); /** * Set the complete position of the bot. @@ -300,11 +300,11 @@ asserv_goto (uint32_t x, uint32_t y, uint8_t backward); /** Reset the motor0 to the zero position. */ void -asserv_motor0_zero_position (int8_t speed); +asserv_motor0_zero_position (int16_t speed); /** Reset the motor1 to the zero position. */ void -asserv_motor1_zero_position (int8_t speed); +asserv_motor1_zero_position (int16_t speed); /** Set PWM to zero for motor0. */ void diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index 8df84538..cc66a280 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -148,89 +148,95 @@ mimot_set_motor1_position (uint16_t position) } void -mimot_move_motor0_absolute (uint16_t position, uint8_t speed) +mimot_move_motor0_absolute (uint16_t position, uint16_t speed) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'b'; buffer[1] = v16_to_v8 (position, 1); buffer[2] = v16_to_v8 (position, 0); - buffer[3] = speed; - twi_master_send_buffer (4); + buffer[3] = v16_to_v8 (speed, 1); + buffer[4] = v16_to_v8 (speed, 0); + twi_master_send_buffer (5); } void -mimot_move_motor1_absolute (uint16_t position, uint8_t speed) +mimot_move_motor1_absolute (uint16_t position, uint16_t speed) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'c'; buffer[1] = v16_to_v8 (position, 1); buffer[2] = v16_to_v8 (position, 0); - buffer[3] = speed; - twi_master_send_buffer (4); + buffer[3] = v16_to_v8 (speed, 1); + buffer[4] = v16_to_v8 (speed, 0); + twi_master_send_buffer (5); } void -mimot_motor0_zero_position (int8_t speed) +mimot_motor0_zero_position (int16_t speed) { mimot_motor0_find_zero (speed, 0, 0); } void -mimot_motor1_zero_position (int8_t speed) +mimot_motor1_zero_position (int16_t speed) { mimot_motor1_find_zero (speed, 0, 0); } void -mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, +mimot_motor0_find_zero (int16_t speed, uint8_t use_switch, uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'B'; buffer[1] = 0; - buffer[2] = speed; - buffer[3] = use_switch; - buffer[4] = v16_to_v8 (reset_position, 1); - buffer[5] = v16_to_v8 (reset_position, 0); - twi_master_send_buffer (6); + buffer[2] = v16_to_v8 (speed, 1); + buffer[3] = v16_to_v8 (speed, 0); + buffer[4] = use_switch; + buffer[5] = v16_to_v8 (reset_position, 1); + buffer[6] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (7); } void -mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, +mimot_motor1_find_zero (int16_t speed, uint8_t use_switch, uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'B'; buffer[1] = 1; - buffer[2] = speed; - buffer[3] = use_switch; - buffer[4] = v16_to_v8 (reset_position, 1); - buffer[5] = v16_to_v8 (reset_position, 0); - twi_master_send_buffer (6); + buffer[2] = v16_to_v8 (speed, 1); + buffer[3] = v16_to_v8 (speed, 0); + buffer[4] = use_switch; + buffer[5] = v16_to_v8 (reset_position, 1); + buffer[6] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (7); } void -mimot_motor0_clamp (int8_t speed, int16_t pwm) +mimot_motor0_clamp (int16_t speed, int16_t pwm) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'l'; buffer[1] = 0; - buffer[2] = speed; - buffer[3] = v16_to_v8 (pwm, 1); - buffer[4] = v16_to_v8 (pwm, 0); - twi_master_send_buffer (5); + buffer[2] = v16_to_v8 (speed, 1); + buffer[3] = v16_to_v8 (speed, 0); + buffer[4] = v16_to_v8 (pwm, 1); + buffer[5] = v16_to_v8 (pwm, 0); + twi_master_send_buffer (6); } void -mimot_motor1_clamp (int8_t speed, int16_t pwm) +mimot_motor1_clamp (int16_t speed, int16_t pwm) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'l'; buffer[1] = 1; - buffer[2] = speed; - buffer[3] = v16_to_v8 (pwm, 1); - buffer[4] = v16_to_v8 (pwm, 0); - twi_master_send_buffer (5); + buffer[2] = v16_to_v8 (speed, 1); + buffer[3] = v16_to_v8 (speed, 0); + buffer[4] = v16_to_v8 (pwm, 1); + buffer[5] = v16_to_v8 (pwm, 0); + twi_master_send_buffer (6); } void diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index c9de7090..35b2dfbe 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -82,37 +82,37 @@ mimot_set_motor1_position (uint16_t position); /** Move motor0 to absolute position in steps. */ void -mimot_move_motor0_absolute (uint16_t position, uint8_t speed); +mimot_move_motor0_absolute (uint16_t position, uint16_t speed); /** Move motor1 to absolute position in steps. */ void -mimot_move_motor1_absolute (uint16_t position, uint8_t speed); +mimot_move_motor1_absolute (uint16_t position, uint16_t speed); /** Reset motor0 to zero position. */ void -mimot_motor0_zero_position (int8_t speed); +mimot_motor0_zero_position (int16_t speed); /** Reset motor1 to zero position. */ void -mimot_motor1_zero_position (int8_t speed); +mimot_motor1_zero_position (int16_t speed); /** Find zero position. */ void -mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, +mimot_motor0_find_zero (int16_t speed, uint8_t use_switch, uint16_t reset_position); /** Find zero position. */ void -mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, +mimot_motor1_find_zero (int16_t speed, uint8_t use_switch, uint16_t reset_position); /** Clamp motor0. */ void -mimot_motor0_clamp (int8_t speed, int16_t pwm); +mimot_motor0_clamp (int16_t speed, int16_t pwm); /** Clamp motor1. */ void -mimot_motor1_clamp (int8_t speed, int16_t pwm); +mimot_motor1_clamp (int16_t speed, int16_t pwm); /** Free motor0. */ void 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'], diff --git a/digital/avr/modules/motor/speed_control/speed_control.c b/digital/avr/modules/motor/speed_control/speed_control.c index 2caf0937..c13b4614 100644 --- a/digital/avr/modules/motor/speed_control/speed_control.c +++ b/digital/avr/modules/motor/speed_control/speed_control.c @@ -33,44 +33,45 @@ static void speed_control_update_by_speed (speed_control_t *speed_control) { /* Update current speed (be careful of overflow!). */ - if (speed_control->cons > speed_control->cur) + if (speed_control->cons_f > speed_control->cur_f) { - if ((uint16_t) (speed_control->cons - speed_control->cur) - < (uint16_t) speed_control->acc) - speed_control->cur = speed_control->cons; + if ((uint32_t) (speed_control->cons_f - speed_control->cur_f) + < (uint32_t) speed_control->acc_f) + speed_control->cur_f = speed_control->cons_f; else - speed_control->cur += speed_control->acc; + speed_control->cur_f += speed_control->acc_f; } else { - if ((uint16_t) (speed_control->cur - speed_control->cons) - < (uint16_t) speed_control->acc) - speed_control->cur = speed_control->cons; + if ((uint32_t) (speed_control->cur_f - speed_control->cons_f) + < (uint32_t) speed_control->acc_f) + speed_control->cur_f = speed_control->cons_f; else - speed_control->cur -= speed_control->acc; + speed_control->cur_f -= speed_control->acc_f; } } /** Compute maximum allowed speed according to: distance left, maximum speed, * current speed and acceleration. */ -static int16_t -speed_control_compute_max_speed (int32_t d, int16_t cur, int16_t acc, - int8_t max) +static int32_t +speed_control_compute_max_speed_f (int32_t d, int32_t cur_f, int16_t acc_f, + int16_t max) { int16_t s; + int32_t s_f; /* Compute maximum speed in order to be able to brake in time. * The "+ 0xff" is to ceil result. * s = sqrt (2 * a * d) */ - s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8); + s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc_f + 0xff) >> 8); /* Apply consign. */ s = UTILS_MIN (max, s); /* Apply sign. */ if (d < 0) s = -s; /* Convert to f8.8 and check acceleration. */ - s = s << 8; - UTILS_BOUND (s, cur - acc, cur + acc); - return s; + s_f = (int32_t) s << 8; + UTILS_BOUND (s_f, cur_f - acc_f, cur_f + acc_f); + return s_f; } /** Update current speed according to a position consign. */ @@ -79,8 +80,9 @@ speed_control_update_by_position (speed_control_t *speed_control, pos_control_t *pos_control) { int32_t diff = speed_control->pos_cons - pos_control->cons; - speed_control->cur = speed_control_compute_max_speed - (diff, speed_control->cur, speed_control->acc, speed_control->max); + speed_control->cur_f = speed_control_compute_max_speed_f + (diff, speed_control->cur_f, speed_control->acc_f, + speed_control->max); } void @@ -103,17 +105,17 @@ speed_control_update (speed_control_t *speed_control, else speed_control_update_by_speed (speed_control); /* Update shaft position. */ - speed_control->pos_control->cons += speed_control->cur >> 8; + speed_control->pos_control->cons += speed_control->cur_f >> 8; } else - speed_control->cur = 0; + speed_control->cur_f = 0; } void -speed_control_set_speed (speed_control_t *speed_control, int8_t speed) +speed_control_set_speed (speed_control_t *speed_control, int16_t speed) { speed_control->use_pos = 0; - speed_control->cons = speed << 8; + speed_control->cons_f = speed << 8; } void @@ -143,8 +145,8 @@ speed_control_hard_stop (speed_control_t *speed_control) { /* No future movement. */ speed_control->use_pos = 0; - speed_control->cur = 0; - speed_control->cons = 0; + speed_control->cur_f = 0; + speed_control->cons_f = 0; /* Really stop right here, position control on the current point. */ speed_control->pos_control->cons = speed_control->pos_control->cur; } diff --git a/digital/avr/modules/motor/speed_control/speed_control.h b/digital/avr/modules/motor/speed_control/speed_control.h index 14e1ef15..714f3d54 100644 --- a/digital/avr/modules/motor/speed_control/speed_control.h +++ b/digital/avr/modules/motor/speed_control/speed_control.h @@ -30,20 +30,20 @@ /** Speed control state. */ struct speed_control_t { - /** Current speed, f8.8. */ - int16_t cur; + /** Current speed, f16.8. */ + int32_t cur_f; /** Whether to use the consign position (1) or not (0). */ uint8_t use_pos; - /** Consign speed, f8.8. */ - int16_t cons; + /** Consign speed, f16.8. */ + int32_t cons_f; /** Consign position. */ uint32_t pos_cons; - /** Maximum speed for position consign, u7. */ - int8_t max; - /** Slow speed, deprecated, u7. */ - int8_t slow; + /** Maximum speed for position consign, u15. */ + int16_t max; + /** Slow speed, deprecated, u15. */ + int16_t slow; /** Acceleration, f8.8. */ - int16_t acc; + int16_t acc_f; /** Associated position control, to simplify function prototypes. */ pos_control_t *pos_control; }; @@ -61,7 +61,7 @@ speed_control_update (speed_control_t *speed_control, /** Set speed consign. Accelerate to the given speed. */ void -speed_control_set_speed (speed_control_t *speed_control, int8_t speed); +speed_control_set_speed (speed_control_t *speed_control, int16_t speed); /** Set position consign offset. Move to a position measured from the current * controlled position (which may be different from the actual position), diff --git a/digital/avr/modules/motor/speed_control/speed_control.txt b/digital/avr/modules/motor/speed_control/speed_control.txt index 94b4d0b4..2cdebb46 100644 --- a/digital/avr/modules/motor/speed_control/speed_control.txt +++ b/digital/avr/modules/motor/speed_control/speed_control.txt @@ -13,7 +13,7 @@ too high. Usage ===== -The `max` and `acc` parameters should be set to maximum speed and maximum +The `max` and `acc_f` parameters should be set to maximum speed and maximum acceleration. Other fields should better be changed using the helper functions. 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