From 339aef5ad2cbc67e7bd6e8b968fe9e373c4654e8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 3 May 2008 13:33:08 +0200 Subject: * digital/asserv/src/asserv: - added better blocking detection. --- digital/asserv/src/asserv/eeprom.avr.c | 8 ++++++-- digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 10 +++++++--- digital/asserv/src/asserv/pos.c | 32 ++++++++++++++++++++++++++------ digital/asserv/src/asserv/pos.h | 7 ++++++- 5 files changed, 46 insertions(+), 13 deletions(-) diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 0c91fbc0..0cb04aae 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -96,7 +96,9 @@ eeprom_read_params (void) pos_e_sat = eeprom_read_word (p16++); pos_i_sat = eeprom_read_word (p16++); pos_d_sat = eeprom_read_word (p16++); - pos_blocked = 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++)); @@ -134,7 +136,9 @@ eeprom_write_params (void) 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); + 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 c9c0a158..30d59780 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 0x4b +#define EEPROM_KEY 0x4c void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 2ea3d95d..228426d6 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -577,8 +577,10 @@ 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', 3): - pos_blocked = v8_to_v16 (args[1], args[2]); + 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]); @@ -619,7 +621,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) proto_send1w ('E', pos_e_sat); proto_send1w ('I', pos_i_sat); proto_send1w ('D', pos_d_sat); - proto_send1w ('b', pos_blocked); + 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 9109e165..af3d0315 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -52,9 +52,12 @@ int32_t pos_e_sat = 1023; int32_t pos_i_sat = 1023; /** Differential saturation. */ int32_t pos_d_sat = 1023; -/** Blocked value. If error is greater than this value, stop the robot and - * report blocked state. */ -int32_t pos_blocked = 15000L; +/** 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: @@ -108,8 +111,23 @@ pos_update (void) /* Compute PID. */ diff_theta = pos_theta.cons - pos_theta.cur; diff_alpha = pos_alpha.cons - pos_alpha.cur; - if (diff_theta < -pos_blocked || pos_blocked < diff_theta - || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) + /* 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)) + 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)) + 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) { /* Blocked. */ pos_reset (&pos_theta); @@ -135,7 +153,7 @@ pos_update (void) pos_aux0.cur += counter_aux0_diff; /* Compute PID. */ diff = pos_aux0.cons - pos_aux0.cur; - if (diff < -pos_blocked || pos_blocked < diff) + if (UTILS_ABS (diff) > 5000) { /* Blocked. */ pos_reset (&pos_aux0); @@ -160,5 +178,7 @@ 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 4f29965c..cf02cda3 100644 --- a/digital/asserv/src/asserv/pos.h +++ b/digital/asserv/src/asserv/pos.h @@ -38,13 +38,18 @@ struct pos_t int32_t i; /** Last error value. */ int32_t e_old; + /** Old current position. */ + uint32_t cur_old; + /** Count the number of blocked detection. */ + uint8_t blocked_counter; }; extern struct pos_t pos_theta, pos_alpha; extern struct pos_t pos_aux0; extern int32_t pos_e_sat, pos_i_sat, pos_d_sat; -extern int32_t pos_blocked; +extern int32_t pos_blocked_error_limit, pos_blocked_speed_limit, + pos_blocked_counter_limit; void pos_update (void); -- cgit v1.2.3