From 1431438a2b0f3fafbafb61a4a15296164dadf5e1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Apr 2009 16:22:53 +0200 Subject: * digital/asserv: - added second auxiliary motor support. --- digital/asserv/src/asserv/main.c | 254 ++++++++++++++++++++++++--------------- 1 file changed, 158 insertions(+), 96 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 f6e07126..647d8777 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -69,6 +69,9 @@ uint8_t main_stat_speed, main_stat_speed_cpt; /** Statistics about shaft position control. */ uint8_t main_stat_pos, main_stat_pos_cpt; +/** Statistics about auxiliary shaft position control. */ +uint8_t main_stat_pos_aux, main_stat_pos_aux_cpt; + /** Statistics about pwm values. */ uint8_t main_stat_pwm, main_stat_pwm_cpt; @@ -147,16 +150,19 @@ main_loop (void) /* Stats. */ if (main_sequence_ack && (state_main.sequence_ack != state_main.sequence_finish - || state_aux0.sequence_ack != state_aux0.sequence_finish) + || state_aux[0].sequence_ack != state_aux[0].sequence_finish + || state_aux[1].sequence_ack != state_aux[1].sequence_finish) && !--main_sequence_ack_cpt) { - proto_send2b ('A', state_main.sequence_finish, - state_aux0.sequence_finish); + proto_send3b ('A', state_main.sequence_finish, + state_aux[0].sequence_finish, + state_aux[1].sequence_finish); main_sequence_ack_cpt = main_sequence_ack; } if (main_stat_counter && !--main_stat_counter_cpt) { - proto_send3w ('C', counter_left, counter_right, counter_aux0); + proto_send4w ('C', counter_left, counter_right, + counter_aux[0], counter_aux[1]); main_stat_counter_cpt = main_stat_counter; } if (main_stat_postrack && !--main_stat_postrack_cpt) @@ -166,22 +172,27 @@ main_loop (void) } if (main_stat_aux_pos && !--main_stat_aux_pos_cpt) { - proto_send1w ('Y', aux0.pos); + proto_send2w ('Y', aux[0].pos, aux[1].pos); main_stat_aux_pos_cpt = main_stat_aux_pos; } if (main_stat_speed && !--main_stat_speed_cpt) { - proto_send3b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8, - speed_aux0.cur >> 8); + proto_send4b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8, + speed_aux[0].cur >> 8, speed_aux[1].cur >> 8); main_stat_speed_cpt = main_stat_speed; } if (main_stat_pos && !--main_stat_pos_cpt) { - proto_send6w ('P', pos_theta.e_old, pos_theta.i, - pos_alpha.e_old, pos_alpha.i, - pos_aux0.e_old, pos_aux0.i); + proto_send4w ('P', pos_theta.e_old, pos_theta.i, + pos_alpha.e_old, 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); + main_stat_pos_aux_cpt = main_stat_pos_aux; + } #ifdef HOST if (main_simu && !--main_simu_cpt) { @@ -193,7 +204,8 @@ main_loop (void) #endif /* HOST */ if (main_stat_pwm && !--main_stat_pwm_cpt) { - proto_send3w ('W', pwm_left.cur, pwm_right.cur, pwm_aux0.cur); + proto_send4w ('W', pwm_left.cur, pwm_right.cur, + pwm_aux[0].cur, pwm_aux[1].cur); main_stat_pwm_cpt = main_stat_pwm; } if (main_stat_timer && !--main_stat_timer_cpt) @@ -218,6 +230,21 @@ main_loop (void) void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) { + /* Many commands use the first argument as a selector. */ + struct aux_t *aux = 0; + struct pwm_t *pwm = 0; + struct pos_t *pos = 0; + struct speed_t *speed = 0; + struct state_t *state = 0; + if (args[0] < AC_ASSERV_AUX_NB) + { + aux = &aux[args[0]]; + pwm = &pwm_aux[args[0]]; + pos = &pos_aux[args[0]]; + speed = &speed_aux[args[0]]; + state = &state_aux[args[0]]; + } + /* Decode command. */ #define c(cmd, size) (cmd << 8 | size) switch (c (cmd, size)) { @@ -234,6 +261,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_set (&pwm_left, 0); pwm_set (&pwm_right, 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); + break; case c ('w', 4): /* Set pwm. * - w: left pwm. @@ -244,12 +280,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_set (&pwm_left, v8_to_v16 (args[0], args[1])); pwm_set (&pwm_right, v8_to_v16 (args[2], args[3])); break; - case c ('w', 2): + case c ('W', 3): /* Set auxiliary pwm. + * - b: aux index. * - w: pwm. */ - pos_reset (&pos_aux0); - state_aux0.mode = MODE_PWM; - pwm_set (&pwm_aux0, v8_to_v16 (args[0], args[1])); + if (!aux) { proto_send0 ('?'); return; } + pos_reset (pos); + state->mode = MODE_PWM; + pwm_set (pwm, v8_to_v16 (args[1], args[2])); break; case c ('c', 4): /* Add to position consign. @@ -260,12 +298,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pos_theta.cons += v8_to_v16 (args[0], args[1]); pos_alpha.cons += v8_to_v16 (args[2], args[3]); break; - case c ('c', 2): + case c ('C', 3): /* Add to auxiliary position consign. + * - b: aux index. * - w: consign offset. */ - state_aux0.mode = MODE_POS; - state_aux0.variant = 0; - pos_aux0.cons += v8_to_v16 (args[0], args[1]); + if (!aux) { proto_send0 ('?'); return; } + state->mode = MODE_POS; + state->variant = 0; + pos->cons += v8_to_v16 (args[1], args[2]); break; case c ('s', 0): /* Stop (set zero speed). */ @@ -285,13 +325,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_theta.cons = args[0] << 8; speed_alpha.cons = args[1] << 8; break; - case c ('s', 1): + case c ('S', 2): /* Set auxiliary speed. + * - b: aux index. * - b: speed. */ - state_aux0.mode = MODE_SPEED; - state_aux0.variant = 0; - speed_aux0.use_pos = 0; - speed_aux0.cons = args[0] << 8; + if (!aux) { proto_send0 ('?'); return; } + state->mode = MODE_SPEED; + state->variant = 0; + speed->use_pos = 0; + speed->cons = args[1] << 8; break; case c ('s', 9): /* Set speed controlled position consign. @@ -307,17 +349,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]); state_start (&state_main, MODE_SPEED, args[8]); break; - case c ('s', 5): - /* Set auxiliary speed controlled position consign. - * - d: consign offset. - * - b: sequence number. */ - if (args[4] == state_aux0.sequence) - break; - speed_aux0.use_pos = 1; - speed_aux0.pos_cons = pos_aux0.cons; - speed_aux0.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); - state_start (&state_aux0, MODE_SPEED, args[4]); - break; case c ('l', 5): /* Set linear speed controlled position consign. * - d: consign offset. @@ -339,6 +370,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2], args[3]), args[4]); break; + case c ('S', 6): + /* Set auxiliary speed controlled position consign. + * - b: aux index. + * - d: consign offset. + * - b: sequence number. */ + if (!aux) { proto_send0 ('?'); return; } + if (args[5] == state->sequence) + 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]); + break; case c ('f', 1): /* Go to the wall. * - b: sequence number. */ @@ -412,24 +456,30 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('y', 3): /* Auxiliary go to position. + * - b: aux index. * - w: pos, i16. * - b: sequence number. */ - if (args[2] == state_aux0.sequence) + if (!aux) { proto_send0 ('?'); return; } + if (args[3] == state->sequence) break; - aux_traj_goto_start (&aux0, v8_to_v16 (args[0], args[1]), args[2]); + aux_traj_goto_start (aux, v8_to_v16 (args[1], args[2]), args[3]); break; case c ('y', 1): /* Auxiliary find zero. + * - b: aux index. * - b: sequence number. */ - if (args[0] == state_aux0.sequence) + if (!aux) { proto_send0 ('?'); return; } + if (args[1] == state->sequence) break; - aux_traj_find_zero_start (&aux0, args[0]); + aux_traj_find_zero_start (aux, args[1]); break; - case c ('a', 2): - /* Set both acknoledge. + case c ('a', 3): + /* Set all acknoledge. * - b: main ack sequence number. - * - b: auxiliary ack sequence number. */ - state_acknowledge (&state_aux0, args[1]); + * - 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]); /* no break; */ case c ('a', 1): /* Set main acknoledge. @@ -462,6 +512,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Motor position control stats. */ main_stat_pos_cpt = main_stat_pos = args[0]; break; + case c ('Q', 1): + /* Auxiliary motor position control stats. */ + main_stat_pos_aux_cpt = main_stat_pos_aux = args[0]; + break; case c ('W', 1): /* Pwm stats. */ main_stat_pwm_cpt = main_stat_pwm = args[0]; @@ -484,6 +538,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Params. */ if (cmd == 'p') { + /* Many commands use the first argument as a selector. */ + switch (args[1]) + { + case 't': + pos = &pos_theta; + speed = &speed_theta; + break; + case 'a': + pos = &pos_alpha; + speed = &speed_alpha; + break; + case 0: + case 1: + pos = &pos_aux[args[1]]; + speed = &speed_aux[args[1]]; + break; + default: + pos = 0; + speed = 0; + break; + } switch (c (args[0], size)) { case c ('X', 1): @@ -518,56 +593,42 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: footing. */ postrack_set_footing (v8_to_v16 (args[1], args[2])); break; - case c ('a', 5): - /* Set main acceleration. - * - w: theta. - * - w: alpha. */ - speed_theta.acc = v8_to_v16 (args[1], args[2]); - speed_alpha.acc = v8_to_v16 (args[3], args[4]); - break; - case c ('a', 3): - /* Set auxiliary acceleration. + case c ('a', 4): + /* Set acceleration. + * - b: index. * - w: acceleration. */ - speed_aux0.acc = v8_to_v16 (args[1], args[2]); - break; - case c ('s', 5): - /* Set main maximum and slow speed. - * - b: theta max. - * - b: alpha max. - * - b: theta slow. - * - b: alpha slow. */ - speed_theta.max = args[1]; - speed_alpha.max = args[2]; - speed_theta.slow = args[3]; - speed_alpha.slow = args[4]; - break; - case c ('s', 3): - /* Set auxiliary maximum and slow speed. - * - b: speed max. - * - b: speed slow. */ - speed_aux0.max = args[1]; - speed_aux0.slow = args[2]; - break; - case c ('p', 3): - pos_aux0.kp = v8_to_v16 (args[1], args[2]); - break; - case c ('p', 5): - pos_theta.kp = v8_to_v16 (args[1], args[2]); - pos_alpha.kp = v8_to_v16 (args[3], args[4]); + if (!speed) { proto_send0 ('?'); return; } + speed->acc = v8_to_v16 (args[2], args[3]); break; - case c ('i', 3): - pos_aux0.ki = v8_to_v16 (args[1], args[2]); + case c ('s', 4): + /* Set maximum and slow speed. + * - b: index. + * - b: max speed. + * - b: slow speed. */ + if (!speed) { proto_send0 ('?'); return; } + speed->max = args[2]; + speed->slow = args[3]; break; - case c ('i', 5): - pos_theta.ki = v8_to_v16 (args[1], args[2]); - pos_alpha.ki = v8_to_v16 (args[3], args[4]); + case c ('p', 4): + /* Set proportional coefficient. + * - b: index. + * - w: P coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->kp = v8_to_v16 (args[2], args[3]); break; - case c ('d', 3): - pos_aux0.kd = v8_to_v16 (args[1], args[2]); + case c ('i', 4): + /* Set integral coefficient. + * - b: index. + * - w: I coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->ki = v8_to_v16 (args[2], args[3]); break; - case c ('d', 5): - pos_theta.kd = v8_to_v16 (args[1], args[2]); - pos_alpha.kd = v8_to_v16 (args[3], args[4]); + case c ('d', 4): + /* Set differential coefficient. + * - b: index. + * - w: D coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->kd = v8_to_v16 (args[2], args[3]); break; case c ('E', 3): pos_e_sat = v8_to_v16 (args[1], args[2]); @@ -592,7 +653,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('w', 2): /* Set PWM direction. - * - b: bits: 0000[aux0][right][left]. */ + * - b: bits: 000[aux1][aux0][right][left]. */ pwm_set_reverse (args[1]); break; case c ('E', 2): @@ -609,16 +670,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) proto_send1d ('c', counter_right_correction); proto_send1w ('f', postrack_footing); proto_send2w ('a', speed_theta.acc, speed_alpha.acc); - proto_send4b ('s', speed_theta.max, speed_alpha.max, - speed_theta.slow, speed_alpha.slow); + proto_send2b ('s', speed_theta.max, speed_theta.slow); + proto_send2b ('s', speed_alpha.max, speed_alpha.slow); 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_send1w ('a', speed_aux0.acc); - proto_send2b ('s', speed_aux0.max, speed_aux0.slow); - proto_send1w ('p', pos_aux0.kp); - proto_send1w ('i', pos_aux0.ki); - proto_send1w ('d', pos_aux0.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_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); -- cgit v1.2.3