summaryrefslogtreecommitdiff
path: root/digital/asserv/src/asserv/traj.c
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-05 11:38:48 +0200
committerNicolas Schodet2008-04-05 11:38:48 +0200
commite668c747f1e5787b4cb8ed2627abbc975d75eff2 (patch)
treeaa9a390f9fdfed24f2ec2e7ec00617c5fa5d2916 /digital/asserv/src/asserv/traj.c
parent8c570ab124d33abbf4d4798fbbcb04ab24b98998 (diff)
* digital/asserv/src/asserv:
- angle offset clean up. - added angle offset proto command.
Diffstat (limited to 'digital/asserv/src/asserv/traj.c')
-rw-r--r--digital/asserv/src/asserv/traj.c22
1 files changed, 21 insertions, 1 deletions
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)