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.c122
1 files changed, 111 insertions, 11 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index d35f9fa..fdc5953 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -27,6 +27,7 @@
* 0: linear move.
* 1: angular move.
* 2: position move.
+ * 4: counter move.
* 10: we fuck the wall.
*/
uint8_t goto_mode;
@@ -34,7 +35,9 @@ uint8_t goto_mode;
uint16_t goto_d;
/** Sign of movement (0: forward, 1 backward). */
uint8_t goto_sign;
-/** Destination position, f24.8. */
+/** Counter aim. */
+int16_t goto_counter_left, goto_counter_right;
+/** Destination/start position, f24.8. */
int32_t goto_x, goto_y;
/** Destination angle, f0.8. */
int8_t goto_a;
@@ -51,11 +54,11 @@ uint8_t goto_finish;
/* -AutoDec */
/** Linear control mode. */
-static inline void
+static void
goto_linear_mode (void)
{
uint16_t d;
- uint32_t dx, dy;
+ int32_t dx, dy;
/* Compute distance from the start point.
* WARNING: this could overflow as dsp_hypot accept a maximum value of
* +/- 65535. */
@@ -80,8 +83,22 @@ goto_linear_mode (void)
}
}
-/** Angular control mode. */
+/** Setup linear mode. */
static inline void
+goto_linear (int16_t d)
+{
+ motor_mode = 2;
+ goto_mode = 0;
+ goto_sign = d >> 15;
+ goto_d = d;
+ if (goto_sign) goto_d = -goto_d;
+ goto_x = postrack_x;
+ goto_y = postrack_y;
+ goto_finish = 0;
+}
+
+/** Angular control mode. */
+static void
goto_angular_mode (void)
{
int32_t angle_diff;
@@ -90,7 +107,7 @@ goto_angular_mode (void)
angle_diff <<= 8;
angle_diff >>= 8;
/* Small angles. */
- if (0x10000L > angle_diff && angle_diff > -0x10000L)
+ if (0x1000L > angle_diff && angle_diff > -0x1000L)
{
speed_left = 0;
speed_right = 0;
@@ -107,11 +124,22 @@ goto_angular_mode (void)
}
}
-/** Position control mode. */
+/** Setup angular mode. */
static inline void
+goto_angular (int8_t a)
+{
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = a;
+ goto_finish = 0;
+}
+
+/** Position control mode. */
+static void
goto_position_mode (void)
{
int32_t c, s, arc;
+ int32_t dla, daa;
goto_dx = goto_x - postrack_x; /* f24.8 */
goto_dy = goto_y - postrack_y;
if (goto_dx < goto_eps && goto_dx > -goto_eps
@@ -130,33 +158,63 @@ goto_position_mode (void)
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);
+ if (goto_dl > 0)
+ dla = goto_dl;
+ else
+ dla = -goto_dl;
+ if (goto_da > 0)
+ daa = goto_da;
+ else
+ daa = -goto_da;
/* If very big angle (> 83 ), rotate. */
- if (goto_da > goto_dl * 8)
+ if (daa > dla * 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)
+ else if (daa * 32 > dla)
{
/* 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)
+ else if (daa * 128 < dla)
{
speed_distance (goto_dl, 0);
}
- /* Else, curve. TODO. */
+ /* Else, curve. */
else
{
speed_distance (goto_dl, 0);
+ if ((goto_da >= 0 && goto_dl >= 0)
+ || (goto_da < 0 && goto_dl < 0))
+ {
+ speed_left_aim--;
+ speed_right_aim++;
+ }
+ else
+ {
+ speed_left_aim++;
+ speed_right_aim--;
+ }
}
}
}
+/** Setup position mode. */
+static inline void
+goto_position (int32_t x, int32_t y)
+{
+ motor_mode = 2;
+ goto_mode = 2;
+ goto_x = x;
+ goto_y = y;
+ goto_finish = 0;
+}
+
/** Position control mode, `exact' method. */
static inline void
goto_position_exact_mode (void)
@@ -184,8 +242,37 @@ goto_position_exact_mode (void)
}
}
-/** We fuck the wall mode. */
+/** Counter control mode. */
+static void
+goto_counter_mode (void)
+{
+ int16_t dl, dr;
+ int32_t dl248, dr248;
+ dl = goto_counter_left - counter_left;
+ dr = goto_counter_right - counter_right;
+ if (dl < 3 && dl > -3
+ && dr < 3 && dr > -3)
+ {
+ goto_finish = 1;
+ }
+ dl248 = dl;
+ dr248 = dr;
+ speed_distance_lr (dl248 << 8, dr248 << 8);
+}
+
+/** Setup counter mode. */
static inline void
+goto_counter (int16_t l, int16_t r)
+{
+ motor_mode = 2;
+ goto_mode = 4;
+ goto_counter_left = speed_left_counter + l;
+ goto_counter_right = speed_right_counter + r;
+ goto_finish = 0;
+}
+
+/** We fuck the wall mode. */
+static void
goto_ftw_mode (void)
{
/* Change speed. */
@@ -211,6 +298,16 @@ goto_ftw_mode (void)
goto_finish = 1;
}
+/** Setup ftw mode. */
+static inline void
+goto_ftw (int8_t s)
+{
+ motor_mode = 2;
+ goto_mode = 10;
+ goto_s = s;
+ goto_finish = 0;
+}
+
/** Update the speed according to the desired destination. */
static void
goto_update (void)
@@ -226,6 +323,9 @@ goto_update (void)
case 2:
goto_position_mode ();
break;
+ case 4:
+ goto_counter_mode ();
+ break;
case 10:
goto_ftw_mode ();
break;