summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/pos.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/asserv/pos.c')
-rw-r--r--n/asserv/src/asserv/pos.c27
1 files changed, 15 insertions, 12 deletions
diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c
index d1c7e8f..b6bee63 100644
--- a/n/asserv/src/asserv/pos.c
+++ b/n/asserv/src/asserv/pos.c
@@ -32,35 +32,35 @@
*/
/** Current theta/alpha. */
-uint16_t pos_theta_cur, pos_alpha_cur;
+uint32_t pos_theta_cur, pos_alpha_cur;
/** Consign theta/alpha. */
-uint16_t pos_theta_cons, pos_alpha_cons;
+uint32_t pos_theta_cons, pos_alpha_cons;
/** Error saturation. */
-int16_t pos_e_sat = 1023;
+int32_t pos_e_sat = 1023;
/** Integral saturation. */
-int16_t pos_int_sat = 8191;
+int32_t pos_int_sat = 1023;
/** P coefficients. */
-uint16_t pos_theta_kp = 0, pos_alpha_kp = 0;
+uint16_t pos_theta_kp = 1200, pos_alpha_kp = 1500;
/** I coefficients. */
-uint16_t pos_theta_ki = 0, pos_alpha_ki = 0;
+uint16_t pos_theta_ki = 120, pos_alpha_ki = 150;
/** D coefficients. */
-uint16_t pos_theta_kd = 0, pos_alpha_kd = 0;
+uint16_t pos_theta_kd = 2000, pos_alpha_kd = 2000;
/** Current integral values. */
-int16_t pos_theta_int, pos_alpha_int;
+int32_t pos_theta_int, pos_alpha_int;
/** Last error values. */
-int16_t pos_theta_e_old, pos_alpha_e_old;
+int32_t pos_theta_e_old, pos_alpha_e_old;
/* +AutoDec */
/* -AutoDec */
/** Compute a PID. */
static inline int16_t
-pos_compute_pid (int16_t e, int16_t *i, int16_t *e_old,
+pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old,
uint16_t kp, uint16_t ki, uint16_t kd)
{
- int16_t diff, pid;
+ int32_t diff, pid;
/* Saturate error. */
UTILS_BOUND (e, -pos_e_sat, pos_e_sat);
/* Integral update. */
@@ -72,7 +72,7 @@ pos_compute_pid (int16_t e, int16_t *i, int16_t *e_old,
pid = e * kp + *i * ki + diff * kd;
/* Save result. */
*e_old = e;
- return pid;
+ return pid >> 12;
}
/** Update PWM according to consign. */
@@ -102,4 +102,7 @@ static void
pos_reset (void)
{
pos_theta_int = pos_alpha_int = 0;
+ pos_theta_cur = pos_alpha_cur = 0;
+ pos_theta_cons = pos_alpha_cons = 0;
+ pos_theta_e_old = pos_alpha_e_old = 0;
}