summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/dsp.c23
-rw-r--r--n/asserv/src/goto.c50
-rw-r--r--n/asserv/src/main.c41
-rw-r--r--n/asserv/src/speed.c36
4 files changed, 143 insertions, 7 deletions
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
index 4484b2f..5bbcf65 100644
--- a/n/asserv/src/dsp.c
+++ b/n/asserv/src/dsp.c
@@ -345,3 +345,26 @@ dsp_sin (int32_t a)
return -dsp_cos_dli (a - (1L << 23) - (1L << 22));
}
+/** Compute square root, uf24.8. */
+uint32_t
+dsp_sqrt (uint32_t x)
+{
+ uint32_t root, bit, test;
+ root = 0;
+ bit = 1L << 30;
+ do
+ {
+ test = root + bit;
+ //printf ("test = 0x%x, root = 0x%x, bit = 0x%x\n", test, root, bit);
+ if (x >= test)
+ {
+ x -= test;
+ root = test + bit;
+ //printf ("x = 0x%x, root = 0x%x\n", x, root);
+ }
+ root >>= 1;
+ bit >>= 2;
+ } while (bit);
+ return root << 4;
+}
+
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
new file mode 100644
index 0000000..75948a8
--- /dev/null
+++ b/n/asserv/src/goto.c
@@ -0,0 +1,50 @@
+/* goto.c */
+/* asserv - Position & speed motor control on a ATmega128. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2005.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/** Destination position, f24.8. */
+int32_t goto_x, goto_y;
+/** Destination angle, f8.24. */
+int32_t goto_a;
+
+/* +AutoDec */
+/* -AutoDec */
+
+/** Update the speed according to the desired destination. */
+static inline void
+goto_update (void)
+{
+ int32_t dx, dy, c, s, dl, da;
+ /* Project in the robot base. */
+ dx = goto_x - postrack_x; /* f24.8 */
+ dy = goto_y - postrack_y;
+ c = dsp_cos (postrack_a);
+ s = dsp_sin (postrack_a);
+ dl = dsp_mul_f824 (dx, c) + dsp_mul_f824 (dy, s);
+ da = dsp_mul_f824 (dy, c) - dsp_mul_f824 (dx, s);
+ /* Convert da into a arc. This is a rough aproximation. */
+ da = da * postrack_footing / 2 / (dl >> 8);
+ speed_distance (dl, da);
+}
+
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 3fed431..bac4cfd 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -34,6 +34,7 @@
#include "counter.c"
#include "speed.c"
#include "postrack.c"
+#include "goto.c"
/** Motor mode :
* 0 - pwm setup;
@@ -61,7 +62,8 @@ uint8_t motor_stat_postrack, motor_stat_postrack_cpt;
/** Record timer value at different stage of computing. Used for performance
* analisys. */
-uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3;
+uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3,
+ motor_timer_4;
/* +AutoDec */
@@ -98,8 +100,15 @@ main_loop (void)
while (!timer_pending ())
counter_poll ();
counter_update ();
- motor_timer_3 = timer_read ();
+ motor_timer_4 = timer_read ();
+ /* Position tracking. */
postrack_update ();
+ motor_timer_3 = timer_read ();
+ /* Position control. */
+ if (motor_mode >= 2)
+ {
+ goto_update ();
+ }
motor_timer_2 = timer_read ();
/* Speed control. */
if (motor_mode >= 1)
@@ -161,6 +170,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('z', 0):
reset ();
break;
+ case c ('g', 8):
+ motor_mode = 2;
+ goto_x = (uint32_t) args[0] << 24 | (uint32_t) args[1] << 16
+ | args[2] << 8 | args[3];
+ goto_y = (uint32_t) args[4] << 24 | (uint32_t) args[5] << 16
+ | args[6] << 8 | args[7];
+ break;
+ case c ('s', 0):
+ motor_mode = 1;
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+ break;
+ case c ('s', 2):
+ motor_mode = 1;
+ speed_left_aim = args[0];
+ speed_right_aim = args[1];
+ break;
+ case c ('s', 3):
+ motor_mode = 1;
+ speed_left_aim = args[0];
+ speed_right_aim = args[1];
+ speed_max = args[2];
+ break;
case c ('w', 0):
speed_restart ();
motor_mode = 0;
@@ -173,11 +205,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_left = args[0] << 8 | args[1];
pwm_right = args[2] << 8 | args[3];
break;
- case c ('s', 2):
- motor_mode = 1;
- speed_left_aim = args[0];
- speed_right_aim = args[1];
- break;
case c ('a', 1):
speed_acc_cpt = speed_acc = args[0];
break;
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;
+}
+