summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--n/asserv/src/asserv/main.c2
-rw-r--r--n/asserv/src/asserv/speed.c20
2 files changed, 13 insertions, 9 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index a58b4b6..8382b39 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -150,7 +150,7 @@ main_loop (void)
if (main_sequence_ack != main_sequence_finish && !--main_sequence_ack_cpt)
{
proto_send1b ('A', main_sequence_finish);
- main_sequence_ack_cpt = 2;
+ main_sequence_ack_cpt = 4;
}
if (main_stat_counter && !--main_stat_counter_cpt)
{
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;
}