summaryrefslogtreecommitdiff
path: root/digital/asserv/src
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-17 19:36:50 +0200
committerNicolas Schodet2008-04-17 19:36:50 +0200
commit2a4a75a37a4a82d0a5e300b43b40d1ce8b0fb5c7 (patch)
tree5d7f787d1de5e32a5859c108b63559ed2329d7a7 /digital/asserv/src
parent0db1380594157e8b15c62b46bdd75ebd010dc9dc (diff)
* digital/asserv/src/asserv:
- added differential part saturation to increase proportionnal part.
Diffstat (limited to 'digital/asserv/src')
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c6
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c8
-rw-r--r--digital/asserv/src/asserv/pos.c13
-rw-r--r--digital/asserv/src/asserv/pos.h3
5 files changed, 20 insertions, 12 deletions
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