From 4895e54c666db9720abda0aa8ae901058679aee3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 18 Dec 2011 15:19:06 +0100 Subject: digital/asserv: convert to new control system --- digital/asserv/src/asserv/main.c | 353 ++++++++++++++++++++------------------- 1 file changed, 179 insertions(+), 174 deletions(-) (limited to 'digital/asserv/src/asserv/main.c') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 677fea84..5d91d90e 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -32,16 +32,14 @@ #include "io.h" #include "misc.h" -#include "state.h" -#include "counter.h" -#include "pwm.h" -#include "pos.h" -#include "speed.h" +#include "cs.h" #include "postrack.h" #include "traj.h" #include "aux.h" +#include "seq.h" + #include "twi_proto.h" #include "eeprom.h" @@ -105,13 +103,11 @@ main (int argc, char **argv) PORTF = 0xfc; PORTG = 0x18; LED_SETUP; - pwm_init (); timer_init (); - counter_init (); uart0_init (); twi_proto_init (); postrack_init (); - speed_init (); + cs_init (); traj_init (); aux_init (); eeprom_read_params (); @@ -126,43 +122,49 @@ main (int argc, char **argv) static void main_loop (void) { + main_timer[3] = timer_read (); + /* Compute absolute position. */ + postrack_update (); + aux_pos_update (); + main_timer[4] = timer_read (); + /* Compute trajectory. */ + traj_update (); + aux_traj_update (); + /* Prepare control system. */ + cs_update_prepare (); main_timer[5] = timer_read (); + /* Wait for next cycle. */ timer_wait (); - /* Counter update. */ - counter_update (); + /* Encoder update. */ + encoder_update (); + encoder_corrector_update (&encoder_right_corrector, &encoder_right); main_timer[0] = timer_read (); - /* Position control. */ - pos_update (); + /* Control system update. */ + cs_update (); main_timer[1] = timer_read (); /* Pwm setup. */ - pwm_update (); + output_update (); main_timer[2] = timer_read (); - /* Compute absolute position. */ - postrack_update (); - aux_pos_update (); - /* Compute trajectory. */ - if (state_main.mode >= MODE_TRAJ) - traj_update (); - aux_traj_update (); - /* Speed control. */ - speed_update (); - main_timer[3] = timer_read (); + /* Sequences. */ + seq_update (&seq_main, &cs_main.state); + seq_update (&seq_aux[0], &cs_aux[0].state); + seq_update (&seq_aux[1], &cs_aux[1].state); /* Stats. */ if (main_sequence_ack - && (state_main.sequence_ack != state_main.sequence_finish - || state_aux[0].sequence_ack != state_aux[0].sequence_finish - || state_aux[1].sequence_ack != state_aux[1].sequence_finish) + && (seq_main.ack != seq_main.finish + || seq_aux[0].ack != seq_aux[0].finish + || seq_aux[1].ack != seq_aux[1].finish) && !--main_sequence_ack_cpt) { - proto_send3b ('A', state_main.sequence_finish, - state_aux[0].sequence_finish, - state_aux[1].sequence_finish); + proto_send3b ('A', seq_main.finish, + seq_aux[0].finish, + seq_aux[1].finish); main_sequence_ack_cpt = main_sequence_ack; } if (main_stat_counter && !--main_stat_counter_cpt) { - proto_send4w ('C', counter_left, counter_right, - counter_aux[0], counter_aux[1]); + proto_send4w ('C', encoder_left.cur, encoder_right.cur, + encoder_aux[0].cur, encoder_aux[1].cur); main_stat_counter_cpt = main_stat_counter; } if (main_stat_postrack && !--main_stat_postrack_cpt) @@ -177,20 +179,26 @@ main_loop (void) } if (main_stat_speed && !--main_stat_speed_cpt) { - proto_send4b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8, - speed_aux[0].cur >> 8, speed_aux[1].cur >> 8); + proto_send4b ('S', cs_main.speed_theta.cur >> 8, + cs_main.speed_alpha.cur >> 8, + cs_aux[0].speed.cur >> 8, + cs_aux[1].speed.cur >> 8); main_stat_speed_cpt = main_stat_speed; } if (main_stat_pos && !--main_stat_pos_cpt) { - proto_send4w ('P', pos_theta.e_old, pos_theta.i, - pos_alpha.e_old, pos_alpha.i); + proto_send4w ('P', cs_main.pos_theta.last_error, + cs_main.pos_theta.i, + cs_main.pos_alpha.last_error, + cs_main.pos_alpha.i); main_stat_pos_cpt = main_stat_pos; } if (main_stat_pos_aux && !--main_stat_pos_aux_cpt) { - proto_send4w ('Q', pos_aux[0].e_old, pos_aux[0].i, - pos_aux[1].e_old, pos_aux[1].i); + proto_send4w ('Q', cs_aux[0].pos.last_error, + cs_aux[0].pos.i, + cs_aux[1].pos.last_error, + cs_aux[1].pos.i); main_stat_pos_aux_cpt = main_stat_pos_aux; } #ifdef HOST @@ -204,8 +212,8 @@ main_loop (void) #endif /* HOST */ if (main_stat_pwm && !--main_stat_pwm_cpt) { - proto_send4w ('W', pwm_left.cur, pwm_right.cur, - pwm_aux[0].cur, pwm_aux[1].cur); + proto_send4w ('W', output_left.cur, output_right.cur, + output_aux[0].cur, output_aux[1].cur); main_stat_pwm_cpt = main_stat_pwm; } if (main_stat_timer && !--main_stat_timer_cpt) @@ -232,17 +240,20 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) { /* Many commands use the first argument as a selector. */ struct aux_t *auxp = 0; - struct pwm_t *pwm = 0; - struct pos_t *pos = 0; - struct speed_t *speed = 0; - struct state_t *state = 0; + pos_control_t *pos = 0; + speed_control_t *speed = 0; + control_state_t *state = 0; + blocking_detection_t *bd = 0; + output_t *output = 0; + seq_t *seq = 0; if (args[0] < AC_ASSERV_AUX_NB) { auxp = &aux[args[0]]; - pwm = &pwm_aux[args[0]]; - pos = &pos_aux[args[0]]; - speed = &speed_aux[args[0]]; - state = &state_aux[args[0]]; + pos = &cs_aux[args[0]].pos; + speed = &cs_aux[args[0]].speed; + state = &cs_aux[args[0]].state; + output = &output_aux[args[0]]; + seq = &seq_aux[args[0]]; } /* Decode command. */ #define c(cmd, size) (cmd << 8 | size) @@ -255,120 +266,105 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Commands. */ case c ('w', 0): /* Set zero pwm. */ - pos_reset (&pos_theta); - pos_reset (&pos_alpha); - state_main.mode = MODE_PWM; - pwm_set (&pwm_left, 0); - pwm_set (&pwm_right, 0); + output_set (&output_left, 0); + output_set (&output_right, 0); + control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0); break; case c ('W', 0): /* Set zero auxiliary pwm. */ - pos_reset (&pos_aux[0]); - pos_reset (&pos_aux[1]); - state_aux[0].mode = MODE_PWM; - state_aux[1].mode = MODE_PWM; - pwm_set (&pwm_aux[0], 0); - pwm_set (&pwm_aux[1], 0); + output_set (&output_aux[0], 0); + output_set (&output_aux[1], 0); + control_state_set_mode (&cs_aux[0].state, CS_MODE_NONE, 0); + control_state_set_mode (&cs_aux[1].state, CS_MODE_NONE, 0); break; case c ('w', 4): /* Set pwm. * - w: left pwm. * - w: right pwm. */ - pos_reset (&pos_theta); - pos_reset (&pos_alpha); - state_main.mode = MODE_PWM; - pwm_set (&pwm_left, v8_to_v16 (args[0], args[1])); - pwm_set (&pwm_right, v8_to_v16 (args[2], args[3])); + output_set (&output_left, v8_to_v16 (args[0], args[1])); + output_set (&output_right, v8_to_v16 (args[2], args[3])); + control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0); break; case c ('W', 3): /* Set auxiliary pwm. * - b: aux index. * - w: pwm. */ if (!auxp) { proto_send0 ('?'); return; } - pos_reset (pos); - state->mode = MODE_PWM; - pwm_set (pwm, v8_to_v16 (args[1], args[2])); + output_set (output, v8_to_v16 (args[1], args[2])); + control_state_set_mode (state, CS_MODE_NONE, 0); break; case c ('c', 4): /* Add to position consign. * - w: theta consign offset. * - w: alpha consign offset. */ - state_main.mode = MODE_POS; - state_main.variant = 0; - pos_theta.cons += v8_to_v16 (args[0], args[1]); - pos_alpha.cons += v8_to_v16 (args[2], args[3]); + cs_main.pos_theta.cons += v8_to_v16 (args[0], args[1]); + cs_main.pos_alpha.cons += v8_to_v16 (args[2], args[3]); + control_state_set_mode (&cs_main.state, CS_MODE_POS_CONTROL, 0); break; case c ('C', 3): /* Add to auxiliary position consign. * - b: aux index. * - w: consign offset. */ if (!auxp) { proto_send0 ('?'); return; } - state->mode = MODE_POS; - state->variant = 0; pos->cons += v8_to_v16 (args[1], args[2]); + control_state_set_mode (state, CS_MODE_POS_CONTROL, 0); break; case c ('s', 0): /* Stop (set zero speed). */ - state_main.mode = MODE_SPEED; - state_main.variant = 0; - speed_theta.use_pos = speed_alpha.use_pos = 0; - speed_theta.cons = 0; - speed_alpha.cons = 0; + speed_control_set_speed (&cs_main.speed_theta, 0); + 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): /* Set speed. * - b: theta speed. * - b: alpha speed. */ - state_main.mode = MODE_SPEED; - state_main.variant = 0; - speed_theta.use_pos = speed_alpha.use_pos = 0; - speed_theta.cons = args[0] << 8; - speed_alpha.cons = args[1] << 8; + speed_control_set_speed (&cs_main.speed_theta, args[0]); + speed_control_set_speed (&cs_main.speed_alpha, args[1]); + control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0); break; case c ('S', 2): /* Set auxiliary speed. * - b: aux index. * - b: speed. */ if (!auxp) { proto_send0 ('?'); return; } - state->mode = MODE_SPEED; - state->variant = 0; - speed->use_pos = 0; - speed->cons = args[1] << 8; + speed_control_set_speed (speed, args[1]); + control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0); break; case c ('s', 9): /* Set speed controlled position consign. * - d: theta consign offset. * - d: alpha consign offset. * - b: sequence number. */ - if (args[8] == state_main.sequence) + if (!seq_start (&seq_main, args[8])) break; - speed_theta.use_pos = speed_alpha.use_pos = 1; - speed_theta.pos_cons = pos_theta.cons; - speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); - speed_alpha.pos_cons = pos_alpha.cons; - speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]); - state_start (&state_main, MODE_SPEED, args[8]); + speed_control_pos_offset (&cs_main.speed_theta, + v8_to_v32 (args[0], args[1], args[2], + args[3])); + speed_control_pos_offset (&cs_main.speed_alpha, + v8_to_v32 (args[4], args[5], args[6], + args[7])); + control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0); break; case c ('l', 5): /* Set linear speed controlled position consign. * - d: consign offset. * - b: sequence number. */ - if (args[4] == state_main.sequence) + if (!seq_start (&seq_main, args[4])) break; - speed_theta.use_pos = speed_alpha.use_pos = 1; - speed_theta.pos_cons = pos_theta.cons; - speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); - speed_alpha.pos_cons = pos_alpha.cons; - state_start (&state_main, MODE_SPEED, args[4]); + speed_control_pos_offset (&cs_main.speed_theta, + v8_to_v32 (args[0], args[1], args[2], args[3])); + speed_control_pos_offset (&cs_main.speed_alpha, 0); + traj_speed_start (); break; case c ('a', 5): /* Set angular speed controlled position consign. * - d: angle offset. * - b: sequence number. */ - if (args[4] == state_main.sequence) + if (!seq_start (&seq_main, args[4])) break; traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2], - args[3]), args[4]); + args[3])); break; case c ('S', 6): /* Set auxiliary speed controlled position consign. @@ -376,29 +372,28 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - d: consign offset. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (args[5] == state->sequence) + if (!seq_start (seq, args[5])) break; - speed->use_pos = 1; - speed->pos_cons = pos->cons; - speed->pos_cons += v8_to_v32 (args[1], args[2], args[3], args[4]); - state_start (state, MODE_SPEED, args[5]); + speed_control_pos_offset (speed, v8_to_v32 (args[1], args[2], args[3], + args[4])); + aux_traj_speed_start (auxp); break; case c ('f', 2): /* Go to the wall. * - b: 0: forward, 1: backward. * - b: sequence number. */ - if (args[1] == state_main.sequence) + if (!seq_start (&seq_main, args[1])) break; - traj_ftw_start (args[0], args[1]); + traj_ftw_start (args[0]); break; case c ('f', 3): /* Go to the wall, using center with a delay. * - b: 0: forward, 1: backward. * - b: delay. * - b: sequence number. */ - if (args[1] == state_main.sequence) + if (!seq_start (&seq_main, args[2])) break; - traj_ftw_start_center (args[0], args[1], args[2]); + traj_ftw_start_center (args[0], args[1]); break; case c ('f', 12): /* Push the wall. @@ -408,7 +403,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: init_a, f0.16. * - b: sequence number. */ { - if (args[11] == state_main.sequence) + if (!seq_start (&seq_main, args[11])) break; int32_t angle; if (args[9] == 0xff && args[10] == 0xff) @@ -418,15 +413,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) traj_ptw_start (args[0], v8_to_v32 (args[1], args[2], args[3], args[4]), v8_to_v32 (args[5], args[6], args[7], args[8]), - angle, args[11]); + angle); } break; case c ('F', 1): /* Go to the dispenser. * - b: sequence number. */ - if (args[0] == state_main.sequence) + if (!seq_start (&seq_main, args[0])) break; - traj_gtd_start (args[0]); + traj_gtd_start (); break; case c ('x', 10): /* Go to position. @@ -434,20 +429,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - d: y, f24.8. * - b: backward (see traj.h). * - b: sequence number. */ - if (args[9] == state_main.sequence) + if (!seq_start (&seq_main, args[9])) break; traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]), v8_to_v32 (args[4], args[5], args[6], args[7]), - args[8], args[9]); + args[8]); break; case c ('x', 3): /* Go to angle. * - d: a, f0.16. * - b: sequence number. */ - if (args[2] == state_main.sequence) + if (!seq_start (&seq_main, args[2])) break; - traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0), - args[2]); + traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0)); break; case c ('x', 12): /* Go to position, then angle. @@ -456,12 +450,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: a, f0.16. * - b: backward (see traj.h). * - b: sequence number. */ - if (args[11] == state_main.sequence) + if (!seq_start (&seq_main, args[11])) break; traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]), v8_to_v32 (args[4], args[5], args[6], args[7]), v8_to_v32 (0, args[8], args[9], 0), - args[10], args[11]); + args[10]); break; case c ('y', 4): /* Auxiliary go to position. @@ -469,9 +463,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: pos, i16. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (args[3] == state->sequence) + if (!seq_start (seq, args[3])) break; - aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]); + aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2])); break; case c ('y', 3): /* Auxiliary find zero. @@ -479,25 +473,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: speed. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (args[2] == state->sequence) + if (!seq_start (seq, args[2])) break; if (args[0] == 0) - aux_traj_find_limit_start (auxp, args[1], args[2]); + aux_traj_find_limit_start (auxp, args[1]); else - aux_traj_find_zero_reverse_start (auxp, args[1], args[2]); + aux_traj_find_zero_reverse_start (auxp, args[1]); break; case c ('a', 3): /* Set all acknoledge. * - b: main ack sequence number. * - b: first auxiliary ack sequence number. * - b: second auxiliary ack sequence number. */ - state_acknowledge (&state_aux[0], args[1]); - state_acknowledge (&state_aux[1], args[2]); + seq_acknowledge (&seq_aux[0], args[1]); + seq_acknowledge (&seq_aux[1], args[2]); /* no break; */ case c ('a', 1): /* Set main acknoledge. * - b: main ack sequence number. */ - state_acknowledge (&state_main, args[0]); + seq_acknowledge (&seq_main, args[0]); break; /* Stats. * - b: interval between stats. */ @@ -555,21 +549,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) switch (args[1]) { case 't': - pos = &pos_theta; - speed = &speed_theta; + pos = &cs_main.pos_theta; + speed = &cs_main.speed_theta; + bd = &cs_main.blocking_detection_theta; break; case 'a': - pos = &pos_alpha; - speed = &speed_alpha; + pos = &cs_main.pos_alpha; + speed = &cs_main.speed_alpha; + bd = &cs_main.blocking_detection_alpha; break; case 0: case 1: - pos = &pos_aux[args[1]]; - speed = &speed_aux[args[1]]; + pos = &cs_aux[args[1]].pos; + speed = &cs_aux[args[1]].speed; + bd = &cs_aux[args[1]].blocking_detection; break; default: pos = 0; speed = 0; + bd = 0; break; } switch (c (args[0], size)) @@ -598,8 +596,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('c', 5): /* Set right counter correction factor. * - d: factor (f8.24). */ - counter_right_correction = v8_to_v32 (args[1], args[2], - args[3], args[4]); + encoder_corrector_set_correction (&encoder_right_corrector, + v8_to_v32 (args[1], args[2], + args[3], args[4])); break; case c ('f', 3): /* Set footing. @@ -649,19 +648,31 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: error limit. * - w: speed limit. * - b: counter limit. */ - if (!pos) { proto_send0 ('?'); return; } - pos->blocked_error_limit = v8_to_v16 (args[2], args[3]); - pos->blocked_speed_limit = v8_to_v16 (args[4], args[5]); - pos->blocked_counter_limit = args[6]; + if (!bd) { proto_send0 ('?'); return; } + bd->error_limit = v8_to_v16 (args[2], args[3]); + bd->speed_limit = v8_to_v16 (args[4], args[5]); + bd->counter_limit = args[6]; break; case c ('E', 3): - pos_e_sat = v8_to_v16 (args[1], args[2]); + cs_main.pos_theta.e_sat = + cs_main.pos_alpha.e_sat = + cs_aux[0].pos.e_sat = + cs_aux[1].pos.e_sat = + v8_to_v16 (args[1], args[2]); break; case c ('I', 3): - pos_i_sat = v8_to_v16 (args[1], args[2]); + cs_main.pos_theta.i_sat = + cs_main.pos_alpha.i_sat = + cs_aux[0].pos.i_sat = + cs_aux[1].pos.i_sat = + v8_to_v16 (args[1], args[2]); break; case c ('D', 3): - pos_d_sat = v8_to_v16 (args[1], args[2]); + cs_main.pos_theta.d_sat = + cs_main.pos_alpha.d_sat = + cs_aux[0].pos.d_sat = + cs_aux[1].pos.d_sat = + v8_to_v16 (args[1], args[2]); break; case c ('e', 5): traj_eps = v8_to_v16 (args[1], args[2]); @@ -673,7 +684,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('w', 2): /* Set PWM direction. * - b: bits: 0000[aux1][aux0][right][left]. */ - pwm_set_reverse (args[1]); + output_set_reverse (&output_left, (args[1] & 1) ? 1 : 0); + output_set_reverse (&output_right, (args[1] & 2) ? 1 : 0); + output_set_reverse (&output_aux[0], (args[1] & 4) ? 1 : 0); + output_set_reverse (&output_aux[1], (args[1] & 8) ? 1 : 0); break; case c ('E', 2): /* Write to eeprom. @@ -686,38 +700,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('P', 1): /* Print current settings. */ proto_send1b ('E', EEPROM_KEY); - proto_send1d ('c', counter_right_correction); + proto_send1d ('c', encoder_right_corrector.correction); proto_send1w ('f', postrack_footing); - proto_send2w ('a', speed_theta.acc, speed_alpha.acc); - proto_send2b ('s', speed_theta.max, speed_theta.slow); - proto_send2b ('s', speed_alpha.max, speed_alpha.slow); - proto_send3w ('b', pos_theta.blocked_error_limit, - pos_theta.blocked_speed_limit, - pos_theta.blocked_counter_limit); - proto_send3w ('b', pos_alpha.blocked_error_limit, - pos_alpha.blocked_speed_limit, - pos_alpha.blocked_counter_limit); - proto_send2w ('p', pos_theta.kp, pos_alpha.kp); - proto_send2w ('i', pos_theta.ki, pos_alpha.ki); - proto_send2w ('d', pos_theta.kd, pos_alpha.kd); - proto_send2w ('a', speed_aux[0].acc, speed_aux[1].acc); - proto_send2b ('s', speed_aux[0].max, speed_aux[0].slow); - proto_send2b ('s', speed_aux[1].max, speed_aux[1].slow); - proto_send3w ('b', pos_aux[0].blocked_error_limit, - pos_aux[0].blocked_speed_limit, - pos_aux[0].blocked_counter_limit); - proto_send3w ('b', pos_aux[1].blocked_error_limit, - pos_aux[1].blocked_speed_limit, - pos_aux[1].blocked_counter_limit); - proto_send2w ('p', pos_aux[0].kp, pos_aux[1].kp); - proto_send2w ('i', pos_aux[0].ki, pos_aux[1].ki); - proto_send2w ('d', pos_aux[0].kd, pos_aux[1].kd); - proto_send1w ('E', pos_e_sat); - proto_send1w ('I', pos_i_sat); - proto_send1w ('D', pos_d_sat); proto_send2w ('e', traj_eps, traj_aeps); proto_send1w ('l', traj_angle_limit); - proto_send1b ('w', pwm_reverse); + proto_send1b ('w', (output_left.reverse ? 1 : 0) + | (output_right.reverse ? 2 : 0) + | (output_aux[0].reverse ? 4 : 0) + | (output_aux[1].reverse ? 8 : 0)); + break; + case c ('P', 2): + /* Print current settings for selected control. + * - b: index. */ + proto_send1b ('E', EEPROM_KEY); + proto_send1w ('a', speed->acc); + proto_send2b ('s', speed->max, speed->slow); + proto_send3w ('b', bd->error_limit, bd->speed_limit, + bd->counter_limit); + proto_send1w ('p', pos->kp); + proto_send1w ('i', pos->ki); + proto_send1w ('d', pos->kd); + proto_send1w ('E', pos->e_sat); + proto_send1w ('I', pos->i_sat); + proto_send1w ('D', pos->d_sat); break; default: proto_send0 ('?'); -- cgit v1.2.3