From 26e64bc1bb5786e4e3638382ae1a11f0fc011374 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 26 Feb 2005 20:16:03 +0000 Subject: Ajout de goto_eps --- n/asserv/src/goto.c | 24 +++++++++++++++++------- n/asserv/src/main.c | 8 ++++++-- 2 files changed, 23 insertions(+), 9 deletions(-) (limited to 'n/asserv') diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index 75948a8..1a5e73f 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -27,6 +27,8 @@ int32_t goto_x, goto_y; /** Destination angle, f8.24. */ int32_t goto_a; +/** Destination epsillon. */ +int32_t goto_eps = 20L << 8; /* +AutoDec */ /* -AutoDec */ @@ -39,12 +41,20 @@ goto_update (void) /* Project in the robot base. */ dx = goto_x - postrack_x; /* f24.8 */ dy = goto_y - postrack_y; - c = dsp_cos (postrack_a); - s = dsp_sin (postrack_a); - dl = dsp_mul_f824 (dx, c) + dsp_mul_f824 (dy, s); - da = dsp_mul_f824 (dy, c) - dsp_mul_f824 (dx, s); - /* Convert da into a arc. This is a rough aproximation. */ - da = da * postrack_footing / 2 / (dl >> 8); - speed_distance (dl, da); + if (dx < goto_eps && dx > -goto_eps && dy < goto_eps && dy > -goto_eps) + { + speed_left_aim = 0; + speed_right_aim = 0; + } + else + { + c = dsp_cos (postrack_a); + s = dsp_sin (postrack_a); + dl = dsp_mul_f824 (dx, c) + dsp_mul_f824 (dy, s); + da = dsp_mul_f824 (dy, c) - dsp_mul_f824 (dx, s); + /* Convert da into a arc. This is a rough aproximation. */ + //da = da * postrack_footing / 2 / (dl >> 8); + speed_distance (dl, da); + } } diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index bac4cfd..9033372 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -146,8 +146,8 @@ main_loop (void) } if (motor_stat_timer && !--motor_stat_timer_cpt) { - proto_send4b ('T', motor_timer_3, motor_timer_2, motor_timer_1, - motor_timer_0); + proto_send5b ('T', motor_timer_4, motor_timer_3, motor_timer_2, + motor_timer_1, motor_timer_0); motor_stat_timer_cpt = motor_stat_timer; } /* Misc. */ @@ -205,6 +205,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_left = args[0] << 8 | args[1]; pwm_right = args[2] << 8 | args[3]; break; + case c ('e', 4): + goto_eps = (uint32_t) args[0] << 24 | (uint32_t) args[1] << 16 + | args[2] << 8 | args[3]; + break; case c ('a', 1): speed_acc_cpt = speed_acc = args[0]; break; -- cgit v1.2.3