From 2a4a75a37a4a82d0a5e300b43b40d1ce8b0fb5c7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 17 Apr 2008 19:36:50 +0200 Subject: * digital/asserv/src/asserv: - added differential part saturation to increase proportionnal part. --- digital/asserv/src/asserv/eeprom.avr.c | 6 ++++-- digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 8 ++++++-- digital/asserv/src/asserv/pos.c | 13 ++++++++----- digital/asserv/src/asserv/pos.h | 3 +-- 5 files changed, 20 insertions(+), 12 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 4277ceac..aee320fb 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -94,7 +94,8 @@ eeprom_read_params (void) pos_alpha.kd = eeprom_read_word (p16++); pos_aux0.kd = eeprom_read_word (p16++); pos_e_sat = eeprom_read_word (p16++); - pos_int_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++); traj_eps = eeprom_read_word (p16++); } @@ -129,7 +130,8 @@ eeprom_write_params (void) eeprom_write_word (p16++, pos_alpha.kd); eeprom_write_word (p16++, pos_aux0.kd); eeprom_write_word (p16++, pos_e_sat); - eeprom_write_word (p16++, pos_int_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++, traj_eps); } diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index c3a57075..b603942d 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 0x48 +#define EEPROM_KEY 0x49 void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 5913c107..99c0e369 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -507,7 +507,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pos_e_sat = v8_to_v16 (args[1], args[2]); break; case c ('I', 3): - pos_int_sat = v8_to_v16 (args[1], args[2]); + pos_i_sat = v8_to_v16 (args[1], args[2]); + break; + 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]); @@ -545,7 +548,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) proto_send1w ('i', pos_aux0.ki); proto_send1w ('d', pos_aux0.kd); proto_send1w ('E', pos_e_sat); - proto_send1w ('I', pos_int_sat); + proto_send1w ('I', pos_i_sat); + proto_send1w ('D', pos_d_sat); proto_send1w ('b', pos_blocked); proto_send1w ('e', traj_eps); proto_send1b ('w', pwm_reverse); diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index 6c317766..02cde731 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -49,7 +49,9 @@ struct pos_t pos_aux0; /** Error saturation. */ int32_t pos_e_sat = 1023; /** Integral saturation. */ -int32_t pos_int_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; @@ -58,8 +60,8 @@ int32_t pos_blocked = 15000L; * How to compute maximum numbers size: * Result is 24 bits (16 bits kept after shift). * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff - * max is 12 bits. - * If int_sat == 1023, i max is 11 bits. + * max is 12 bits (can be saturated with d_sat). + * If i_sat == 1023, i max is 11 bits. * In the final addition, let's give 23 bits to the p part, and 22 bits to the * i and d part (23b + 22b + 22b => 23b + 23b => 24b). * Therefore, kp can be 23 - 11 = 12 bits (f4.8). @@ -68,7 +70,7 @@ int32_t pos_blocked = 15000L; * How to increase this number: * - lower the shift. * - bound the value returned. - * - lower e & int saturation. */ + * - lower e, i & d saturation. */ static inline int16_t pos_compute_pid (int32_t e, struct pos_t *pos) { @@ -77,9 +79,10 @@ pos_compute_pid (int32_t e, struct pos_t *pos) UTILS_BOUND (e, -pos_e_sat, pos_e_sat); /* Integral update. */ pos->i += e; - UTILS_BOUND (pos->i, -pos_int_sat, pos_int_sat); + UTILS_BOUND (pos->i, -pos_i_sat, pos_i_sat); /* Differential value. */ diff = e - pos->e_old; + UTILS_BOUND (diff, -pos_d_sat, pos_d_sat); /* Compute PID. */ pid = e * pos->kp + pos->i * pos->ki + diff * pos->kd; /* Save result. */ diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h index d28db334..4f29965c 100644 --- a/digital/asserv/src/asserv/pos.h +++ b/digital/asserv/src/asserv/pos.h @@ -43,8 +43,7 @@ struct pos_t extern struct pos_t pos_theta, pos_alpha; extern struct pos_t pos_aux0; -extern int32_t pos_e_sat; -extern int32_t pos_int_sat; +extern int32_t pos_e_sat, pos_i_sat, pos_d_sat; extern int32_t pos_blocked; void -- cgit v1.2.3