summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv')
-rw-r--r--n/asserv/src/avrconfig.h2
-rw-r--r--n/asserv/src/dsp.c39
-rw-r--r--n/asserv/src/goto.c85
-rw-r--r--n/asserv/src/main.c81
-rw-r--r--n/asserv/src/postrack.c8
-rw-r--r--n/asserv/src/speed.c4
-rw-r--r--n/asserv/src/test_dsp.c10
7 files changed, 186 insertions, 43 deletions
diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h
index 769257c..be35d22 100644
--- a/n/asserv/src/avrconfig.h
+++ b/n/asserv/src/avrconfig.h
@@ -60,5 +60,7 @@
#define AC_PROTO_CALLBACK proto_callback
/** Putchar function name. */
#define AC_PROTO_PUTC rs232_putc
+/** Support for quote parameter. */
+#define AC_PROTO_QUOTE 1
#endif /* avrconfig_h */
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
index 5bbcf65..2c6b21f 100644
--- a/n/asserv/src/dsp.c
+++ b/n/asserv/src/dsp.c
@@ -39,7 +39,7 @@
*
* Angles are mapped from [0, 2pi) to [0,1). */
-/** Add two signed words (i16) and saturate. UNTESTED. */
+/** Add two signed words (i16) and saturate. UNUSED and UNTESTED. */
extern inline int16_t
dsp_add_sat_i16i16 (int16_t a, int16_t b)
{
@@ -315,7 +315,7 @@ dsp_cos_dli (int32_t a)
/** Compute cosinus, angle f8.24, result f8.24. */
int32_t
-dsp_cos (int32_t a)
+dsp_cos_f824 (int32_t a)
{
a &= (1L << 24) - 1;
uint8_t z = ((uint32_t) a >> 16) & 0xc0;
@@ -331,7 +331,7 @@ dsp_cos (int32_t a)
/** Compute sinus, angle f8.24, result f8.24. */
int32_t
-dsp_sin (int32_t a)
+dsp_sin_f824 (int32_t a)
{
a &= (1L << 24) - 1;
uint8_t z = ((uint32_t) a >> 16) & 0xc0;
@@ -347,7 +347,7 @@ dsp_sin (int32_t a)
/** Compute square root, uf24.8. */
uint32_t
-dsp_sqrt (uint32_t x)
+dsp_sqrt_f248 (uint32_t x)
{
uint32_t root, bit, test;
root = 0;
@@ -368,3 +368,34 @@ dsp_sqrt (uint32_t x)
return root << 4;
}
+/** Compute square root, ui32 -> ui16. */
+uint16_t
+dsp_sqrt_ui32 (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;
+}
+
+/** Compute hypothenuse, f24.8 -> ui16. Input limited to +-65535 */
+uint16_t
+dsp_hypot (int32_t dx, int32_t dy)
+{
+ dx >>= 8;
+ dy >>= 8;
+ return dsp_sqrt_ui32 (dx * dx + dy * dy);
+}
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index dc8cc85..bd57d85 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -23,10 +23,20 @@
*
* }}} */
+/** Goto mode:
+ * 0: linear move.
+ * 1: angular move.
+ * 2: position move.
+ */
+uint8_t goto_mode;
+/** Distance for mode 0, ui16. */
+uint16_t goto_d;
+/** Sign of movement (0: forward, 1 backward). */
+uint8_t goto_sign;
/** Destination position, f24.8. */
int32_t goto_x, goto_y;
-/** Destination angle, f8.24. */
-int32_t goto_a;
+/** Destination angle, f0.8. */
+int8_t goto_a;
/** Destination epsillon. */
int32_t goto_eps = 20L << 8;
/** Debug values. */
@@ -35,9 +45,52 @@ int32_t goto_dx, goto_dy, goto_dl, goto_da;
/* +AutoDec */
/* -AutoDec */
-/** Update the speed according to the desired destination. */
+/** Linear control mode. */
static inline void
-goto_update (void)
+goto_linear_mode (void)
+{
+ uint16_t d;
+ uint32_t dx, dy;
+ /* Compute distance from the start point.
+ * WARNING: this could overflow as dsp_hypot accept a maximum value of
+ * +/- 65535. */
+ dx = goto_x - postrack_x;
+ dy = goto_y - postrack_y;
+ d = dsp_hypot (dx, dy);
+ /* Change speed. */
+ if (d > goto_d)
+ {
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+ }
+ else
+ {
+ /* Convert back to f24.8. */
+ int32_t com = goto_d - d;
+ com <<= 8;
+ speed_distance (goto_sign ? -com : com, 0);
+ }
+}
+
+/** Angular control mode. */
+static inline void
+goto_angular_mode (void)
+{
+ int8_t angle_diff;
+ int32_t arc;
+ /* Compute angle diff. This works because a full circle is 256. */
+ angle_diff = goto_a - v32_to_v8 (postrack_a, 2);
+ /* Compute arc. */
+ arc = angle_diff; /* i8 */
+ arc *= postrack_footing / 2; /* i24 */
+ arc <<= 8; /* f24.8 */
+ /* Change speed. */
+ speed_distance (0, arc);
+}
+
+/** Position control mode. */
+static inline void
+goto_position_mode (void)
{
int32_t c, s;
/* Project in the robot base. */
@@ -51,13 +104,31 @@ goto_update (void)
}
else
{
- c = dsp_cos (postrack_a);
- s = dsp_sin (postrack_a);
+ c = dsp_cos_f824 (postrack_a);
+ s = dsp_sin_f824 (postrack_a);
goto_dl = dsp_mul_f824 (goto_dx, c) + dsp_mul_f824 (goto_dy, s);
goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s);
/* Convert da into a arc. This is a rough aproximation. */
- //da = da * postrack_footing / 2 / (dl >> 8);
+ goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8);
speed_distance (goto_dl, goto_da);
}
}
+/** Update the speed according to the desired destination. */
+static inline void
+goto_update (void)
+{
+ switch (goto_mode)
+ {
+ case 0:
+ goto_linear_mode ();
+ break;
+ case 1:
+ goto_angular_mode ();
+ break;
+ case 2:
+ goto_position_mode ();
+ break;
+ }
+}
+
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 2d21804..d119e4b 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -38,8 +38,8 @@
#include "goto.c"
/** Motor mode :
- * 0 - pwm setup;
- * 1 - speed control;
+ * 0 - pwm setup.
+ * 1 - speed control.
* 2 - position control. */
int8_t motor_mode;
@@ -180,8 +180,23 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
reset ();
break;
/* Commands. */
+ case c ('l', 2):
+ motor_mode = 2;
+ goto_mode = 0;
+ goto_sign = args[0] >> 7;
+ goto_d = v8_to_v16 (args[0], args[1]);
+ if (goto_sign) goto_d = -goto_d;
+ goto_x = postrack_x;
+ goto_y = postrack_y;
+ break;
+ case c ('a', 1):
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = args[0];
+ break;
case c ('g', 8):
motor_mode = 2;
+ goto_mode = 2;
goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]);
goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]);
break;
@@ -199,7 +214,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
motor_mode = 1;
speed_left_aim = args[0];
speed_right_aim = args[1];
- speed_max = args[2];
break;
case c ('w', 0):
speed_restart ();
@@ -213,22 +227,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_left = v8_to_v16 (args[0], args[1]);
pwm_right = v8_to_v16 (args[2], args[3]);
break;
- /* Params. */
- case c ('e', 4):
- goto_eps = v8_to_v32 (args[0], args[1], args[2], args[3]);
- break;
- case c ('a', 1):
- speed_acc_cpt = speed_acc = args[0];
- break;
- case c ('p', 2):
- speed_kp = v8_to_v16 (args[0], args[1]);
- break;
- case c ('i', 2):
- speed_ki = v8_to_v16 (args[0], args[1]);
- break;
- case c ('f', 2):
- postrack_set_footing (v8_to_v16 (args[0], args[1]));
- break;
/* Stats. */
case c ('C', 1):
motor_stat_counter_cpt = motor_stat_counter = args[0];
@@ -249,8 +247,49 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
motor_stat_timer_cpt = motor_stat_timer = args[0];
break;
default:
- proto_send0 ('e');
- return;
+ /* Params. */
+ if (cmd == 'p')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('x', 5):
+ postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('y', 5):
+ postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('a', 5):
+ postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('f', 3):
+ postrack_set_footing (v8_to_v16 (args[1], args[2]));
+ break;
+ case c ('e', 5):
+ goto_eps = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('p', 3):
+ speed_kp = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('i', 3):
+ speed_ki = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('a', 2):
+ speed_acc_cpt = speed_acc = args[1];
+ break;
+ case c ('m', 2):
+ speed_max = args[1];
+ break;
+ default:
+ proto_send0 ('e');
+ return;
+ }
+ }
+ else
+ {
+ proto_send0 ('e');
+ return;
+ }
+ break;
}
proto_send (cmd, size, args);
#undef c
diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c
index d7e6bc8..ae0d0c3 100644
--- a/n/asserv/src/postrack.c
+++ b/n/asserv/src/postrack.c
@@ -76,8 +76,8 @@ postrack_update (void)
if (counter_right_diff == counter_left_diff)
{
/* Line. */
- postrack_x += dsp_mul_f824 (d, dsp_cos (postrack_a)) >> 8;
- postrack_y += dsp_mul_f824 (d, dsp_sin (postrack_a)) >> 8;
+ postrack_x += dsp_mul_f824 (d, dsp_cos_f824 (postrack_a)) >> 8;
+ postrack_y += dsp_mul_f824 (d, dsp_sin_f824 (postrack_a)) >> 8;
}
else
{
@@ -90,11 +90,11 @@ postrack_update (void)
/* Compute da in radians. */
da = dsp_mul_f824 (da, 2 * M_PI * (1L << 24));
/* X increment. */
- dsc = dsp_sin (na) - dsp_sin (postrack_a); /* 8.24b */
+ dsc = dsp_sin_f824 (na) - dsp_sin_f824 (postrack_a); /* 8.24b */
dsc = dsp_div_f824 (dsc, da); /* 8.24b < 1 */
postrack_x += dsp_mul_f824 (d, dsc) >> 8; /* 24.8b */
/* Y increment. */
- dsc = dsp_cos (postrack_a) - dsp_cos (na); /* 8.24b */
+ dsc = dsp_cos_f824 (postrack_a) - dsp_cos_f824 (na); /* 8.24b */
dsc = dsp_div_f824 (dsc, da); /* 8.24b < 1 */
postrack_y += dsp_mul_f824 (d, dsc) >> 8; /* 24.8b */
/* Angle update. */
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 3b9a709..38842a1 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -177,9 +177,9 @@ speed_distance (int32_t dist, int32_t arc)
if (vas) arc = -arc;
/* Compute speed.
* v = sqrt (2 * a * d), a = 1/speed_acc */
- vl248 = dsp_sqrt (dist * 2 / speed_acc);
+ vl248 = dsp_sqrt_f248 (dist * 2 / speed_acc);
vl = (vl248 >> 8) & 0xff;
- va248 = dsp_sqrt (arc * 2 / speed_acc);
+ va248 = dsp_sqrt_f248 (arc * 2 / speed_acc);
va = (va248 >> 8) & 0xff;
/* Saturate. */
if (vl248 & 0xffff0000 || vl > speed_max)
diff --git a/n/asserv/src/test_dsp.c b/n/asserv/src/test_dsp.c
index b1a0c9b..7016954 100644
--- a/n/asserv/src/test_dsp.c
+++ b/n/asserv/src/test_dsp.c
@@ -79,9 +79,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1d ('r', rl);
break;
case 'c' | 1 << 8:
- rl = dsp_cos (sa);
+ rl = dsp_cos_f824 (sa);
proto_send1d ('r', rl);
- rl = dsp_sin (sa);
+ rl = dsp_sin_f824 (sa);
proto_send1d ('r', rl);
break;
case 'M' | 0 << 8:
@@ -134,9 +134,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
for (al = 0; al < (1L << 24) + (1L << 21); al += 32 << 8)
{
proto_send1d ('c', al);
- rl = dsp_cos (al);
+ rl = dsp_cos_f824 (al);
proto_send1d ('r', rl);
- rl = dsp_sin (al);
+ rl = dsp_sin_f824 (al);
proto_send1d ('r', rl);
}
break;
@@ -147,7 +147,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
al = wl[k] >> i;
proto_send1d ('s', al);
- rl = dsp_sqrt (al);
+ rl = dsp_sqrt_f248 (al);
proto_send1d ('r', rl);
}
}