summaryrefslogtreecommitdiff
path: root/n/asserv/src/speed.c
diff options
context:
space:
mode:
authorschodet2005-02-11 14:36:33 +0000
committerschodet2005-02-11 14:36:33 +0000
commite4589d3099a11723b3cd017c9c8c97b070455ea9 (patch)
treefe3db349538cac3fcb52ead0b5cbeb5887edbc1b /n/asserv/src/speed.c
parentc1e2d98380a352117a1e361dc09376b2195fd0db (diff)
Ajout de la première version d'asservissement en position.
Diffstat (limited to 'n/asserv/src/speed.c')
-rw-r--r--n/asserv/src/speed.c36
1 files changed, 36 insertions, 0 deletions
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 54962b3..64450f9 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -27,6 +27,8 @@
int8_t speed_left, speed_right;
/** Wanted speed. */
int8_t speed_left_aim, speed_right_aim;
+/** Max speed. */
+int8_t speed_max = 0x20;
/** Acceleration value. */
uint8_t speed_acc = 8;
/** Acceleration counter, speed gets updated when it reachs 0. */
@@ -157,3 +159,37 @@ speed_restart (void)
speed_left = 0;
speed_right = 0;
}
+
+/** Set maximum speed in order to be able to break before a distance and
+ * angle, inputs are f24.8 */
+static inline void
+speed_distance (int32_t dist, int32_t arc)
+{
+ /* XXX: There are some buggy assertions there if dist and arc are both
+ * big. */
+ uint8_t vls, vas;
+ uint32_t vl248, va248;
+ int8_t vl, va;
+ /* Drop sign. */
+ vls = dist < 0;
+ if (vls) dist = -dist;
+ vas = arc < 0;
+ if (vas) arc = -arc;
+ /* Compute speed.
+ * v = sqrt (2 * a * d), a = 1/speed_acc */
+ vl248 = dsp_sqrt (dist * 2 / speed_acc);
+ vl = (vl248 >> 8) & 0xff;
+ va248 = dsp_sqrt (arc * 2 / speed_acc);
+ va = (va248 >> 8) & 0xff;
+ /* Saturate. */
+ if (vl248 & 0xffff0000 || vl > speed_max)
+ vl = speed_max;
+ if (va248 & 0xffff0000 || va > speed_max)
+ va = speed_max;
+ /* Get sign back. */
+ if (vls) vl = -vl;
+ if (vas) va = -va;
+ speed_left_aim = vl - va;
+ speed_right_aim = vl + va;
+}
+