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.c85
1 files changed, 78 insertions, 7 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index dc8cc85..bd57d85 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -23,10 +23,20 @@
*
* }}} */
+/** Goto mode:
+ * 0: linear move.
+ * 1: angular move.
+ * 2: position move.
+ */
+uint8_t goto_mode;
+/** Distance for mode 0, ui16. */
+uint16_t goto_d;
+/** Sign of movement (0: forward, 1 backward). */
+uint8_t goto_sign;
/** Destination position, f24.8. */
int32_t goto_x, goto_y;
-/** Destination angle, f8.24. */
-int32_t goto_a;
+/** Destination angle, f0.8. */
+int8_t goto_a;
/** Destination epsillon. */
int32_t goto_eps = 20L << 8;
/** Debug values. */
@@ -35,9 +45,52 @@ int32_t goto_dx, goto_dy, goto_dl, goto_da;
/* +AutoDec */
/* -AutoDec */
-/** Update the speed according to the desired destination. */
+/** Linear control mode. */
static inline void
-goto_update (void)
+goto_linear_mode (void)
+{
+ uint16_t d;
+ uint32_t dx, dy;
+ /* Compute distance from the start point.
+ * WARNING: this could overflow as dsp_hypot accept a maximum value of
+ * +/- 65535. */
+ dx = goto_x - postrack_x;
+ dy = goto_y - postrack_y;
+ d = dsp_hypot (dx, dy);
+ /* Change speed. */
+ if (d > goto_d)
+ {
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+ }
+ else
+ {
+ /* Convert back to f24.8. */
+ int32_t com = goto_d - d;
+ com <<= 8;
+ speed_distance (goto_sign ? -com : com, 0);
+ }
+}
+
+/** Angular control mode. */
+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);
+}
+
+/** Position control mode. */
+static inline void
+goto_position_mode (void)
{
int32_t c, s;
/* Project in the robot base. */
@@ -51,13 +104,31 @@ goto_update (void)
}
else
{
- c = dsp_cos (postrack_a);
- s = dsp_sin (postrack_a);
+ 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. */
- //da = da * postrack_footing / 2 / (dl >> 8);
+ goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8);
speed_distance (goto_dl, goto_da);
}
}
+/** Update the speed according to the desired destination. */
+static inline void
+goto_update (void)
+{
+ switch (goto_mode)
+ {
+ case 0:
+ goto_linear_mode ();
+ break;
+ case 1:
+ goto_angular_mode ();
+ break;
+ case 2:
+ goto_position_mode ();
+ break;
+ }
+}
+