summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/speed.c
diff options
context:
space:
mode:
authorschodet2006-03-26 14:16:20 +0000
committerschodet2006-03-26 14:16:20 +0000
commit98f731d45cb51040bb6ee364920cb0de34939e0a (patch)
treece769578f3fac0c4925de5d5d99625ce00272fb7 /n/asserv/src/asserv/speed.c
parenta0966ea8b0ed34d2a95a33c311e60a9a331290b8 (diff)
Ajout du déplacement avec consigne donnée en position, controlé en vitesse.
Ajout des stats sur la vitesse. Ajout d'explications sur le PID. Correction de la formule de tension dans la simulation à cause du mode PWM fast.
Diffstat (limited to 'n/asserv/src/asserv/speed.c')
-rw-r--r--n/asserv/src/asserv/speed.c35
1 files changed, 31 insertions, 4 deletions
diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c
index d354307..324e231 100644
--- a/n/asserv/src/asserv/speed.c
+++ b/n/asserv/src/asserv/speed.c
@@ -31,7 +31,7 @@
/** Current speed, f8.8. */
int16_t speed_theta_cur, speed_alpha_cur;
-/** Consign speed, f8.8. */
+/** Consign speed or maximum speed for position consign, f8.8. */
int16_t speed_theta_cons, speed_alpha_cons;
/** Consign position. */
uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
@@ -63,11 +63,38 @@ speed_update_by_speed (void)
speed_alpha_cur -= speed_alpha_acc;
}
+/** Compute maximum allowed speed according to: distance left, maximum speed
+ * consign, current speed and acceleration. */
+static int16_t
+speed_compute_max_speed (int32_t d, int16_t cons, int16_t cur, int16_t acc)
+{
+ int16_t s;
+ /* Compute maximum speed in order to be able to brake in time.
+ * s = sqrt (2 * a * d) */
+ s = fixed_sqrt_ui32 (2 * (acc >> 8) * UTILS_ABS (d));
+ /* Apply consign. */
+ s = UTILS_MIN (cons >> 8, s);
+ /* Apply sign. */
+ if (d < 0)
+ s = -s;
+ /* Convert to f8.8 and check acceleration. */
+ s = s << 8;
+ UTILS_BOUND (s, cur - acc, cur + acc);
+ return s;
+}
+
/** Update shaft position consign according to a position consign. */
static void
speed_update_by_position (void)
{
- // TODO
+ speed_theta_cur =
+ speed_compute_max_speed (speed_theta_pos_cons - pos_theta_cons,
+ speed_theta_cons, speed_theta_cur,
+ speed_theta_acc);
+ speed_alpha_cur =
+ speed_compute_max_speed (speed_alpha_pos_cons - pos_alpha_cons,
+ speed_alpha_cons, speed_alpha_cur,
+ speed_alpha_acc);
}
/** Update shaft position consign according to consign. */
@@ -80,7 +107,7 @@ speed_update (void)
else
speed_update_by_speed ();
/* Update shaft position. */
- pos_theta_cur += speed_theta_cur >> 8;
- pos_alpha_cur += speed_alpha_cur >> 8;
+ pos_theta_cons += speed_theta_cur >> 8;
+ pos_alpha_cons += speed_alpha_cur >> 8;
}