summaryrefslogtreecommitdiff
path: root/n/asserv/src/goto.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/goto.c')
-rw-r--r--n/asserv/src/goto.c24
1 files changed, 17 insertions, 7 deletions
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);
+ }
}