summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/main.c9
-rw-r--r--digital/asserv/src/asserv/postrack.c5
-rw-r--r--digital/asserv/src/asserv/traj.c22
-rw-r--r--digital/asserv/src/asserv/traj.h3
-rw-r--r--digital/asserv/src/asserv/twi_proto.c16
5 files changed, 35 insertions, 20 deletions
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. */