summaryrefslogtreecommitdiff
path: root/n/asserv/src/speed.c
diff options
context:
space:
mode:
authorschodet2005-05-03 01:26:58 +0000
committerschodet2005-05-03 01:26:58 +0000
commit74550d3214bca333dcbcd73b75a1e7b535c7e1f0 (patch)
tree58a9a4a54db4cc884bc062050eb93a62d8f77c4e /n/asserv/src/speed.c
parentdaea50b1df19aea257018e4725463f33ce09b937 (diff)
Ajout d'une vitesse max l et a.
Ajout des coefs l et r. Ajout de goto_counter. Ajout des fonctions de goto setup.
Diffstat (limited to 'n/asserv/src/speed.c')
-rw-r--r--n/asserv/src/speed.c105
1 files changed, 78 insertions, 27 deletions
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 06b3abc..dd326a6 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -28,9 +28,9 @@ int8_t speed_left, speed_right;
/** Wanted speed. */
int8_t speed_left_aim, speed_right_aim;
/** Max speed for automatic movements. */
-int8_t speed_max = 0x10;
+int8_t speed_max_l = 0x08, speed_max_a = 0x05;
/** Acceleration value. */
-uint8_t speed_acc = 0x40;
+uint8_t speed_acc = 0x60;
/** Acceleration counter, speed gets updated when it reachs 0. */
uint8_t speed_acc_cpt;
/** Error saturation. */
@@ -38,11 +38,11 @@ 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;
+uint16_t speed_left_kp = 0x0fff, speed_right_kp = 0x0fff;
/** I coeficients. 3.8 fixed point format. */
-uint16_t speed_ki = 0x0008;
+uint16_t speed_left_ki = 0x0008, speed_right_ki = 0x0008;
/** D coeficients. 3.8 fixed point format. */
-uint16_t speed_kd = 0x0fff;
+uint16_t speed_left_kd = 0x0fff, speed_right_kd = 0x0fff;
/** D sample period. */
uint8_t speed_ds = 2;
/** Integral term. */
@@ -83,6 +83,16 @@ speed_compute_right_pwm (void);
static inline void
speed_restart (void);
+/** Set maximum speed in order to be able to brake before a distance and
+ * angle, inputs are f24.8 */
+static inline void
+speed_distance (int32_t dist, int32_t arc);
+
+/** Set maximum left & right speeds in order to be able to brake. Inputs are
+ * f24.8 */
+static inline void
+speed_distance_lr (int32_t left, int32_t right);
+
/* -AutoDec */
/** Initialise speed parameters. */
@@ -95,6 +105,7 @@ speed_init (void)
static inline void
speed_update (void)
{
+ /* Accelerate. */
if (speed_acc)
{
speed_acc_cpt--;
@@ -127,6 +138,13 @@ speed_update (void)
speed_left_counter += speed_left;
speed_right_counter += speed_right;
}
+ /* Compute pwm. */
+ speed_compute_left_pwm ();
+ speed_compute_right_pwm ();
+ /* Update D history pointer. */
+ speed_d_hist_ptr++;
+ if (speed_d_hist_ptr >= speed_ds)
+ speed_d_hist_ptr = 0;
}
/** Compute new pwm value for left motor. */
@@ -135,29 +153,26 @@ speed_compute_left_pwm (void)
{
int16_t e, diff, pwm;
/* Get and saturate error. */
- e = speed_left_counter - counter_left; /* 11b */
+ e = speed_left_counter - counter_left;
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_sat) /* 11b */
+ speed_left_int += e;
+ if (speed_left_int > speed_int_sat)
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 */
+ diff = e - speed_left_e_old;
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 */
- + dsp_mul_i16f88 (speed_left_int, speed_ki) /* 14b */
- + dsp_mul_i16f88 (speed_left_d_hist_sum, speed_kd); /* 14b */
+ pwm = dsp_mul_i16f88 (e, speed_left_kp)
+ + dsp_mul_i16f88 (speed_left_int, speed_left_ki)
+ + dsp_mul_i16f88 (speed_left_d_hist_sum, speed_left_kd);
/* Save result. */
speed_left_e_old = e;
pwm_left = pwm;
@@ -169,26 +184,26 @@ speed_compute_right_pwm (void)
{
int16_t e, diff, pwm;
/* Get and saturate error. */
- e = speed_right_counter - counter_right; /* 11b */
+ e = speed_right_counter - counter_right;
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_sat) /* 11b */
+ speed_right_int += e;
+ if (speed_right_int > speed_int_sat)
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 */
+ diff = e - speed_right_e_old;
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 */
- + dsp_mul_i16f88 (speed_right_int, speed_ki) /* 14b */
- + dsp_mul_i16f88 (speed_right_d_hist_sum, speed_kd); /* 14b */
+ pwm = dsp_mul_i16f88 (e, speed_right_kp)
+ + dsp_mul_i16f88 (speed_right_int, speed_right_ki)
+ + dsp_mul_i16f88 (speed_right_d_hist_sum, speed_right_kd);
/* Save result. */
speed_right_e_old = e;
pwm_right = pwm;
@@ -217,7 +232,7 @@ speed_restart (void)
}
}
-/** Set maximum speed in order to be able to break before a distance and
+/** Set maximum speed in order to be able to brake before a distance and
* angle, inputs are f24.8 */
static inline void
speed_distance (int32_t dist, int32_t arc)
@@ -239,10 +254,10 @@ speed_distance (int32_t dist, int32_t arc)
va248 = dsp_sqrt_f248 (arc * 2 / speed_acc);
va = (va248 >> 8) & 0xff;
/* Saturate. */
- if (vl248 & 0xffff0000 || vl > speed_max)
- vl = speed_max;
- if (va248 & 0xffff0000 || va > speed_max)
- va = speed_max;
+ if (vl248 & 0xffff0000 || vl > speed_max_l)
+ vl = speed_max_l;
+ if (va248 & 0xffff0000 || va > speed_max_a)
+ va = speed_max_a;
/* Get sign back. */
if (vls) vl = -vl;
if (vas) va = -va;
@@ -250,3 +265,39 @@ speed_distance (int32_t dist, int32_t arc)
speed_right_aim = vl + va;
}
+/** Set maximum left & right speeds in order to be able to brake. Inputs are
+ * f24.8 */
+static inline void
+speed_distance_lr (int32_t left, int32_t right)
+{
+ uint8_t vls, vrs;
+ uint32_t vl248, vr248;
+ int8_t vl, vr;
+ uint8_t max;
+ /* Drop sign. */
+ vls = left < 0;
+ if (vls) left = -left;
+ vrs = right < 0;
+ if (vrs) right = -right;
+ /* Compute speed.
+ * v = sqrt (2 * a * d), a = 1/speed_acc */
+ vl248 = dsp_sqrt_f248 (left * 2 / speed_acc);
+ vl = (vl248 >> 8) & 0xff;
+ vr248 = dsp_sqrt_f248 (right * 2 / speed_acc);
+ vr = (vr248 >> 8) & 0xff;
+ /* Saturate. */
+ if (vls != vrs)
+ max = speed_max_l;
+ else
+ max = speed_max_a;
+ if (vl248 & 0xffff0000 || vl > max)
+ vl = max;
+ if (vr248 & 0xffff0000 || vr > max)
+ vr = max;
+ /* Get sign back. */
+ if (vls) vl = -vl;
+ if (vrs) vr = -vr;
+ speed_left_aim = vl;
+ speed_right_aim = vr;
+}
+