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.c31
1 files changed, 26 insertions, 5 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index d833204..d35f9fa 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -111,8 +111,7 @@ goto_angular_mode (void)
static inline void
goto_position_mode (void)
{
- int32_t c, s;
- /* Project in the robot base. */
+ int32_t c, s, arc;
goto_dx = goto_x - postrack_x; /* f24.8 */
goto_dy = goto_y - postrack_y;
if (goto_dx < goto_eps && goto_dx > -goto_eps
@@ -126,13 +125,35 @@ goto_position_mode (void)
}
else
{
+ /* Project in the robot base. */
c = dsp_cos_f824 (postrack_a);
s = dsp_sin_f824 (postrack_a);
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. */
- goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8);
- speed_distance (goto_dl / 2, goto_da * 2);
+ /* If very big angle (> 83 ), rotate. */
+ if (goto_da > goto_dl * 8)
+ {
+ /* Compute arc. */
+ arc = postrack_footing_2pi / 4;
+ speed_distance (0, goto_da > 0 ? arc : -arc);
+ }
+ /* If big angle (> 1.7), rotate. */
+ else if (goto_da * 32 > goto_dl)
+ {
+ /* Compute arc. This is a rough aproximation. */
+ arc = goto_da / (goto_dl >> 8) * (postrack_footing / 2);
+ speed_distance (0, arc);
+ }
+ /* Is small angle (< 0.44), strait ahead. */
+ else if (goto_da * 128 < goto_dl)
+ {
+ speed_distance (goto_dl, 0);
+ }
+ /* Else, curve. TODO. */
+ else
+ {
+ speed_distance (goto_dl, 0);
+ }
}
}