From fc9956a4b1858d490a195eae4574bfa4d18b066f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 Mar 2008 00:26:41 +0100 Subject: * digital/asserv/src/asserv: - moved speed control state to structures. --- digital/asserv/src/asserv/eeprom.avr.c | 24 +++++----- digital/asserv/src/asserv/main.c | 44 +++++++++--------- digital/asserv/src/asserv/speed.c | 81 ++++++++++++++-------------------- digital/asserv/src/asserv/speed.h | 26 ++++++++--- digital/asserv/src/asserv/traj.c | 22 ++++----- 5 files changed, 98 insertions(+), 99 deletions(-) (limited to 'digital/asserv/src/asserv') diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index f435925e..dbbdbdec 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -42,15 +42,15 @@ eeprom_read_params (void) uint16_t *p16; if (eeprom_read_byte (p8++) != EEPROM_KEY) return; - speed_theta_max = eeprom_read_byte (p8++); - speed_alpha_max = eeprom_read_byte (p8++); - speed_theta_slow = eeprom_read_byte (p8++); - speed_alpha_slow = eeprom_read_byte (p8++); + speed_theta.max = eeprom_read_byte (p8++); + speed_alpha.max = eeprom_read_byte (p8++); + speed_theta.slow = eeprom_read_byte (p8++); + speed_alpha.slow = eeprom_read_byte (p8++); pwm_set_reverse (eeprom_read_byte (p8++)); 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_theta.acc = eeprom_read_word (p16++); + speed_alpha.acc = eeprom_read_word (p16++); pos_theta.kp = eeprom_read_word (p16++); pos_alpha.kp = eeprom_read_word (p16++); pos_theta.ki = eeprom_read_word (p16++); @@ -69,15 +69,15 @@ eeprom_write_params (void) uint8_t *p8 = (uint8_t *) EEPROM_START; uint16_t *p16; 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_theta_slow); - eeprom_write_byte (p8++, speed_alpha_slow); + eeprom_write_byte (p8++, speed_theta.max); + eeprom_write_byte (p8++, speed_alpha.max); + eeprom_write_byte (p8++, speed_theta.slow); + eeprom_write_byte (p8++, speed_alpha.slow); eeprom_write_byte (p8++, pwm_reverse); 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_theta.acc); + eeprom_write_word (p16++, speed_alpha.acc); eeprom_write_word (p16++, pos_theta.kp); eeprom_write_word (p16++, pos_alpha.kp); eeprom_write_word (p16++, pos_theta.ki); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index d2340b10..6d761315 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -158,7 +158,7 @@ main_loop (void) } if (main_stat_speed && !--main_stat_speed_cpt) { - proto_send2b ('S', speed_theta_cur >> 8, speed_alpha_cur >> 8); + proto_send2b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8); main_stat_speed_cpt = main_stat_speed; } if (main_stat_pos && !--main_stat_pos_cpt) @@ -251,18 +251,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('s', 0): /* Stop (set zero speed). */ state_main.mode = MODE_SPEED; - speed_pos = 0; - speed_theta_cons = 0; - speed_alpha_cons = 0; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = 0; + speed_alpha.cons = 0; break; case c ('s', 2): /* Set speed. * - b: theta speed. * - b: alpha speed. */ state_main.mode = MODE_SPEED; - speed_pos = 0; - speed_theta_cons = args[0] << 8; - speed_alpha_cons = args[1] << 8; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = args[0] << 8; + speed_alpha.cons = args[1] << 8; break; case c ('s', 9): /* Set speed controlled position consign. @@ -272,11 +272,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) if (args[8] == state_main.sequence) break; state_main.mode = MODE_SPEED; - speed_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]); + 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, args[8]); break; case c ('f', 1): @@ -285,7 +285,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) if (args[0] == state_main.sequence) break; state_main.mode = MODE_TRAJ; - speed_pos = 0; + speed_theta.use_pos = speed_alpha.use_pos = 0; traj_mode = 10; state_start (&state_main, args[0]); break; @@ -375,8 +375,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set 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]); + speed_theta.acc = v8_to_v16 (args[1], args[2]); + speed_alpha.acc = v8_to_v16 (args[3], args[4]); break; case c ('s', 5): /* Set maximum and slow speed. @@ -384,10 +384,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 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]; + speed_theta.max = args[1]; + speed_alpha.max = args[2]; + speed_theta.slow = args[3]; + speed_alpha.slow = args[4]; break; case c ('p', 3): pos_theta.kp = pos_alpha.kp = v8_to_v16 (args[1], args[2]); @@ -436,9 +436,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Print current settings. */ proto_send1b ('E', EEPROM_KEY); 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_send2w ('a', speed_theta.acc, speed_alpha.acc); + proto_send4b ('s', speed_theta.max, speed_alpha.max, + speed_theta.slow, 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); diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index c2106909..b50a6b52 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -37,39 +37,20 @@ * shaft position. */ -/** Current speed, f8.8. */ -int16_t speed_theta_cur, speed_alpha_cur; -/** Consign speed, f8.8. */ -int16_t speed_theta_cons, speed_alpha_cons; -/** Maximum speed for position consign, u8. */ -int8_t speed_theta_max, speed_alpha_max; -/** Slow speed for position consign, u8. */ -int8_t speed_theta_slow, speed_alpha_slow; -/** Consign position. */ -uint32_t speed_theta_pos_cons, speed_alpha_pos_cons; -/** Whether to use the consign position (1) or not (0). */ -uint8_t speed_pos; - -/** Acceleration, uf8.8. */ -int16_t speed_theta_acc, speed_alpha_acc; +/** Theta/alpha speed control states. */ +struct speed_t speed_theta, speed_alpha; /** Update shaft position consign according to a speed consign. */ static void -speed_update_by_speed (void) +speed_update_by_speed (struct speed_t *speed) { /* Update current speed. */ - if (UTILS_ABS (speed_theta_cons - speed_theta_cur) < speed_theta_acc) - speed_theta_cur = speed_theta_cons; - else if (speed_theta_cons > speed_theta_cur) - speed_theta_cur += speed_theta_acc; - else - speed_theta_cur -= speed_theta_acc; - if (UTILS_ABS (speed_alpha_cons - speed_alpha_cur) < speed_alpha_acc) - speed_alpha_cur = speed_alpha_cons; - else if (speed_alpha_cons > speed_alpha_cur) - speed_alpha_cur += speed_alpha_acc; + if (UTILS_ABS (speed->cons - speed->cur) < speed->acc) + speed->cur = speed->cons; + else if (speed->cons > speed->cur) + speed->cur += speed->acc; else - speed_alpha_cur -= speed_alpha_acc; + speed->cur -= speed->acc; } /** Compute maximum allowed speed according to: distance left, maximum speed, @@ -94,22 +75,26 @@ speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max) /** Update shaft position consign according to a position consign. */ static void -speed_update_by_position (void) +speed_update_by_position (struct speed_t *speed, struct pos_t *pos) { - int32_t theta_d = speed_theta_pos_cons - pos_theta.cons; - int32_t alpha_d = speed_alpha_pos_cons - pos_alpha.cons; - if (theta_d >= -speed_theta_max && theta_d <= speed_theta_max) - speed_theta_cur = theta_d << 8; + int32_t diff = speed->pos_cons - pos->cons; + if (diff >= -speed->max && diff <= speed->max) + speed->cur = diff << 8; else - speed_theta_cur = speed_compute_max_speed (theta_d, speed_theta_cur, - speed_theta_acc, speed_theta_max); - if (alpha_d >= -speed_alpha_max && alpha_d <= speed_alpha_max) - speed_alpha_cur = alpha_d << 8; + speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc, + speed->max); +} + +/** Update shaft position consign according to its consign type. */ +static void +speed_update_by (struct speed_t *speed, struct pos_t *pos) +{ + if (speed->use_pos) + speed_update_by_position (speed, pos); else - speed_alpha_cur = speed_compute_max_speed (alpha_d, speed_alpha_cur, - speed_alpha_acc, speed_alpha_max); - if (speed_theta_cur == 0 && speed_alpha_cur == 0) - state_finish (&state_main); + speed_update_by_speed (speed); + /* Update shaft position. */ + pos->cons += speed->cur >> 8; } /** Update shaft position consign according to consign. */ @@ -117,12 +102,14 @@ void speed_update (void) { /* Update speed. */ - if (speed_pos) - speed_update_by_position (); - else - speed_update_by_speed (); - /* Update shaft position. */ - pos_theta.cons += speed_theta_cur >> 8; - pos_alpha.cons += speed_alpha_cur >> 8; + speed_update_by (&speed_theta, &pos_theta); + speed_update_by (&speed_alpha, &pos_alpha); + /* Check for completion. */ + if ((speed_theta.use_pos || speed_alpha.use_pos) + && (!speed_theta.use_pos || speed_theta.cur == 0) + && (!speed_alpha.use_pos || speed_alpha.cur == 0)) + { + state_finish (&state_main); + } } diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h index 82899871..f62f267f 100644 --- a/digital/asserv/src/asserv/speed.h +++ b/digital/asserv/src/asserv/speed.h @@ -25,14 +25,26 @@ * * }}} */ -extern int16_t speed_theta_cur, speed_alpha_cur; -extern int16_t speed_theta_cons, speed_alpha_cons; -extern int8_t speed_theta_max, speed_alpha_max; -extern int8_t speed_theta_slow, speed_alpha_slow; -extern uint32_t speed_theta_pos_cons, speed_alpha_pos_cons; -extern uint8_t speed_pos; +/** Speed control state. */ +struct speed_t +{ + /** Current speed, f8.8. */ + int16_t cur; + /** Consign speed, f8.8. */ + int16_t cons; + /** Maximum speed for position consign, u8. */ + int8_t max; + /** Slow speed for position consign, u8. */ + int8_t slow; + /** Acceleration, f8.8. */ + int16_t acc; + /** Consign position. */ + uint32_t pos_cons; + /** Whether to use the consign position (1) or not (0). */ + uint8_t use_pos; +}; -extern int16_t speed_theta_acc, speed_alpha_acc; +extern struct speed_t speed_theta, speed_alpha; void speed_update (void); diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index fc41d3a5..226430a2 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -46,29 +46,29 @@ static void traj_ftw (void) { int16_t speed; - speed = speed_theta_slow; + speed = speed_theta.slow; speed *= 256; if (PINC & _BV (0) && PINC & _BV (1)) { - speed_theta_cons = -speed; - speed_alpha_cons = 0; + speed_theta.cons = -speed; + speed_alpha.cons = 0; } else if (PINC & _BV (0)) { - speed_theta_cons = -speed / 2; - speed_alpha_cons = speed / 2; + speed_theta.cons = -speed / 2; + speed_alpha.cons = speed / 2; } else if (PINC & _BV (1)) { - speed_theta_cons = -speed / 2; - speed_alpha_cons = -speed / 2; + speed_theta.cons = -speed / 2; + speed_alpha.cons = -speed / 2; } else { - speed_theta_cons = 0; - speed_alpha_cons = 0; - speed_theta_cur = 0; - speed_alpha_cur = 0; + speed_theta.cons = 0; + speed_alpha.cons = 0; + speed_theta.cur = 0; + speed_alpha.cur = 0; state_finish (&state_main); traj_mode = 11; } -- cgit v1.2.3