summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/speed.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/asserv/speed.c')
-rw-r--r--n/asserv/src/asserv/speed.c20
1 files changed, 11 insertions, 9 deletions
diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c
index 324e231..eb9b568 100644
--- a/n/asserv/src/asserv/speed.c
+++ b/n/asserv/src/asserv/speed.c
@@ -31,8 +31,10 @@
/** Current speed, f8.8. */
int16_t speed_theta_cur, speed_alpha_cur;
-/** Consign speed or maximum speed for position consign, f8.8. */
+/** Consign speed, f8.8. */
int16_t speed_theta_cons, speed_alpha_cons;
+/** Maximum speed for position consign, u8. */
+int8_t speed_theta_max, speed_alpha_max;
/** Consign position. */
uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
/** Weither to use the consign position (1) or not (0). */
@@ -63,17 +65,17 @@ 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. */
+/** Compute maximum allowed speed according to: distance left, maximum speed,
+ * current speed and acceleration. */
static int16_t
-speed_compute_max_speed (int32_t d, int16_t cons, int16_t cur, int16_t acc)
+speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
{
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);
+ s = UTILS_MIN (max, s);
/* Apply sign. */
if (d < 0)
s = -s;
@@ -89,12 +91,12 @@ speed_update_by_position (void)
{
speed_theta_cur =
speed_compute_max_speed (speed_theta_pos_cons - pos_theta_cons,
- speed_theta_cons, speed_theta_cur,
- speed_theta_acc);
+ speed_theta_cur, speed_theta_acc,
+ speed_theta_max);
speed_alpha_cur =
speed_compute_max_speed (speed_alpha_pos_cons - pos_alpha_cons,
- speed_alpha_cons, speed_alpha_cur,
- speed_alpha_acc);
+ speed_alpha_cur, speed_alpha_acc,
+ speed_alpha_max);
}
/** Update shaft position consign according to consign. */