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.c17
1 files changed, 10 insertions, 7 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index 1a5e73f..dc8cc85 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -29,6 +29,8 @@ int32_t goto_x, goto_y;
int32_t goto_a;
/** Destination epsillon. */
int32_t goto_eps = 20L << 8;
+/** Debug values. */
+int32_t goto_dx, goto_dy, goto_dl, goto_da;
/* +AutoDec */
/* -AutoDec */
@@ -37,11 +39,12 @@ int32_t goto_eps = 20L << 8;
static inline void
goto_update (void)
{
- int32_t dx, dy, c, s, dl, da;
+ int32_t c, s;
/* Project in the robot base. */
- dx = goto_x - postrack_x; /* f24.8 */
- dy = goto_y - postrack_y;
- if (dx < goto_eps && dx > -goto_eps && dy < goto_eps && dy > -goto_eps)
+ goto_dx = goto_x - postrack_x; /* f24.8 */
+ goto_dy = goto_y - postrack_y;
+ if (goto_dx < goto_eps && goto_dx > -goto_eps
+ && goto_dy < goto_eps && goto_dy > -goto_eps)
{
speed_left_aim = 0;
speed_right_aim = 0;
@@ -50,11 +53,11 @@ goto_update (void)
{
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);
+ goto_dl = dsp_mul_f824 (goto_dx, c) + dsp_mul_f824 (goto_dy, s);
+ goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s);
/* Convert da into a arc. This is a rough aproximation. */
//da = da * postrack_footing / 2 / (dl >> 8);
- speed_distance (dl, da);
+ speed_distance (goto_dl, goto_da);
}
}