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/main.c | 9 +++++++++ digital/asserv/src/asserv/postrack.c | 5 +---- digital/asserv/src/asserv/traj.c | 22 +++++++++++++++++++++- digital/asserv/src/asserv/traj.h | 3 +++ digital/asserv/src/asserv/twi_proto.c | 16 +--------------- 5 files changed, 35 insertions(+), 20 deletions(-) (limited to 'digital') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index e96ebf90..99854d77 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -312,6 +312,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_aux0.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); state_start (&state_aux0, args[4]); break; + case c ('a', 5): + /* Set angular speed controlled position consign. + * - d: angle offset. + * - b: sequence number. */ + if (args[4] == state_main.sequence) + break; + traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2], + args[3]), 0); + break; case c ('f', 1): /* Go to the wall. * - b: sequence number. */ diff --git a/digital/asserv/src/asserv/postrack.c b/digital/asserv/src/asserv/postrack.c index 47a86015..a3b2e8cb 100644 --- a/digital/asserv/src/asserv/postrack.c +++ b/digital/asserv/src/asserv/postrack.c @@ -26,6 +26,7 @@ #include "postrack.h" #include "modules/math/fixed/fixed.h" +#include "modules/math/math.h" #include "counter.h" @@ -54,8 +55,6 @@ postrack_init (void) postrack_set_footing (0x1000); } -#define M_PI 3.14159265358979323846 /* pi */ - /** Update the current position. */ void postrack_update (void) @@ -93,8 +92,6 @@ postrack_update (void) } } -#define M_1_PI 0.31830988618379067154 /* 1/pi */ - /** Change the footing value. */ void postrack_set_footing (uint16_t footing) 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) diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index fc127d60..2eaf9a0e 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -30,4 +30,7 @@ extern uint8_t traj_mode; void traj_update (void); +void +traj_angle_offset_start (int32_t angle, uint8_t seq); + #endif /* traj_h */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 5b398996..9b3d8f36 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -28,7 +28,6 @@ #include "modules/utils/utils.h" #include "modules/utils/byte.h" #include "modules/twi/twi.h" -#include "modules/math/fixed/fixed.h" #include "io.h" #include "misc.h" @@ -40,8 +39,6 @@ #include "postrack.h" #include "traj.h" -#define M_PI 3.14159265358979323846 /* pi */ - struct twi_proto_t { u8 seq; @@ -132,18 +129,7 @@ twi_proto_callback (u8 *buf, u8 size) case c ('a', 2): /* Set angular speed controlled position consign. * - w: angle offset. */ - { - int32_t a = ((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8; - a = fixed_mul_f824 (a, 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, 0); - } + traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8, 0); break; case c ('f', 0): /* Go to the wall. */ -- cgit v1.2.3