From cab14d84256329f483c53f658a205415a5c029f4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 3 May 2009 00:19:36 +0200 Subject: * digital/asserv: - changed blocking detection. --- digital/asserv/src/asserv/eeprom.avr.c | 30 ++++++++++++---- digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 31 ++++++++++++----- digital/asserv/src/asserv/pos.c | 62 +++++++++++++++++----------------- digital/asserv/src/asserv/pos.h | 8 +++-- digital/asserv/tools/asserv/asserv.py | 8 +++-- 6 files changed, 91 insertions(+), 50 deletions(-) diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index cbb92b33..ddc10f3c 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -99,12 +99,21 @@ eeprom_read_params (void) pos_alpha.kd = eeprom_read_word (p16++); pos_aux[0].kd = eeprom_read_word (p16++); pos_aux[1].kd = eeprom_read_word (p16++); + pos_theta.blocked_error_limit = eeprom_read_word (p16++); + pos_theta.blocked_speed_limit = eeprom_read_word (p16++); + pos_theta.blocked_counter_limit = eeprom_read_word (p16++); + pos_alpha.blocked_error_limit = eeprom_read_word (p16++); + pos_alpha.blocked_speed_limit = eeprom_read_word (p16++); + pos_alpha.blocked_counter_limit = eeprom_read_word (p16++); + pos_aux[0].blocked_error_limit = eeprom_read_word (p16++); + pos_aux[0].blocked_speed_limit = eeprom_read_word (p16++); + pos_aux[0].blocked_counter_limit = eeprom_read_word (p16++); + pos_aux[1].blocked_error_limit = eeprom_read_word (p16++); + pos_aux[1].blocked_speed_limit = eeprom_read_word (p16++); + pos_aux[1].blocked_counter_limit = 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++); - pos_blocked_error_limit = eeprom_read_word (p16++); - pos_blocked_speed_limit = eeprom_read_word (p16++); - pos_blocked_counter_limit = eeprom_read_word (p16++); traj_eps = eeprom_read_word (p16++); traj_aeps = eeprom_read_word (p16++); traj_set_angle_limit (eeprom_read_word (p16++)); @@ -145,12 +154,21 @@ eeprom_write_params (void) eeprom_write_word (p16++, pos_alpha.kd); eeprom_write_word (p16++, pos_aux[0].kd); eeprom_write_word (p16++, pos_aux[1].kd); + eeprom_write_word (p16++, pos_theta.blocked_error_limit); + eeprom_write_word (p16++, pos_theta.blocked_speed_limit); + eeprom_write_word (p16++, pos_theta.blocked_counter_limit); + eeprom_write_word (p16++, pos_alpha.blocked_error_limit); + eeprom_write_word (p16++, pos_alpha.blocked_speed_limit); + eeprom_write_word (p16++, pos_alpha.blocked_counter_limit); + eeprom_write_word (p16++, pos_aux[0].blocked_error_limit); + eeprom_write_word (p16++, pos_aux[0].blocked_speed_limit); + eeprom_write_word (p16++, pos_aux[0].blocked_counter_limit); + eeprom_write_word (p16++, pos_aux[1].blocked_error_limit); + eeprom_write_word (p16++, pos_aux[1].blocked_speed_limit); + eeprom_write_word (p16++, pos_aux[1].blocked_counter_limit); eeprom_write_word (p16++, pos_e_sat); eeprom_write_word (p16++, pos_i_sat); eeprom_write_word (p16++, pos_d_sat); - eeprom_write_word (p16++, pos_blocked_error_limit); - eeprom_write_word (p16++, pos_blocked_speed_limit); - eeprom_write_word (p16++, pos_blocked_counter_limit); eeprom_write_word (p16++, traj_eps); eeprom_write_word (p16++, traj_aeps); eeprom_write_word (p16++, traj_angle_limit); diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index 052f37de..000a0e14 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 0x4d +#define EEPROM_KEY 0x4e void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 04352f3b..527b1940 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -630,6 +630,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) if (!pos) { proto_send0 ('?'); return; } pos->kd = v8_to_v16 (args[2], args[3]); break; + case c ('b', 7): + /* Set blocking detection parameters. + * - b: index. + * - 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]; + break; case c ('E', 3): pos_e_sat = v8_to_v16 (args[1], args[2]); break; @@ -639,11 +650,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('D', 3): pos_d_sat = v8_to_v16 (args[1], args[2]); break; - case c ('b', 7): - pos_blocked_error_limit = v8_to_v16 (args[1], args[2]); - pos_blocked_speed_limit = v8_to_v16 (args[3], args[4]); - pos_blocked_counter_limit = v8_to_v16 (args[5], args[6]); - break; case c ('e', 5): traj_eps = v8_to_v16 (args[1], args[2]); traj_aeps = v8_to_v16 (args[3], args[4]); @@ -672,21 +678,30 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) 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_send3w ('b', pos_blocked_error_limit, - pos_blocked_speed_limit, - pos_blocked_counter_limit); proto_send2w ('e', traj_eps, traj_aeps); proto_send1w ('l', traj_angle_limit); proto_send1b ('w', pwm_reverse); diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index 0679de40..dc995a16 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -52,12 +52,6 @@ int32_t pos_e_sat = 1023; int32_t pos_i_sat = 1023; /** Differential saturation. */ int32_t pos_d_sat = 1023; -/** Blocking detection: error limit. */ -int32_t pos_blocked_error_limit = 2048; -/** Blocking detection: speed limit. */ -int32_t pos_blocked_speed_limit = 0x10; -/** Blocking detection: counter limit. */ -int32_t pos_blocked_counter_limit = 20; /** Compute a PID. * How to compute maximum numbers size: @@ -103,34 +97,33 @@ pos_update_polar (struct state_t *state, if (state->mode >= MODE_POS) { int16_t pid_theta, pid_alpha; - int32_t diff_theta, diff_alpha; + int32_t error_theta, error_alpha; + int16_t cur_speed_theta, cur_speed_alpha; /* Update current shaft positions. */ - pos_theta->cur += counter_left_diff + counter_right_diff; - pos_alpha->cur += counter_right_diff - counter_left_diff; + cur_speed_theta = counter_left_diff + counter_right_diff; + cur_speed_alpha = counter_right_diff - counter_left_diff; + pos_theta->cur += cur_speed_theta; + pos_alpha->cur += cur_speed_alpha; if (state->variant & 1) pos_reset (pos_theta); if (state->variant & 2) pos_reset (pos_alpha); - /* Compute PID. */ - diff_theta = pos_theta->cons - pos_theta->cur; - diff_alpha = pos_alpha->cons - pos_alpha->cur; - /* Compute actual speed and test for blocking. */ - int32_t cur_speed_theta = pos_theta->cur - pos_theta->cur_old; - pos_theta->cur_old = pos_theta->cur; - int32_t cur_speed_alpha = pos_alpha->cur - pos_alpha->cur_old; - pos_alpha->cur_old = pos_alpha->cur; - if ((UTILS_ABS (diff_theta) > pos_blocked_error_limit - && UTILS_ABS (cur_speed_theta) < pos_blocked_speed_limit)) + /* Compute error. */ + error_theta = pos_theta->cons - pos_theta->cur; + error_alpha = pos_alpha->cons - pos_alpha->cur; + /* Test for blocking. */ + if (UTILS_ABS (error_theta) > pos_theta->blocked_error_limit + && UTILS_ABS (cur_speed_theta) < pos_theta->blocked_speed_limit) pos_theta->blocked_counter++; else pos_theta->blocked_counter = 0; - if ((UTILS_ABS (diff_alpha) > pos_blocked_error_limit - && UTILS_ABS (cur_speed_alpha) < pos_blocked_speed_limit)) + if (UTILS_ABS (error_alpha) > pos_alpha->blocked_error_limit + && UTILS_ABS (cur_speed_alpha) < pos_alpha->blocked_speed_limit) pos_alpha->blocked_counter++; else pos_alpha->blocked_counter = 0; - if (pos_theta->blocked_counter > pos_blocked_counter_limit - || pos_alpha->blocked_counter > pos_blocked_counter_limit) + if (pos_theta->blocked_counter > pos_theta->blocked_counter_limit + || pos_alpha->blocked_counter > pos_alpha->blocked_counter_limit) { /* Blocked. */ pos_reset (pos_theta); @@ -141,8 +134,9 @@ pos_update_polar (struct state_t *state, } else { - pid_theta = pos_compute_pid (diff_theta, pos_theta); - pid_alpha = pos_compute_pid (diff_alpha, pos_alpha); + /* Compute PID. */ + pid_theta = pos_compute_pid (error_theta, pos_theta); + pid_alpha = pos_compute_pid (error_alpha, pos_alpha); /* Update PWM. */ pwm_set (pwm_left, pid_theta - pid_alpha); pwm_set (pwm_right, pid_theta + pid_alpha); @@ -158,12 +152,18 @@ pos_update_single (struct state_t *state, struct pos_t *pos, if (state->mode >= MODE_POS) { int16_t pid; - int32_t diff; + int32_t error; /* Update current shaft position. */ pos->cur += counter_diff; - /* Compute PID. */ - diff = pos->cons - pos->cur; - if (UTILS_ABS (diff) > 5000) + /* Compute error. */ + error = pos->cons - pos->cur; + /* Test or blocking. */ + if (UTILS_ABS (error) > pos->blocked_error_limit + && UTILS_ABS (counter_diff) < pos->blocked_speed_limit) + pos->blocked_counter++; + else + pos->blocked_counter = 0; + if (pos->blocked_counter > pos->blocked_counter_limit) { /* Blocked. */ pos_reset (pos); @@ -172,7 +172,8 @@ pos_update_single (struct state_t *state, struct pos_t *pos, } else { - pid = pos_compute_pid (diff, pos); + /* Compute PID. */ + pid = pos_compute_pid (error, pos); /* Update PWM. */ pwm_set (pwm, pid); } @@ -201,7 +202,6 @@ pos_reset (struct pos_t *pos) pos->cur = 0; pos->cons = 0; pos->e_old = 0; - pos->cur_old = 0; pos->blocked_counter = 0; } diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h index 4ced49b8..5c586026 100644 --- a/digital/asserv/src/asserv/pos.h +++ b/digital/asserv/src/asserv/pos.h @@ -38,8 +38,12 @@ struct pos_t int32_t i; /** Last error value. */ int32_t e_old; - /** Old current position. */ - uint32_t cur_old; + /** Blocking detection: error limit. */ + int32_t blocked_error_limit; + /** Blocking detection: speed limit. */ + int16_t blocked_speed_limit; + /** Blocking detection: counter limit. */ + uint8_t blocked_counter_limit; /** Count the number of blocked detection. */ uint8_t blocked_counter; }; diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 323d74c6..5c7830c6 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -106,14 +106,17 @@ class Proto: scale = 1, tkp = 0, tki = 0, tkd = 0, ta = 1, tsm = 0, tss = 0, + tbe = 2048, tbs = 0x10, tbc = 20, akp = 0, aki = 0, akd = 0, aa = 1, asm = 0, ass = 0, + abe = 2048, abs = 0x10, abc = 20, a0kp = 0, a0ki = 0, a0kd = 0, a0a = 1, a0sm = 0, a0ss = 0, + a0be = 2048, a0bs = 0x10, a0bc = 20, a1kp = 0, a1ki = 0, a1kd = 0, a1a = 1, a1sm = 0, a1ss = 0, + a1be = 2048, a1bs = 0x10, a1bc = 20, E = 1023, I = 1023, D = 1023, - be = 2048, bs = 0x10, bc = 20, c = 1, f = 0x1000, l = 0x2000, w = 0x00, @@ -273,10 +276,11 @@ class Proto: self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + 'kd'])) self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + 'a'])) self.proto.send ('p', 'cBBB', 's', index, p[m + 'sm'], p[m + 'ss']) + self.proto.send ('p', 'cBHHB', 'b', index, p[m + 'be'], + p[m + 'bs'], p[m + 'bc']) self.proto.send ('p', 'cH', 'E', p['E']) self.proto.send ('p', 'cH', 'I', p['I']) self.proto.send ('p', 'cH', 'D', p['D']) - self.proto.send ('p', 'cHHH', 'b', p['be'], p['bs'], p['bc']) self.proto.send ('p', 'cL', 'c', f824 (p['c'])) self.proto.send ('p', 'cH', 'f', p['f']) self.proto.send ('p', 'cH', 'l', p['l']) -- cgit v1.2.3