summaryrefslogtreecommitdiff
path: root/n/asserv/src/goto.c
diff options
context:
space:
mode:
authorschodet2005-04-17 02:53:57 +0000
committerschodet2005-04-17 02:53:57 +0000
commit0fcaebc243f457973fcb1a83fe8a00705be72f20 (patch)
treed67cb1830d90708534b72496319eb97d7fb46b6a /n/asserv/src/goto.c
parent71564bd2908facb66710ab7454ad5b00025f21f6 (diff)
PINC -> PINA.
Move_angular ok. Sharps en twi.
Diffstat (limited to 'n/asserv/src/goto.c')
-rw-r--r--n/asserv/src/goto.c39
1 files changed, 25 insertions, 14 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index c427af9..553a4f4 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -41,7 +41,7 @@ int8_t goto_a;
/** Travel speed for fixed speed movements, i8. */
int8_t goto_s;
/** Destination epsillon. */
-int32_t goto_eps = 20L << 8;
+int32_t goto_eps = 200L << 8;
/** Debug values. */
int32_t goto_dx, goto_dy, goto_dl, goto_da;
/** Movement finished. */
@@ -82,16 +82,27 @@ goto_linear_mode (void)
static inline void
goto_angular_mode (void)
{
- int8_t angle_diff;
- int32_t arc;
- /* Compute angle diff. This works because a full circle is 256. */
- angle_diff = goto_a - v32_to_v8 (postrack_a, 2);
- /* Compute arc. */
- arc = angle_diff; /* i8 */
- arc *= postrack_footing / 2; /* i24 */
- arc <<= 8; /* f24.8 */
- /* Change speed. */
- speed_distance (0, arc);
+ int32_t angle_diff;
+ /* Compute angle diff. */
+ angle_diff = v8_to_v32 (0, goto_a, 0, 0) - postrack_a;
+ angle_diff <<= 8;
+ angle_diff >>= 8;
+ /* Small angles. */
+ if (0x10000 > angle_diff && angle_diff < -0x10000)
+ {
+ goto_finish = 1;
+ speed_left = 0;
+ speed_right = 0;
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+ }
+ else
+ {
+ /* Compute arc. */
+ goto_da = dsp_mul_f824 (angle_diff, postrack_footing_2pi);
+ /* Set speed. */
+ speed_distance (0, goto_da);
+ }
}
/** Position control mode. */
@@ -153,7 +164,7 @@ static inline void
goto_ftw_mode (void)
{
/* Change speed. */
- if (PINC & _BV (0))
+ if (PINA & _BV (0))
{
speed_left_aim = goto_s;
}
@@ -162,7 +173,7 @@ goto_ftw_mode (void)
speed_left = 0;
speed_left_aim = 0;
}
- if (PINC & _BV (7))
+ if (PINA & _BV (7))
{
speed_right_aim = goto_s;
}
@@ -171,7 +182,7 @@ goto_ftw_mode (void)
speed_right = 0;
speed_right_aim = 0;
}
- if (!(PINC & (_BV (0) || _BV (7))))
+ if (!(PINA & (_BV (0) || _BV (7))))
goto_finish = 1;
}