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.c57
1 files changed, 57 insertions, 0 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index bd57d85..8aa5e91 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.
+ * 10: we fuck the wall.
*/
uint8_t goto_mode;
/** Distance for mode 0, ui16. */
@@ -37,6 +38,8 @@ uint8_t goto_sign;
int32_t goto_x, goto_y;
/** Destination angle, f0.8. */
int8_t goto_a;
+/** Travel speed for fixed speed movements, i8. */
+int8_t goto_s;
/** Destination epsillon. */
int32_t goto_eps = 20L << 8;
/** Debug values. */
@@ -110,10 +113,61 @@ goto_position_mode (void)
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);
+ }
+}
+
+/** Position control mode, `exact' method. */
+static inline void
+goto_position_exact_mode (void)
+{
+ int32_t c, s;
+ /* Project in the robot base. */
+ 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;
+ }
+ else
+ {
+ 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, goto_da);
}
}
+/** We fuck the wall mode. */
+static inline void
+goto_ftw_mode (void)
+{
+ /* Change speed. */
+ if (PINC & _BV (0))
+ {
+ speed_left_aim = goto_s;
+ }
+ else
+ {
+ speed_left = 0;
+ speed_left_aim = 0;
+ }
+ if (PINC & _BV (7))
+ {
+ speed_right_aim = goto_s;
+ }
+ else
+ {
+ speed_right = 0;
+ speed_right_aim = 0;
+ }
+}
+
/** Update the speed according to the desired destination. */
static inline void
goto_update (void)
@@ -129,6 +183,9 @@ goto_update (void)
case 2:
goto_position_mode ();
break;
+ case 10:
+ goto_ftw_mode ();
+ break;
}
}