From 980e5558dc43b992116ab6ca90309ddf62579e5c Mon Sep 17 00:00:00 2001 From: dufourj Date: Thu, 25 May 2006 13:39:05 +0000 Subject: Asserv : - correction du bug de ack. --- n/asserv/src/asserv/main.c | 2 +- n/asserv/src/asserv/speed.c | 20 ++++++++++++-------- 2 files changed, 13 insertions(+), 9 deletions(-) (limited to 'n/asserv/src/asserv') 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; } -- cgit v1.2.3