summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv/pos.c
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/asserv/pos.c
parent0db1380594157e8b15c62b46bdd75ebd010dc9dc (diff)
* digital/asserv/src/asserv:
- added differential part saturation to increase proportionnal part.
Diffstat (limited to 'digital/asserv/src/asserv/pos.c')
-rw-r--r--digital/asserv/src/asserv/pos.c13
1 files changed, 8 insertions, 5 deletions
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. */