From e668c747f1e5787b4cb8ed2627abbc975d75eff2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 5 Apr 2008 11:38:48 +0200 Subject: * digital/asserv/src/asserv: - angle offset clean up. - added angle offset proto command. --- digital/asserv/src/asserv/traj.c | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) (limited to 'digital/asserv/src/asserv/traj.c') diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 226430a2..3b8bc7d3 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -25,11 +25,16 @@ #include "common.h" #include "traj.h" +#include "modules/math/fixed/fixed.h" +#include "modules/math/math.h" #include "io.h" -#include "speed.h" #include "state.h" +#include "pos.h" +#include "speed.h" +#include "postrack.h" + #ifdef HOST # include "simu.host.h" #endif @@ -41,6 +46,21 @@ */ uint8_t traj_mode; +/** Angle offset. Directly handled to speed layer. */ +void +traj_angle_offset_start (int32_t angle, uint8_t seq) +{ + int32_t a = fixed_mul_f824 (angle, 2 * M_PI * (1L << 24)); + uint32_t f = postrack_footing; + int32_t arc = fixed_mul_f824 (f, a); + state_main.mode = MODE_SPEED; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + speed_alpha.pos_cons += arc; + state_start (&state_main, seq); +} + /** Go to the wall mode. */ static void traj_ftw (void) -- cgit v1.2.3