summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/goto.c24
-rw-r--r--n/asserv/src/main.c8
2 files changed, 23 insertions, 9 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index 75948a8..1a5e73f 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -27,6 +27,8 @@
int32_t goto_x, goto_y;
/** Destination angle, f8.24. */
int32_t goto_a;
+/** Destination epsillon. */
+int32_t goto_eps = 20L << 8;
/* +AutoDec */
/* -AutoDec */
@@ -39,12 +41,20 @@ goto_update (void)
/* Project in the robot base. */
dx = goto_x - postrack_x; /* f24.8 */
dy = goto_y - postrack_y;
- c = dsp_cos (postrack_a);
- s = dsp_sin (postrack_a);
- dl = dsp_mul_f824 (dx, c) + dsp_mul_f824 (dy, s);
- da = dsp_mul_f824 (dy, c) - dsp_mul_f824 (dx, s);
- /* Convert da into a arc. This is a rough aproximation. */
- da = da * postrack_footing / 2 / (dl >> 8);
- speed_distance (dl, da);
+ if (dx < goto_eps && dx > -goto_eps && dy < goto_eps && dy > -goto_eps)
+ {
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+ }
+ else
+ {
+ c = dsp_cos (postrack_a);
+ s = dsp_sin (postrack_a);
+ dl = dsp_mul_f824 (dx, c) + dsp_mul_f824 (dy, s);
+ da = dsp_mul_f824 (dy, c) - dsp_mul_f824 (dx, s);
+ /* Convert da into a arc. This is a rough aproximation. */
+ //da = da * postrack_footing / 2 / (dl >> 8);
+ speed_distance (dl, da);
+ }
}
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index bac4cfd..9033372 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -146,8 +146,8 @@ main_loop (void)
}
if (motor_stat_timer && !--motor_stat_timer_cpt)
{
- proto_send4b ('T', motor_timer_3, motor_timer_2, motor_timer_1,
- motor_timer_0);
+ proto_send5b ('T', motor_timer_4, motor_timer_3, motor_timer_2,
+ motor_timer_1, motor_timer_0);
motor_stat_timer_cpt = motor_stat_timer;
}
/* Misc. */
@@ -205,6 +205,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_left = args[0] << 8 | args[1];
pwm_right = args[2] << 8 | args[3];
break;
+ case c ('e', 4):
+ goto_eps = (uint32_t) args[0] << 24 | (uint32_t) args[1] << 16
+ | args[2] << 8 | args[3];
+ break;
case c ('a', 1):
speed_acc_cpt = speed_acc = args[0];
break;