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/aux.c | 24 ++- digital/asserv/src/asserv/aux.h | 2 +- digital/asserv/src/asserv/avrconfig.h | 2 + digital/asserv/src/asserv/counter.h | 6 +- digital/asserv/src/asserv/counter_ext.avr.c | 35 +++- digital/asserv/src/asserv/eeprom.avr.c | 36 ++-- digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/init | 14 -- digital/asserv/src/asserv/main.c | 254 +++++++++++++++++----------- digital/asserv/src/asserv/models.host.c | 31 ++-- digital/asserv/src/asserv/models.host.h | 10 +- digital/asserv/src/asserv/move_bot_blue | 1 - digital/asserv/src/asserv/move_bot_red | 1 - digital/asserv/src/asserv/pos.c | 7 +- digital/asserv/src/asserv/pos.h | 2 +- digital/asserv/src/asserv/pwm.avr.c | 4 +- digital/asserv/src/asserv/pwm.h | 13 +- digital/asserv/src/asserv/pwm_mp.avr.c | 10 +- digital/asserv/src/asserv/pwm_ocr.avr.c | 32 ++-- digital/asserv/src/asserv/simu.host.c | 73 +++++--- digital/asserv/src/asserv/speed.c | 10 +- digital/asserv/src/asserv/speed.h | 2 +- digital/asserv/src/asserv/state.c | 2 +- digital/asserv/src/asserv/state.h | 4 +- digital/asserv/src/asserv/test_counter.c | 3 +- digital/asserv/src/asserv/twi_proto.c | 16 +- 26 files changed, 367 insertions(+), 229 deletions(-) delete mode 100644 digital/asserv/src/asserv/init delete mode 100644 digital/asserv/src/asserv/move_bot_blue delete mode 100644 digital/asserv/src/asserv/move_bot_red (limited to 'digital/asserv/src/asserv') diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c index 05ada060..f8665fbb 100644 --- a/digital/asserv/src/asserv/aux.c +++ b/digital/asserv/src/asserv/aux.c @@ -38,8 +38,8 @@ # include "simu.host.h" #endif -/** Motor state. */ -struct aux_t aux0; +/** Motors states. */ +struct aux_t aux[AC_ASSERV_AUX_NB]; /** Trajectory modes. */ enum @@ -62,18 +62,24 @@ enum void aux_init (void) { - aux0.state = &state_aux0; - aux0.speed = &speed_aux0; - aux0.zero_pin = &PINC; - aux0.zero_bv = _BV (5); + aux[0].state = &state_aux[0]; + aux[0].speed = &speed_aux[0]; + aux[0].zero_pin = &PINC; + aux[0].zero_bv = _BV (5); + aux[1].state = &state_aux[1]; + aux[1].speed = &speed_aux[1]; + aux[1].zero_pin = &PINC; + aux[1].zero_bv = _BV (6); } /** Update positions. */ void aux_pos_update (void) { + uint8_t i; /* Easy... */ - aux0.pos += counter_aux0_diff; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + aux[i].pos += counter_aux_diff[i]; } /** Goto position. */ @@ -190,6 +196,8 @@ aux_traj_update_single (struct aux_t *aux) void aux_traj_update (void) { - aux_traj_update_single (&aux0); + uint8_t i; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + aux_traj_update_single (&aux[i]); } diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h index 9460e87f..5a95bdaf 100644 --- a/digital/asserv/src/asserv/aux.h +++ b/digital/asserv/src/asserv/aux.h @@ -46,7 +46,7 @@ struct aux_t uint8_t zero_bv; }; -extern struct aux_t aux0; +extern struct aux_t aux[AC_ASSERV_AUX_NB]; void aux_init (void); diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h index b8451a09..f95f74f8 100644 --- a/digital/asserv/src/asserv/avrconfig.h +++ b/digital/asserv/src/asserv/avrconfig.h @@ -84,6 +84,8 @@ #define AC_PROTO_QUOTE 1 /* asserv. */ +/** Number of auxiliary motors. */ +#define AC_ASSERV_AUX_NB 2 /** Use external counters. */ #define AC_ASSERV_COUNTER_EXTERNAL 1 /** TWI address. */ diff --git a/digital/asserv/src/asserv/counter.h b/digital/asserv/src/asserv/counter.h index c5eae18b..45b7392f 100644 --- a/digital/asserv/src/asserv/counter.h +++ b/digital/asserv/src/asserv/counter.h @@ -25,9 +25,11 @@ * * }}} */ -extern uint16_t counter_left, counter_right, counter_aux0; +extern uint16_t counter_left, counter_right, + counter_aux[AC_ASSERV_AUX_NB]; extern uint32_t counter_right_correction; -extern int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; +extern int16_t counter_left_diff, counter_right_diff, + counter_aux_diff[AC_ASSERV_AUX_NB]; void counter_init (void); diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c index 4f8cad3b..16463aba 100644 --- a/digital/asserv/src/asserv/counter_ext.avr.c +++ b/digital/asserv/src/asserv/counter_ext.avr.c @@ -43,6 +43,8 @@ #define COUNTER_RIGHT 1 /** Define the first auxiliary counter address. */ #define COUNTER_AUX0 2 +/** Define the second auxiliary counter address. */ +#define COUNTER_AUX1 3 /** Define to 1 to reverse the left counter. */ #define COUNTER_LEFT_REVERSE 1 @@ -50,22 +52,27 @@ #define COUNTER_RIGHT_REVERSE 0 /** Define to 1 to reverse the first auxiliary counter. */ #define COUNTER_AUX0_REVERSE 0 +/** Define to 1 to reverse the second auxiliary counter. */ +#define COUNTER_AUX1_REVERSE 0 /** Define to 1 to use the AVR External Memory system, or 0 to use hand made * signals. */ #define COUNTER_USE_XMEM 1 /** Last values. */ -static uint8_t counter_left_old, counter_right_old, counter_aux0_old; +static uint8_t counter_left_old, counter_right_old, + counter_aux_old[AC_ASSERV_AUX_NB]; /** Overall counter values. */ -uint16_t counter_left, counter_right, counter_aux0; +uint16_t counter_left, counter_right, + counter_aux[AC_ASSERV_AUX_NB]; /** Overall uncorrected counter values. */ static int32_t counter_right_raw; /** Correction factor (f8.24). */ uint32_t counter_right_correction = 1L << 24; /** Counter differences since last update. * Maximum of 9 significant bits, sign included. */ -int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; +int16_t counter_left_diff, counter_right_diff, + counter_aux_diff[AC_ASSERV_AUX_NB]; #if !COUNTER_USE_XMEM # define COUNTER_ALE _BV (2) @@ -117,18 +124,20 @@ counter_init (void) /* Begin with safe values. */ counter_left_old = counter_read (COUNTER_LEFT); counter_right_old = counter_read (COUNTER_RIGHT); - counter_aux0_old = counter_read (COUNTER_AUX0); + counter_aux_old[0] = counter_read (COUNTER_AUX0); + counter_aux_old[1] = counter_read (COUNTER_AUX1); } /** Update overall counter values and compute diffs. */ void counter_update (void) { - uint8_t left, right, aux0; + uint8_t left, right, aux0, aux1; /* Sample counters. */ left = counter_read (COUNTER_LEFT); right = counter_read (COUNTER_RIGHT); aux0 = counter_read (COUNTER_AUX0); + aux1 = counter_read (COUNTER_AUX1); /* Left counter. */ #if !COUNTER_LEFT_REVERSE counter_left_diff = (int8_t) (left - counter_left_old); @@ -152,11 +161,19 @@ counter_update (void) counter_right = right_new; /* First auxiliary counter. */ #if !COUNTER_AUX0_REVERSE - counter_aux0_diff = (int8_t) (aux0 - counter_aux0_old); + counter_aux_diff[0] = (int8_t) (aux0 - counter_aux_old[0]); #else - counter_aux0_diff = (int8_t) (counter_aux0_old - aux0); + counter_aux_diff[0] = (int8_t) (counter_aux_old[0] - aux0); #endif - counter_aux0_old = aux0; - counter_aux0 += counter_aux0_diff; + counter_aux_old[0] = aux0; + counter_aux[0] += counter_aux_diff[0]; + /* Second auxiliary counter. */ +#if !COUNTER_AUX1_REVERSE + counter_aux_diff[1] = (int8_t) (aux1 - counter_aux_old[1]); +#else + counter_aux_diff[1] = (int8_t) (counter_aux_old[1] - aux1); +#endif + counter_aux_old[1] = aux1; + counter_aux[1] += counter_aux_diff[1]; } diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 0cb04aae..cbb92b33 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -73,26 +73,32 @@ eeprom_read_params (void) return; speed_theta.max = eeprom_read_byte (p8++); speed_alpha.max = eeprom_read_byte (p8++); - speed_aux0.max = eeprom_read_byte (p8++); + speed_aux[0].max = eeprom_read_byte (p8++); + speed_aux[1].max = eeprom_read_byte (p8++); speed_theta.slow = eeprom_read_byte (p8++); speed_alpha.slow = eeprom_read_byte (p8++); - speed_aux0.slow = eeprom_read_byte (p8++); + speed_aux[0].slow = eeprom_read_byte (p8++); + speed_aux[1].slow = eeprom_read_byte (p8++); pwm_set_reverse (eeprom_read_byte (p8++)); counter_right_correction = eeprom_read_32 (p8); p8 += 4; p16 = (uint16_t *) p8; postrack_set_footing (eeprom_read_word (p16++)); speed_theta.acc = eeprom_read_word (p16++); speed_alpha.acc = eeprom_read_word (p16++); - speed_aux0.acc = eeprom_read_word (p16++); + speed_aux[0].acc = eeprom_read_word (p16++); + speed_aux[1].acc = eeprom_read_word (p16++); pos_theta.kp = eeprom_read_word (p16++); pos_alpha.kp = eeprom_read_word (p16++); - pos_aux0.kp = eeprom_read_word (p16++); + pos_aux[0].kp = eeprom_read_word (p16++); + pos_aux[1].kp = eeprom_read_word (p16++); pos_theta.ki = eeprom_read_word (p16++); pos_alpha.ki = eeprom_read_word (p16++); - pos_aux0.ki = eeprom_read_word (p16++); + pos_aux[0].ki = eeprom_read_word (p16++); + pos_aux[1].ki = eeprom_read_word (p16++); pos_theta.kd = eeprom_read_word (p16++); pos_alpha.kd = eeprom_read_word (p16++); - pos_aux0.kd = eeprom_read_word (p16++); + pos_aux[0].kd = eeprom_read_word (p16++); + pos_aux[1].kd = eeprom_read_word (p16++); pos_e_sat = eeprom_read_word (p16++); pos_i_sat = eeprom_read_word (p16++); pos_d_sat = eeprom_read_word (p16++); @@ -113,26 +119,32 @@ eeprom_write_params (void) eeprom_write_byte (p8++, EEPROM_KEY); eeprom_write_byte (p8++, speed_theta.max); eeprom_write_byte (p8++, speed_alpha.max); - eeprom_write_byte (p8++, speed_aux0.max); + eeprom_write_byte (p8++, speed_aux[0].max); + eeprom_write_byte (p8++, speed_aux[1].max); eeprom_write_byte (p8++, speed_theta.slow); eeprom_write_byte (p8++, speed_alpha.slow); - eeprom_write_byte (p8++, speed_aux0.slow); + eeprom_write_byte (p8++, speed_aux[0].slow); + eeprom_write_byte (p8++, speed_aux[1].slow); eeprom_write_byte (p8++, pwm_reverse); eeprom_write_32 (p8, counter_right_correction); p8 += 4; p16 = (uint16_t *) p8; eeprom_write_word (p16++, postrack_footing); eeprom_write_word (p16++, speed_theta.acc); eeprom_write_word (p16++, speed_alpha.acc); - eeprom_write_word (p16++, speed_aux0.acc); + eeprom_write_word (p16++, speed_aux[0].acc); + eeprom_write_word (p16++, speed_aux[1].acc); eeprom_write_word (p16++, pos_theta.kp); eeprom_write_word (p16++, pos_alpha.kp); - eeprom_write_word (p16++, pos_aux0.kp); + eeprom_write_word (p16++, pos_aux[0].kp); + eeprom_write_word (p16++, pos_aux[1].kp); eeprom_write_word (p16++, pos_theta.ki); eeprom_write_word (p16++, pos_alpha.ki); - eeprom_write_word (p16++, pos_aux0.ki); + eeprom_write_word (p16++, pos_aux[0].ki); + eeprom_write_word (p16++, pos_aux[1].ki); eeprom_write_word (p16++, pos_theta.kd); eeprom_write_word (p16++, pos_alpha.kd); - eeprom_write_word (p16++, pos_aux0.kd); + eeprom_write_word (p16++, pos_aux[0].kd); + eeprom_write_word (p16++, pos_aux[1].kd); eeprom_write_word (p16++, pos_e_sat); eeprom_write_word (p16++, pos_i_sat); eeprom_write_word (p16++, pos_d_sat); diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index 30d59780..052f37de 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 0x4c +#define EEPROM_KEY 0x4d void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/init b/digital/asserv/src/asserv/init deleted file mode 100644 index 6ec03ce4..00000000 --- a/digital/asserv/src/asserv/init +++ /dev/null @@ -1,14 +0,0 @@ -!p'f0dd1 -!p'p01000200 -!p'i00000000 -!p'd10001000 -!p'E03ff -!p'D01ff -!p'a00800080 -!p's60302010 -!p'p0100 -!p'i0000 -!p'd1000 -!p'a0080 -!p's0a05 -!Aff 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); diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index b8197b60..97341626 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -96,7 +96,7 @@ static const struct robot_t gloubi_robot = 13.0, // approx /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, NULL, 0 + 0.0, 0.0, { NULL, NULL }, { 0, 0 } }; /* Taz, APBTeam/Efrei 2005. */ @@ -116,7 +116,7 @@ static const struct robot_t taz_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, NULL, 0 + 0.0, 0.0, { NULL, NULL }, { 0, 0 } }; /* TazG, Taz with RE25G motors. */ @@ -136,7 +136,7 @@ static const struct robot_t tazg_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, NULL, 0 + 0.0, 0.0, { NULL, NULL }, { 0, 0 } }; /* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */ @@ -179,10 +179,10 @@ static const struct robot_t giboulee_robot = 0.063 / 2, /** Distance between the encoders wheels (m). */ 0.28, - /** First auxiliary motor or NULL if none. */ - &giboulee_arm_model, - /** Number of steps on the first auxiliary motor encoder. */ - 500, + /** Auxiliary motors, NULL if not present. */ + { &giboulee_arm_model, NULL }, + /** Number of steps for each auxiliary motor encoder. */ + { 500, 0 }, }; /* Table of models. */ @@ -214,8 +214,9 @@ models_get (const char *name) /** Initialise simulation models. */ void models_init (const struct robot_t *robot, struct motor_t *main_motor_left, - struct motor_t *main_motor_right, struct motor_t *aux0_motor) + struct motor_t *main_motor_right, struct motor_t aux_motor[]) { + int i; if (main_motor_left) { main_motor_left->m = *robot->main_motor; @@ -232,11 +233,17 @@ models_init (const struct robot_t *robot, struct motor_t *main_motor_left, main_motor_right->h = ECHANT_PERIOD; main_motor_right->d = 1000; } - if (aux0_motor && robot->aux0_motor) + if (aux_motor) { - aux0_motor->m = *robot->aux0_motor; - aux0_motor->h = ECHANT_PERIOD; - aux0_motor->d = 1000; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + { + if (robot->aux_motor[i]) + { + aux_motor[i].m = *robot->aux_motor[i]; + aux_motor[i].h = ECHANT_PERIOD; + aux_motor[i].d = 1000; + } + } } } diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h index 36099e64..ae2bc21d 100644 --- a/digital/asserv/src/asserv/models.host.h +++ b/digital/asserv/src/asserv/models.host.h @@ -49,10 +49,10 @@ struct robot_t double encoder_wheel_r; /** Distance between the encoders wheels (m). */ double encoder_footing; - /** First auxiliary motor or NULL if none. */ - const struct motor_def_t *aux0_motor; - /** Number of steps on the first auxiliary motor encoder. */ - int aux0_encoder_steps; + /** Auxiliary motors, NULL if not present. */ + const struct motor_def_t *aux_motor[AC_ASSERV_AUX_NB]; + /** Number of steps for each auxiliary motor encoder. */ + int aux_encoder_steps[AC_ASSERV_AUX_NB]; }; /** Get a pointer to a model by name, or return 0. */ @@ -62,6 +62,6 @@ models_get (const char *name); /** Initialise simulation models. */ void models_init (const struct robot_t *robot, struct motor_t *main_motor_left, - struct motor_t *main_motor_right, struct motor_t *aux0_motor); + struct motor_t *main_motor_right, struct motor_t aux_motor[]); #endif /* models_host_h */ diff --git a/digital/asserv/src/asserv/move_bot_blue b/digital/asserv/src/asserv/move_bot_blue deleted file mode 100644 index ec72254b..00000000 --- a/digital/asserv/src/asserv/move_bot_blue +++ /dev/null @@ -1 +0,0 @@ -!h'X00C807EE12D7 diff --git a/digital/asserv/src/asserv/move_bot_red b/digital/asserv/src/asserv/move_bot_red deleted file mode 100644 index 28d8ef29..00000000 --- a/digital/asserv/src/asserv/move_bot_red +++ /dev/null @@ -1 +0,0 @@ -!h'X0AF007EE12D7 diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index eafd2877..0679de40 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -44,7 +44,7 @@ struct pos_t pos_theta, pos_alpha; /** Auxiliaries control states. */ -struct pos_t pos_aux0; +struct pos_t pos_aux[AC_ASSERV_AUX_NB]; /** Error saturation. */ int32_t pos_e_sat = 1023; @@ -183,10 +183,13 @@ pos_update_single (struct state_t *state, struct pos_t *pos, void pos_update (void) { + uint8_t i; pos_update_polar (&state_main, &pos_theta, &pos_alpha, counter_left_diff, counter_right_diff, &pwm_left, &pwm_right); - pos_update_single (&state_aux0, &pos_aux0, counter_aux0_diff, &pwm_aux0); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + pos_update_single (&state_aux[i], &pos_aux[i], counter_aux_diff[i], + &pwm_aux[i]); } /** Reset position control state. To be called when the position control is diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h index cf02cda3..4ced49b8 100644 --- a/digital/asserv/src/asserv/pos.h +++ b/digital/asserv/src/asserv/pos.h @@ -45,7 +45,7 @@ struct pos_t }; extern struct pos_t pos_theta, pos_alpha; -extern struct pos_t pos_aux0; +extern struct pos_t pos_aux[AC_ASSERV_AUX_NB]; extern int32_t pos_e_sat, pos_i_sat, pos_d_sat; extern int32_t pos_blocked_error_limit, pos_blocked_speed_limit, diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c index 6a5b0ff4..cf8df769 100644 --- a/digital/asserv/src/asserv/pwm.avr.c +++ b/digital/asserv/src/asserv/pwm.avr.c @@ -30,7 +30,9 @@ /** PWM control states. */ struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left); struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right); -struct pwm_t pwm_aux0 = PWM_INIT_FOR (pwm_aux0); +struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = { + PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1) +}; /** PWM reverse directions. */ uint8_t pwm_reverse; diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h index 7877d4eb..7b834c05 100644 --- a/digital/asserv/src/asserv/pwm.h +++ b/digital/asserv/src/asserv/pwm.h @@ -41,16 +41,25 @@ struct pwm_t int16_t min; }; -extern struct pwm_t pwm_left, pwm_right, pwm_aux0; +extern struct pwm_t pwm_left, pwm_right, pwm_aux[AC_ASSERV_AUX_NB]; extern uint8_t pwm_reverse; +/** Define current PWM value for each output. */ +#define PWM_VALUE(x) PWM_VALUE_ (x) +#define PWM_VALUE_(x) PWM_MAX_FOR_ ## x +#define PWM_VALUE_pwm_left pwm_left.cur +#define PWM_VALUE_pwm_right pwm_right.cur +#define PWM_VALUE_pwm_aux0 pwm_aux[0].cur +#define PWM_VALUE_pwm_aux1 pwm_aux[1].cur + /** Define maximum PWM value for each output. */ #define PWM_MAX_FOR(x) PWM_MAX_FOR_ (x) #define PWM_MAX_FOR_(x) PWM_MAX_FOR_ ## x #define PWM_MAX_FOR_pwm_left PWM_MAX #define PWM_MAX_FOR_pwm_right PWM_MAX #define PWM_MAX_FOR_pwm_aux0 (PWM_MAX / 2) +#define PWM_MAX_FOR_pwm_aux1 (PWM_MAX / 2) /** Define minimum PWM value for each output, if the value is less than the * minimum, use 0. */ @@ -59,6 +68,7 @@ extern uint8_t pwm_reverse; #define PWM_MIN_FOR_pwm_left 0x10 #define PWM_MIN_FOR_pwm_right 0x10 #define PWM_MIN_FOR_pwm_aux0 0x10 +#define PWM_MIN_FOR_pwm_aux1 0x10 /** Define which bit controls the PWM inversion. */ #define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x) @@ -66,6 +76,7 @@ extern uint8_t pwm_reverse; #define PWM_REVERSE_BIT_pwm_left _BV (0) #define PWM_REVERSE_BIT_pwm_right _BV (1) #define PWM_REVERSE_BIT_pwm_aux0 _BV (2) +#define PWM_REVERSE_BIT_pwm_aux1 _BV (3) /** State init macro. */ #define PWM_INIT_FOR(x) \ diff --git a/digital/asserv/src/asserv/pwm_mp.avr.c b/digital/asserv/src/asserv/pwm_mp.avr.c index 47b50763..62ef2190 100644 --- a/digital/asserv/src/asserv/pwm_mp.avr.c +++ b/digital/asserv/src/asserv/pwm_mp.avr.c @@ -34,7 +34,7 @@ #define PWM1 pwm_left #define PWM2 pwm_right #define PWM3 pwm_aux0 -#undef PWM4 +#define PWM4 pwm_aux1 #include "pwm_config.h" @@ -98,8 +98,8 @@ void pwm_mp_update (void) { #if PWM1or2 || PWM3or4 - if (PWM1c (PWM1.cur) || PWM2c (PWM2.cur) - || PWM3c (PWM3.cur) || PWM4c (PWM4.cur)) + if (PWM1c (PWM_VALUE (PWM1)) || PWM2c (PWM_VALUE (PWM2)) + || PWM3c (PWM_VALUE (PWM3)) || PWM4c (PWM_VALUE (PWM4))) pwm_mp_go = 1; if (!pwm_mp_go) return; @@ -107,7 +107,7 @@ pwm_mp_update (void) #if PWM1or2 /* Chip enable. */ PORTB &= ~_BV (0); - pwm_mp_send (PWM1c (PWM1.cur), PWM2c (PWM2.cur), + pwm_mp_send (PWM1c (PWM_VALUE (PWM1)), PWM2c (PWM_VALUE (PWM2)), PWM1c (pwm_reverse & PWM_REVERSE_BIT (PWM1)), PWM2c (pwm_reverse & PWM_REVERSE_BIT (PWM2))); /* Chip disable. */ @@ -116,7 +116,7 @@ pwm_mp_update (void) #if PWM3or4 /* Chip enable. */ PORTE &= ~_BV (4); - pwm_mp_send (PWM3c (PWM3.cur), PWM4c (PWM4.cur), + pwm_mp_send (PWM3c (PWM_VALUE (PWM3)), PWM4c (PWM_VALUE (PWM4)), PWM3c (pwm_reverse & PWM_REVERSE_BIT (PWM3)), PWM4c (pwm_reverse & PWM_REVERSE_BIT (PWM4))); /* Chip disable. */ diff --git a/digital/asserv/src/asserv/pwm_ocr.avr.c b/digital/asserv/src/asserv/pwm_ocr.avr.c index 75857f10..25f2cfc7 100644 --- a/digital/asserv/src/asserv/pwm_ocr.avr.c +++ b/digital/asserv/src/asserv/pwm_ocr.avr.c @@ -95,35 +95,35 @@ pwm_ocr_update (void) # ifdef PWM1 uint16_t pwm1; /* Set PWM1. */ - if (PWM1.cur == 0) + if (PWM_VALUE (PWM1) == 0) { pwm1 = 0; } - else if (PWM1.cur < 0) + else if (PWM_VALUE (PWM1) < 0) { - pwm1 = -PWM1.cur; + pwm1 = -PWM_VALUE (PWM1); } else { dir_b |= _BV (PWM1_DIR); - pwm1 = PWM1.cur; + pwm1 = PWM_VALUE (PWM1); } # endif /* PWM1 */ # ifdef PWM2 uint16_t pwm2; /* Set PWM2. */ - if (PWM2.cur == 0) + if (PWM_VALUE (PWM2) == 0) { pwm2 = 0; } - else if (PWM2.cur < 0) + else if (PWM_VALUE (PWM2) < 0) { - pwm2 = -PWM2.cur; + pwm2 = -PWM_VALUE (PWM2); } else { dir_b |= _BV (PWM2_DIR); - pwm2 = PWM2.cur; + pwm2 = PWM_VALUE (PWM2); } # endif /* PWM2 */ #endif /* PWM1or2 */ @@ -134,35 +134,35 @@ pwm_ocr_update (void) # ifdef PWM3 uint16_t pwm3; /* Set PWM3. */ - if (PWM3.cur == 0) + if (PWM_VALUE (PWM3) == 0) { pwm3 = 0; } - else if (PWM3.cur < 0) + else if (PWM_VALUE (PWM3) < 0) { - pwm3 = -PWM3.cur; + pwm3 = -PWM_VALUE (PWM3); } else { dir_e |= _BV (PWM3_DIR); - pwm3 = PWM3.cur; + pwm3 = PWM_VALUE (PWM3); } # endif /* PWM3 */ # ifdef PWM4 uint16_t pwm4; /* Set PWM4. */ - if (PWM4.cur == 0) + if (PWM_VALUE (PWM4) == 0) { pwm4 = 0; } - else if (PWM4.cur < 0) + else if (PWM_VALUE (PWM4) < 0) { - pwm4 = -PWM4.cur; + pwm4 = -PWM_VALUE (PWM4); } else { dir_e |= _BV (PWM4_DIR); - pwm4 = PWM4.cur; + pwm4 = PWM_VALUE (PWM4); } # endif /* PWM4 */ #endif /* PWM3or4 */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index d496b61e..2ae3c3d4 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -38,6 +38,7 @@ #include #include "pwm.h" +#include "aux.h" #include "motor_model.host.h" #include "models.host.h" @@ -46,10 +47,12 @@ uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; /** Overall counter values. */ -uint16_t counter_left, counter_right, counter_aux0; +uint16_t counter_left, counter_right, + counter_aux[AC_ASSERV_AUX_NB]; /** Counter differences since last update. * Maximum of 9 significant bits, sign included. */ -int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; +int16_t counter_left_diff, counter_right_diff, + counter_aux_diff[AC_ASSERV_AUX_NB]; /** Overall uncorrected counter values. */ static int32_t counter_right_raw; /** Correction factor (f8.24). */ @@ -58,7 +61,9 @@ uint32_t counter_right_correction = 1L << 24; /** PWM control states. */ struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left); struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right); -struct pwm_t pwm_aux0 = PWM_INIT_FOR (pwm_aux0); +struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = { + PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1) +}; /** PWM reverse directions. */ uint8_t pwm_reverse; @@ -66,13 +71,15 @@ uint8_t pwm_reverse; const struct robot_t *simu_robot; /** Motor models. */ -struct motor_t simu_left_model, simu_right_model, simu_aux0_model; +struct motor_t simu_left_model, simu_right_model, + simu_aux_model[AC_ASSERV_AUX_NB]; /** Computed simulated position (mm, rad). */ double simu_pos_x, simu_pos_y, simu_pos_a; /** Full counter values. */ -uint32_t simu_counter_left, simu_counter_right, simu_counter_aux0; +uint32_t simu_counter_left, simu_counter_right, + simu_counter_aux[AC_ASSERV_AUX_NB]; double simu_counter_left_th, simu_counter_right_th; /** Use mex. */ @@ -103,7 +110,7 @@ simu_init (void) exit (1); } models_init (simu_robot, &simu_left_model, &simu_right_model, - &simu_aux0_model); + simu_aux_model); simu_pos_x = simu_pos_y = simu_pos_a = 0; } @@ -158,31 +165,40 @@ simu_sensor_update (void) PINC |= sensors_bit[i]; } /** Top zero sensor. */ - double aa = simu_aux0_model.th / simu_aux0_model.m.i_G * 3; - if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5)) - PINC |= _BV (5); + double aa; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + { + aa = simu_aux_model[i].th / simu_aux_model[i].m.i_G * 3; + if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5)) + *aux[i].zero_pin |= aux[i].zero_bv; + } } /** Do a simulation step. */ static void simu_step (void) { - double old_left_th, old_right_th, old_aux0_th; + int i; + double old_left_th, old_right_th, old_aux_th[AC_ASSERV_AUX_NB]; /* Convert pwm value into voltage. */ simu_left_model.u = simu_left_model.m.u_max * ((double) pwm_left.cur / (PWM_MAX + 1)); simu_right_model.u = simu_right_model.m.u_max * ((double) pwm_right.cur / (PWM_MAX + 1)); - simu_aux0_model.u = simu_aux0_model.m.u_max - * ((double) pwm_aux0.cur / (PWM_MAX + 1)); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + simu_aux_model[i].u = simu_aux_model[i].m.u_max + * ((double) pwm_aux[i].cur / (PWM_MAX + 1)); /* Make one step. */ old_left_th = simu_left_model.th; old_right_th = simu_right_model.th; - old_aux0_th = simu_aux0_model.th; motor_model_step (&simu_left_model); motor_model_step (&simu_right_model); - if (simu_robot->aux0_motor) - motor_model_step (&simu_aux0_model); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + { + old_aux_th[i] = simu_aux_model[i].th; + if (simu_robot->aux_motor[i]) + motor_model_step (&simu_aux_model[i]); + } /* Modify counters. */ uint32_t counter_left_new; uint32_t counter_right_new; @@ -224,13 +240,16 @@ simu_step (void) counter_right = right_new; simu_counter_right = counter_right_new; /* Update auxiliary counter. */ - if (simu_robot->aux0_motor) + for (i = 0; i < AC_ASSERV_AUX_NB; i++) { - uint32_t counter_aux0_new = simu_aux0_model.th / (2*M_PI) - * simu_robot->aux0_encoder_steps; - counter_aux0_diff = counter_aux0_new - simu_counter_aux0; - counter_aux0 += counter_aux0_diff; - simu_counter_aux0 = counter_aux0_new; + if (simu_robot->aux_motor[i]) + { + uint32_t counter_aux_new = simu_aux_model[i].th / (2*M_PI) + * simu_robot->aux_encoder_steps[i]; + counter_aux_diff[i] = counter_aux_new - simu_counter_aux[i]; + counter_aux[i] += counter_aux_diff[i]; + simu_counter_aux[i] = counter_aux_new; + } } /* Update position. */ simu_pos_update ((simu_left_model.th - old_left_th) @@ -246,6 +265,7 @@ simu_step (void) static void simu_send (void) { + int i; mex_msg_t *m; /* Send position. */ m = mex_msg_new (0xa0); @@ -254,12 +274,15 @@ simu_send (void) mex_node_send (m); /* Send PWM. */ m = mex_msg_new (0xa1); - mex_msg_push (m, "hhh", pwm_left.cur, pwm_right.cur, pwm_aux0.cur); + mex_msg_push (m, "hh", pwm_left.cur, pwm_right.cur); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + mex_msg_push (m, "h", pwm_aux[i].cur); mex_node_send (m); - /* Send Arm position. */ + /* Send Aux position. */ m = mex_msg_new (0xa8); - mex_msg_push (m, "l", (int32_t) (1024.0 * simu_aux0_model.th / - simu_aux0_model.m.i_G)); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + mex_msg_push (m, "l", (int32_t) (1024.0 * simu_aux_model[i].th + / simu_aux_model[i].m.i_G)); mex_node_send (m); } diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index b9e6a714..5c221b7e 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -41,15 +41,17 @@ struct speed_t speed_theta, speed_alpha; /** Auxiliaries speed control states. */ -struct speed_t speed_aux0; +struct speed_t speed_aux[AC_ASSERV_AUX_NB]; /** Initialise speed control states. */ void speed_init (void) { + uint8_t i; speed_theta.pos = &pos_theta; speed_alpha.pos = &pos_alpha; - speed_aux0.pos = &pos_aux0; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + speed_aux[i].pos = &pos_aux[i]; } /** Update current speed according to a speed consign. */ @@ -147,7 +149,9 @@ speed_update_single (struct state_t *state, struct speed_t *speed) void speed_update (void) { + uint8_t i; speed_update_double (&state_main, &speed_theta, &speed_alpha); - speed_update_single (&state_aux0, &speed_aux0); + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + speed_update_single (&state_aux[i], &speed_aux[i]); } diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h index 3ed02393..66ad53e2 100644 --- a/digital/asserv/src/asserv/speed.h +++ b/digital/asserv/src/asserv/speed.h @@ -47,7 +47,7 @@ struct speed_t }; extern struct speed_t speed_theta, speed_alpha; -extern struct speed_t speed_aux0; +extern struct speed_t speed_aux[AC_ASSERV_AUX_NB]; void speed_init (void); diff --git a/digital/asserv/src/asserv/state.c b/digital/asserv/src/asserv/state.c index 21b994d1..712b445c 100644 --- a/digital/asserv/src/asserv/state.c +++ b/digital/asserv/src/asserv/state.c @@ -27,5 +27,5 @@ struct state_t state_main; -struct state_t state_aux0; +struct state_t state_aux[AC_ASSERV_AUX_NB]; diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h index ebfa11dd..7967ed65 100644 --- a/digital/asserv/src/asserv/state.h +++ b/digital/asserv/src/asserv/state.h @@ -79,8 +79,8 @@ struct state_t /** Main motors state. */ extern struct state_t state_main; -/** First auxiliary motor state. */ -extern struct state_t state_aux0; +/** Auxiliary motor states. */ +extern struct state_t state_aux[AC_ASSERV_AUX_NB]; /** Start a new command execution. */ static inline void diff --git a/digital/asserv/src/asserv/test_counter.c b/digital/asserv/src/asserv/test_counter.c index 002d08ec..548cc3ee 100644 --- a/digital/asserv/src/asserv/test_counter.c +++ b/digital/asserv/src/asserv/test_counter.c @@ -85,7 +85,8 @@ main (void) } if (count && !--count_cpt) { - proto_send3w ('C', counter_left, counter_right, counter_aux0); + proto_send4w ('C', counter_left, counter_right, + counter_aux[0], counter_aux[1]); count_cpt = count; } while (uart0_poll ()) diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 13cc441d..f1baecb6 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -73,8 +73,8 @@ twi_proto_update (void) u8 status[12]; status[0] = (speed_theta.cur < 0 ? (1 << 5) : 0) | (speed_theta.cur > 0 ? (1 << 4) : 0) - | (state_aux0.blocked << 3) - | (state_aux0.finished << 2) + | (state_aux[0].blocked << 3) + | (state_aux[0].finished << 2) | (state_main.blocked << 1) | (state_main.finished << 0); status[1] = twi_proto.seq; @@ -86,8 +86,8 @@ twi_proto_update (void) status[7] = v32_to_v8 (postrack_y, 1); status[8] = v32_to_v8 (postrack_a, 2); status[9] = v32_to_v8 (postrack_a, 1); - status[10] = v16_to_v8 (aux0.pos, 1); - status[11] = v16_to_v8 (aux0.pos, 0); + status[10] = v16_to_v8 (aux[0].pos, 1); + status[11] = v16_to_v8 (aux[0].pos, 0); twi_sl_update (status, sizeof (status)); } @@ -190,14 +190,14 @@ twi_proto_callback (u8 *buf, u8 size) /* Move the arm. * - w: new position. * - b: speed. */ - speed_aux0.max = buf[4]; - aux_traj_goto_start (&aux0, v8_to_v16 (buf[2], buf[3]), 0); + speed_aux[0].max = buf[4]; + aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0); break; case c ('B', 1): /* Find the zero position of the arm. * - b: speed. */ - speed_aux0.max = buf[2]; - aux_traj_find_zero_start (&aux0, 0); + speed_aux[0].max = buf[2]; + aux_traj_find_zero_start (&aux[0], 0); break; case c ('p', x): /* Set parameters. */ -- cgit v1.2.3