summaryrefslogtreecommitdiff
path: root/n/asserv/src/speed.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/speed.c')
-rw-r--r--n/asserv/src/speed.c95
1 files changed, 62 insertions, 33 deletions
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 8050b29..06b3abc 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -27,24 +27,34 @@
int8_t speed_left, speed_right;
/** Wanted speed. */
int8_t speed_left_aim, speed_right_aim;
-/** Max speed. */
-int8_t speed_max = 0x20;
+/** Max speed for automatic movements. */
+int8_t speed_max = 0x10;
/** Acceleration value. */
-uint8_t speed_acc = 8;
+uint8_t speed_acc = 0x40;
/** Acceleration counter, speed gets updated when it reachs 0. */
uint8_t speed_acc_cpt;
+/** Error saturation. */
+int16_t speed_e_sat = 1023;
+/** Integral saturation. */
+int16_t speed_int_sat = 8191;
+/** P coeficients. 4.8 fixed point format. */
+uint16_t speed_kp = 0x0fff;
+/** I coeficients. 3.8 fixed point format. */
+uint16_t speed_ki = 0x0008;
+/** D coeficients. 3.8 fixed point format. */
+uint16_t speed_kd = 0x0fff;
+/** D sample period. */
+uint8_t speed_ds = 2;
/** Integral term. */
int16_t speed_left_int, speed_right_int;
-/** 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. 4.8 fixed point format. */
-uint16_t speed_kp = 0x0200;
-/** I coeficients. 3.8 fixed point format. */
-uint16_t speed_ki = 0x0004;
-/** D coeficients. 3.8 fixed point format. */
-uint16_t speed_kd = 0x0100;
+/** D history. */
+int16_t speed_left_d_hist[16], speed_right_d_hist[16];
+/** D history pointer. */
+uint8_t speed_d_hist_ptr;
+/** D history sum. */
+int16_t speed_left_d_hist_sum, speed_right_d_hist_sum;
/** Counter value wanted. */
uint16_t speed_left_counter, speed_right_counter;
/** Do not update counter. */
@@ -126,22 +136,28 @@ speed_compute_left_pwm (void)
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;
+ if (e > speed_e_sat)
+ e = speed_e_sat;
+ else if (e < -speed_e_sat)
+ e = -speed_e_sat;
/* 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;
+ if (speed_left_int > speed_int_sat) /* 11b */
+ speed_left_int = speed_int_sat;
+ else if (speed_left_int < -speed_int_sat)
+ speed_left_int = -speed_int_sat;
/* Differential value. */
diff = e - speed_left_e_old; /* 11b */
+ speed_left_d_hist_sum += diff;
+ speed_left_d_hist_sum -= speed_left_d_hist[speed_d_hist_ptr];
+ speed_left_d_hist[speed_d_hist_ptr] = diff;
+ speed_d_hist_ptr++;
+ if (speed_d_hist_ptr >= speed_ds)
+ speed_d_hist_ptr = 0;
/* 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 */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b */
+ + dsp_mul_i16f88 (speed_left_int, speed_ki) /* 14b */
+ + dsp_mul_i16f88 (speed_left_d_hist_sum, speed_kd); /* 14b */
/* Save result. */
speed_left_e_old = e;
pwm_left = pwm;
@@ -154,22 +170,25 @@ speed_compute_right_pwm (void)
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;
+ if (e > speed_e_sat)
+ e = speed_e_sat;
+ else if (e < -speed_e_sat)
+ e = -speed_e_sat;
/* Integral update. */
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;
+ if (speed_right_int > speed_int_sat) /* 11b */
+ speed_right_int = speed_int_sat;
+ else if (speed_right_int < -speed_int_sat)
+ speed_right_int = -speed_int_sat;
/* Differential value. */
diff = e - speed_right_e_old; /* 11b */
+ speed_right_d_hist_sum += diff;
+ speed_right_d_hist_sum -= speed_right_d_hist[speed_d_hist_ptr];
+ speed_right_d_hist[speed_d_hist_ptr] = diff;
/* 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 */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b */
+ + dsp_mul_i16f88 (speed_right_int, speed_ki) /* 14b */
+ + dsp_mul_i16f88 (speed_right_d_hist_sum, speed_kd); /* 14b */
/* Save result. */
speed_right_e_old = e;
pwm_right = pwm;
@@ -180,12 +199,22 @@ speed_compute_right_pwm (void)
static inline void
speed_restart (void)
{
+ int i;
speed_left_int = 0;
speed_right_int = 0;
speed_left = 0;
speed_right = 0;
speed_left_counter = counter_left;
speed_right_counter = counter_right;
+ speed_left_d_hist_sum = 0;
+ speed_right_d_hist_sum = 0;
+ speed_d_hist_ptr = 0;
+ for (i = 0; i < sizeof (speed_left_d_hist)
+ / sizeof (speed_left_d_hist[0]); i++)
+ {
+ speed_left_d_hist[i] = 0;
+ speed_right_d_hist[i] = 0;
+ }
}
/** Set maximum speed in order to be able to break before a distance and