From d11d86aac76679041f836125215016ccd4c10f15 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 Mar 2008 00:26:08 +0100 Subject: * digital/asserv/src/asserv: - moved position control parameters to a structure. --- digital/asserv/src/asserv/eeprom.avr.c | 24 +++++++------- digital/asserv/src/asserv/main.c | 49 ++++++++++++++------------- digital/asserv/src/asserv/pos.c | 60 ++++++++++++---------------------- digital/asserv/src/asserv/pos.h | 26 ++++++++++----- digital/asserv/src/asserv/speed.c | 8 ++--- 5 files changed, 81 insertions(+), 86 deletions(-) (limited to 'digital/asserv/src') diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 6b54a02b..f435925e 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -51,12 +51,12 @@ eeprom_read_params (void) postrack_set_footing (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++); - pos_alpha_ki = eeprom_read_word (p16++); - pos_theta_kd = eeprom_read_word (p16++); - pos_alpha_kd = 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++); + pos_alpha.ki = eeprom_read_word (p16++); + pos_theta.kd = eeprom_read_word (p16++); + pos_alpha.kd = eeprom_read_word (p16++); pos_e_sat = eeprom_read_word (p16++); pos_int_sat = eeprom_read_word (p16++); pos_blocked = eeprom_read_word (p16++); @@ -78,12 +78,12 @@ eeprom_write_params (void) eeprom_write_word (p16++, postrack_footing); 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); - eeprom_write_word (p16++, pos_alpha_ki); - eeprom_write_word (p16++, pos_theta_kd); - eeprom_write_word (p16++, pos_alpha_kd); + eeprom_write_word (p16++, pos_theta.kp); + eeprom_write_word (p16++, pos_alpha.kp); + eeprom_write_word (p16++, pos_theta.ki); + eeprom_write_word (p16++, pos_alpha.ki); + eeprom_write_word (p16++, pos_theta.kd); + eeprom_write_word (p16++, pos_alpha.kd); eeprom_write_word (p16++, pos_e_sat); eeprom_write_word (p16++, pos_int_sat); eeprom_write_word (p16++, pos_blocked); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index b4872ac5..d2340b10 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -163,8 +163,8 @@ main_loop (void) } if (main_stat_pos && !--main_stat_pos_cpt) { - proto_send4w ('P', pos_theta_e_old, pos_theta_int, - pos_alpha_e_old, pos_alpha_int); + proto_send4w ('P', pos_theta.e_old, pos_theta.i, + pos_alpha.e_old, pos_alpha.i); main_stat_pos_cpt = main_stat_pos; } #ifdef HOST @@ -214,7 +214,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Commands. */ case c ('w', 0): /* Set zero pwm. */ - pos_reset (); + pos_reset (&pos_theta); + pos_reset (&pos_alpha); state_main.mode = MODE_PWM; pwm_left = 0; pwm_right = 0; @@ -223,7 +224,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set pwm. * - w: left pwm. * - w: right pwm. */ - pos_reset (); + pos_reset (&pos_theta); + pos_reset (&pos_alpha); state_main.mode = MODE_PWM; pwm_left = v8_to_v16 (args[0], args[1]); UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); @@ -233,7 +235,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('w', 2): /* Set auxiliary pwm. * - w: pwm. */ - pos_reset (); // TODO + //pos_reset (); // TODO state_aux0.mode = MODE_PWM; pwm_aux0 = v8_to_v16 (args[0], args[1]); UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX); @@ -243,8 +245,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: theta consign offset. * - w: alpha consign offset. */ state_main.mode = MODE_POS; - pos_theta_cons += v8_to_v16 (args[0], args[1]); - pos_alpha_cons += v8_to_v16 (args[2], args[3]); + pos_theta.cons += v8_to_v16 (args[0], args[1]); + pos_alpha.cons += v8_to_v16 (args[2], args[3]); break; case c ('s', 0): /* Stop (set zero speed). */ @@ -271,9 +273,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; state_main.mode = MODE_SPEED; speed_pos = 1; - speed_theta_pos_cons = pos_theta_cons; + 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 = 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; @@ -291,7 +293,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set acknoledge. * - b: ack sequence number. */ if (state_main.blocked) - pos_reset (); + { + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + } state_acknowledge (&state_main, args[0]); break; /* Stats. @@ -385,25 +390,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_alpha_slow = args[4]; break; case c ('p', 3): - pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]); + pos_theta.kp = pos_alpha.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]); + pos_theta.kp = v8_to_v16 (args[1], args[2]); + pos_alpha.kp = v8_to_v16 (args[3], args[4]); break; case c ('i', 3): - pos_theta_ki = pos_alpha_ki = v8_to_v16 (args[1], args[2]); + pos_theta.ki = pos_alpha.ki = v8_to_v16 (args[1], args[2]); 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]); + pos_theta.ki = v8_to_v16 (args[1], args[2]); + pos_alpha.ki = v8_to_v16 (args[3], args[4]); break; case c ('d', 3): - pos_theta_kd = pos_alpha_kd = v8_to_v16 (args[1], args[2]); + pos_theta.kd = pos_alpha.kd = v8_to_v16 (args[1], args[2]); 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]); + pos_theta.kd = v8_to_v16 (args[1], args[2]); + pos_alpha.kd = v8_to_v16 (args[3], args[4]); break; case c ('E', 3): pos_e_sat = v8_to_v16 (args[1], args[2]); @@ -434,9 +439,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) 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); + 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 ('E', pos_e_sat); proto_send1w ('I', pos_int_sat); proto_send1w ('b', pos_blocked); diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index 0a63ec4f..e241b0fb 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -40,30 +40,17 @@ * 16 bits are enough as long as there is no long blocking (see 2005 cup!). */ -/** Current theta/alpha. */ -uint32_t pos_theta_cur, pos_alpha_cur; -/** Consign theta/alpha. */ -uint32_t pos_theta_cons, pos_alpha_cons; +/** Theta/alpha control states. */ +struct pos_t pos_theta, pos_alpha; /** Error saturation. */ int32_t pos_e_sat = 1023; /** Integral saturation. */ int32_t pos_int_sat = 1023; -/** P coefficients. */ -uint16_t pos_theta_kp, pos_alpha_kp; -/** I coefficients. */ -uint16_t pos_theta_ki, pos_alpha_ki; -/** D coefficients. */ -uint16_t pos_theta_kd, pos_alpha_kd; /** Blocked value. If error is greater than this value, stop the robot and * report blocked state. */ int32_t pos_blocked = 15000L; -/** Current integral values. */ -int32_t pos_theta_int, pos_alpha_int; -/** Last error values. */ -int32_t pos_theta_e_old, pos_alpha_e_old; - /** Compute a PID. * How to compute maximum numbers size: * Result is 24 bits (16 bits kept after shift). @@ -80,21 +67,20 @@ int32_t pos_theta_e_old, pos_alpha_e_old; * - bound the value returned. * - lower e & int saturation. */ static inline int16_t -pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old, - uint16_t kp, uint16_t ki, uint16_t kd) +pos_compute_pid (int32_t e, struct pos_t *pos) { int32_t diff, pid; /* Saturate error. */ UTILS_BOUND (e, -pos_e_sat, pos_e_sat); /* Integral update. */ - *i += e; - UTILS_BOUND (*i, -pos_int_sat, pos_int_sat); + pos->i += e; + UTILS_BOUND (pos->i, -pos_int_sat, pos_int_sat); /* Differential value. */ - diff = e - *e_old; + diff = e - pos->e_old; /* Compute PID. */ - pid = e * kp + *i * ki + diff * kd; + pid = e * pos->kp + pos->i * pos->ki + diff * pos->kd; /* Save result. */ - *e_old = e; + pos->e_old = e; return pid >> 8; } @@ -105,11 +91,11 @@ pos_update (void) int16_t pid_theta, pid_alpha; int32_t diff_theta, diff_alpha; /* Update current shaft positions. */ - pos_theta_cur += counter_left_diff + counter_right_diff; - pos_alpha_cur += counter_right_diff - counter_left_diff; + pos_theta.cur += counter_left_diff + counter_right_diff; + pos_alpha.cur += counter_right_diff - counter_left_diff; /* Compute PID. */ - diff_theta = pos_theta_cons - pos_theta_cur; - diff_alpha = pos_alpha_cons - pos_alpha_cur; + diff_theta = pos_theta.cons - pos_theta.cur; + diff_alpha = pos_alpha.cons - pos_alpha.cur; if (state_main.blocked || diff_theta < -pos_blocked || pos_blocked < diff_theta || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) @@ -121,12 +107,8 @@ pos_update (void) } else { - pid_theta = pos_compute_pid (diff_theta, &pos_theta_int, - &pos_theta_e_old, pos_theta_kp, - pos_theta_ki, pos_theta_kd); - pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha_int, - &pos_alpha_e_old, pos_alpha_kp, - pos_alpha_ki, pos_alpha_kd); + pid_theta = pos_compute_pid (diff_theta, &pos_theta); + pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha); /* Update PWM. */ pwm_left = pid_theta - pid_alpha; UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); @@ -135,14 +117,14 @@ pos_update (void) } } -/** Reset position control internal state. To be called when the position - * control is deactivated. */ +/** Reset position control state. To be called when the position control is + * deactivated. */ void -pos_reset (void) +pos_reset (struct pos_t *pos) { - pos_theta_int = pos_alpha_int = 0; - pos_theta_cur = pos_alpha_cur = 0; - pos_theta_cons = pos_alpha_cons = 0; - pos_theta_e_old = pos_alpha_e_old = 0; + pos->i = 0; + pos->cur = 0; + pos->cons = 0; + pos->e_old = 0; } diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h index ec6d93c9..1859d178 100644 --- a/digital/asserv/src/asserv/pos.h +++ b/digital/asserv/src/asserv/pos.h @@ -25,23 +25,31 @@ * * }}} */ -extern uint32_t pos_theta_cur, pos_alpha_cur; -extern uint32_t pos_theta_cons, pos_alpha_cons; +/** Position control state. */ +struct pos_t +{ + /** Current position. */ + uint32_t cur; + /** Consign position. */ + uint32_t cons; + /** PID coefficients (f8.8, maximum depends on saturation values). */ + uint16_t kp, ki, kd; + /** Current integral value. */ + int32_t i; + /** Last error value. */ + int32_t e_old; +}; + +extern struct pos_t pos_theta, pos_alpha; extern int32_t pos_e_sat; extern int32_t pos_int_sat; -extern uint16_t pos_theta_kp, pos_alpha_kp; -extern uint16_t pos_theta_ki, pos_alpha_ki; -extern uint16_t pos_theta_kd, pos_alpha_kd; extern int32_t pos_blocked; -extern int32_t pos_theta_int, pos_alpha_int; -extern int32_t pos_theta_e_old, pos_alpha_e_old; - void pos_update (void); void -pos_reset (void); +pos_reset (struct pos_t *pos); #endif /* pos_h */ diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index 887fab11..c2106909 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -96,8 +96,8 @@ speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max) static void speed_update_by_position (void) { - int32_t theta_d = speed_theta_pos_cons - pos_theta_cons; - int32_t alpha_d = speed_alpha_pos_cons - pos_alpha_cons; + 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; else @@ -122,7 +122,7 @@ speed_update (void) else speed_update_by_speed (); /* Update shaft position. */ - pos_theta_cons += speed_theta_cur >> 8; - pos_alpha_cons += speed_alpha_cur >> 8; + pos_theta.cons += speed_theta_cur >> 8; + pos_alpha.cons += speed_alpha_cur >> 8; } -- cgit v1.2.3