summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv')
-rw-r--r--n/asserv/src/speed.c65
1 files changed, 40 insertions, 25 deletions
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 7fdb0dc..8c67207 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -35,16 +35,18 @@ uint8_t speed_acc = 8;
uint8_t speed_acc_cpt;
/** Integral term. */
int16_t speed_left_int, speed_right_int;
-/** Integral max value. */
+/** Integral max value. 1024 maximum. */
int16_t speed_int_max = 1024;
/** Last error value. */
int16_t speed_left_e_old, speed_right_e_old;
-/** P coeficients. 5.8 fixed point format. */
+/** P coeficients. 4.8 fixed point format. */
uint16_t speed_kp = 5 * 255;
-/** I coeficients. 4.8 fixed point format. */
+/** I coeficients. 3.8 fixed point format. */
uint16_t speed_ki = 1 * 255;
-/** Dead zone. */
-uint16_t speed_dead_zone = 0;
+/** D coeficients. 3.8 fixed point format. */
+uint16_t speed_kd = 1 * 255;
+/** Counter value wanted. */
+uint16_t speed_left_counter, speed_right_counter;
/* +AutoDec */
@@ -107,27 +109,34 @@ speed_update (void)
speed_left = speed_left_aim;
speed_right = speed_right_aim;
}
+ /* Update counter. */
+ speed_left_counter += speed_left;
+ speed_right_counter += speed_right;
}
/** Compute new pwm value for left motor. */
static inline void
speed_compute_left_pwm (void)
{
- int16_t e;
- int16_t pwm;
- e = speed_left - counter_left_diff; /* 10b = 8b + 9b */
- /* Dead zone. */
- if (speed_dead_zone > e && e > -speed_dead_zone)
- e = 0;
+ int16_t e, diff, pwm;
+ /* Get and saturate error. */
+ e = speed_left_counter - counter_left; /* 11b */
+ if (e > 1024)
+ e = 1024;
+ else if (e < -1024)
+ e = -1024;
/* Integral update. */
speed_left_int += e; /* 12b = 11b + 10b */
if (speed_left_int > speed_int_max) /* 11b */
speed_left_int = speed_int_max;
else if (speed_left_int < -speed_int_max)
speed_left_int = -speed_int_max;
- /* Compute PI. */ /* 16b = 15b + 15b */
- pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 10b * 5.8b */
- + dsp_mul_i16f88 (speed_left_int, speed_ki); /* 15b = 11b * 4.8b */
+ /* Differential value. */
+ diff = e - speed_left_e_old; /* 11b */
+ /* Compute PI. */ /* 16b = 15b + 14b + 14b */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 11b * 4.8b */
+ + dsp_mul_i16f88 (speed_left_int, speed_ki) /* 14b = 11b * 3.8b */
+ + dsp_mul_i16f88 (diff, speed_kd); /* 14b = 11b * 3.8b */
/* Save result. */
speed_left_e_old = e;
pwm_left = pwm;
@@ -137,21 +146,25 @@ speed_compute_left_pwm (void)
static inline void
speed_compute_right_pwm (void)
{
- int16_t e;
- int16_t pwm;
- e = speed_right - counter_right_diff;
- /* Dead zone. */
- if (speed_dead_zone > e && e > -speed_dead_zone)
- e = 0;
+ int16_t e, diff, pwm;
+ /* Get and saturate error. */
+ e = speed_right_counter - counter_right; /* 11b */
+ if (e > 1024)
+ e = 1024;
+ else if (e < -1024)
+ e = -1024;
/* Integral update. */
- speed_right_int += e;
- if (speed_right_int > speed_int_max)
+ speed_right_int += e; /* 12b = 11b + 10b */
+ if (speed_right_int > speed_int_max) /* 11b */
speed_right_int = speed_int_max;
else if (speed_right_int < -speed_int_max)
speed_right_int = -speed_int_max;
- /* Compute PI. */
- pwm = dsp_mul_i16f88 (e, speed_kp)
- + dsp_mul_i16f88 (speed_right_int, speed_ki);
+ /* Differential value. */
+ diff = e - speed_right_e_old; /* 11b */
+ /* Compute PI. */ /* 16b = 15b + 14b + 14b */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 11b * 4.8b */
+ + dsp_mul_i16f88 (speed_right_int, speed_ki) /* 14b = 11b * 3.8b */
+ + dsp_mul_i16f88 (diff, speed_kd); /* 14b = 11b * 3.8b */
/* Save result. */
speed_right_e_old = e;
pwm_right = pwm;
@@ -166,6 +179,8 @@ speed_restart (void)
speed_right_int = 0;
speed_left = 0;
speed_right = 0;
+ speed_left_counter = counter_left;
+ speed_right_counter = counter_right;
}
/** Set maximum speed in order to be able to break before a distance and