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, 12 insertions, 8 deletions
diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c
index 596cf35..7e8ed55 100644
--- a/n/asserv/src/asserv/speed.c
+++ b/n/asserv/src/asserv/speed.c
@@ -91,14 +91,18 @@ speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
static void
speed_update_by_position (void)
{
- speed_theta_cur =
- speed_compute_max_speed (speed_theta_pos_cons - pos_theta_cons,
- 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_cur, speed_alpha_acc,
- speed_alpha_max);
+ int32_t theta_d = speed_theta_pos_cons - pos_theta_cons;
+ int32_t alpha_d = speed_alpha_pos_cons - pos_alpha_cons;
+ if (theta_d >= -speed_theta_max && theta_d <= speed_theta_max)
+ speed_theta_cur = theta_d << 8;
+ else
+ speed_theta_cur = speed_compute_max_speed (theta_d, speed_theta_cur,
+ speed_theta_acc, speed_theta_max);
+ if (alpha_d >= -speed_alpha_max && alpha_d <= speed_alpha_max)
+ speed_alpha_cur = alpha_d << 8;
+ else
+ speed_alpha_cur = speed_compute_max_speed (alpha_d, speed_alpha_cur,
+ speed_alpha_acc, speed_alpha_max);
if (speed_theta_cur == 0 && speed_alpha_cur == 0)
main_sequence_finish = main_sequence;
}