summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
authorschodet2004-11-06 19:38:15 +0000
committerschodet2004-11-06 19:38:15 +0000
commite7503d02686ce5b4bf01bd8b55e2eb2cda1c3ea6 (patch)
tree995ca8fa2451f8d9b046379af544bbdb231dcfa0 /n/asserv/src
parentf6a81257c8f365b61fa91489f79fdbf3a6d60d09 (diff)
Ajout de mul_f824
Test de cos
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/Makefile8
-rw-r--r--n/asserv/src/cos_check.c157
-rw-r--r--n/asserv/src/dsp.c86
-rw-r--r--n/asserv/src/dsp.h2
-rw-r--r--n/asserv/src/dsp.txt22
-rw-r--r--n/asserv/src/main.c1
-rw-r--r--n/asserv/src/speed.c4
-rw-r--r--n/asserv/src/test_dsp.c31
8 files changed, 305 insertions, 6 deletions
diff --git a/n/asserv/src/Makefile b/n/asserv/src/Makefile
index 35ce9b5..f1be6f7 100644
--- a/n/asserv/src/Makefile
+++ b/n/asserv/src/Makefile
@@ -15,6 +15,9 @@ OPTIMIZE = -O2
DEFS =
LDLIBS =
+EXTRA_TARGETS = cos_check
+EXTRA_CLEAN_FILES = cos_check
+
include Makefile.avr
asserv.elf: $(asserv_OBJECTS)
@@ -22,3 +25,8 @@ asserv.elf: $(asserv_OBJECTS)
test_pwm.elf: $(test_pwm_OBJECTS)
test_dsp.elf: $(test_dsp_OBJECTS)
+
+cos_check: CC = gcc
+cos_check: CPPFLAGS =
+cos_check: CFLAGS = -Wall -g -O2
+cos_check: LDLIBS = -lm
diff --git a/n/asserv/src/cos_check.c b/n/asserv/src/cos_check.c
new file mode 100644
index 0000000..6dbc54f
--- /dev/null
+++ b/n/asserv/src/cos_check.c
@@ -0,0 +1,157 @@
+/* cos_check.c */
+/* asserv - Position & speed motor control on a ATmega128. {{{
+ *
+ * Copyright (C) 2004 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.
+ *
+ * }}} */
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+#include <stdlib.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+static int
+cos_dli (int a, int prec)
+{
+ static const int f[] = {
+ (1 << 24) * -26.42625678337439745096,
+ (1 << 24) * 60.24464137187666035919,
+ (1 << 24) * -85.45681720669372773226,
+ (1 << 24) * 64.93939402266829148905,
+ (1 << 24) * -19.73920880217871723738,
+ (1 << 24) * 1,
+ 0
+ };
+ int r;
+ int a2 = (a * a) >> 8;
+ int i;
+ r = 0;
+ for (i = 0; f[i] != 0; i++)
+ {
+ if (prec)
+ prec--;
+ else
+ r = (((long long)r * a2) >> 24) + (f[i] << 0);
+ }
+ return r; //(r >> 8) + ((r >> 7) & 1);
+}
+
+static int
+cos_tbl (int a)
+{
+ /* Bof, il faudrait augmenter la précicion et donc la taille des
+ * tables. */
+ static int tcosc[257];
+ static int tcosf[257];
+ static int tsinf[257];
+ static int inited = 0;
+ if (!inited)
+ {
+ double x;
+ int i;
+ for (i = 0; i <= 0x100; i++)
+ {
+ x = (double) i * 2.0 * M_PI / 0x100;
+ tcosc[i] = cos (x) * (1 << 16);
+ x /= 0x100;
+ tcosf[i] = cos (x) * (1 << 16);
+ tsinf[i] = sin (x) * (1 << 16);
+ }
+ }
+ return (tcosc[(a >> 8) & 0xff] * tcosf[a & 0xff]
+ - tcosc[0x100 - ((a >> 8) & 0xff)] * tsinf[a & 0xff]) >> 16;
+}
+
+static float
+cos_dlf (float a)
+{
+ static const float f[] = {
+ -26.42625678337439745096f,
+ 60.24464137187666035919f,
+ -85.45681720669372773226f,
+ 64.93939402266829148905f,
+ -19.73920880217871723738f,
+ 1.0f,
+ 0
+ };
+ float r;
+ float a2 = a * a;
+ int i;
+ r = 0.0f;
+ for (i = 0; f[i] != 0; i++)
+ {
+ r = r * a2 + f[i];
+ }
+ return r;
+}
+
+static double
+cos_dl (double a)
+{
+ static const double f[] = {
+ -26.42625678337439745096,
+ 60.24464137187666035919,
+ -85.45681720669372773226,
+ 64.93939402266829148905,
+ -19.73920880217871723738,
+ 1,
+ 0
+ };
+ double r;
+ double a2 = a * a;
+ int i;
+ r = 0.0;
+ for (i = 0; f[i] != 0; i++)
+ {
+ r = r * a2 + f[i];
+ }
+ return r;
+}
+
+int
+main (int argc, char **argv)
+{
+ double a, c;
+ int i, ci;
+ int prec = 0;
+ if (argc >= 2)
+ prec = atoi (argv[1]);
+ /*for (i = 0; i < 1024; i++)
+ {
+ f = ((double) i) / 1024.0 / 4.0;
+ printf ("%f %f\n", f * 2.0 * M_PI, cos_dl (f));
+ }*/
+ /*for (i = 0; i < 1024; i++)
+ {
+ a = ((float) i) / 1024.0 / 4.0;
+ printf ("%f %f\n", a * 2.0 * M_PI, cos_dlf (a));
+ }*/
+ for (i = 0; i < (1 << 14); i++)
+ {
+ a = ((double) i) * 2.0 * M_PI / (1 << 16);
+ ci = cos_dli (i, prec);
+ c = ((double) ci) / (1 << 24);
+ printf ("%f %f %i\n", a, c, ci);
+ }
+ return 0;
+}
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
index 9520513..2e940db 100644
--- a/n/asserv/src/dsp.c
+++ b/n/asserv/src/dsp.c
@@ -48,7 +48,7 @@ dsp_add_sat_i16i16 (int16_t a, int16_t b)
return a;
}
-/** Multiply signed 16 by fixed 8.8. */
+/** Multiply signed 16 by fixed unsigned 8.8. */
extern inline int16_t
dsp_mul_i16f88 (int16_t a, uint16_t b)
{
@@ -64,7 +64,89 @@ dsp_mul_i16f88 (int16_t a, uint16_t b)
"add %A0, r0\n\t"
"adc %B0, r1\n\t"
"clr r1\n\t"
- : "=&r" (r) : "a" (a), "a" (b) : "r0", "r1");
+ : "=&r" (r) : "a" (a), "a" (b) : "r0");
+ return r;
+}
+
+/** Multiply f8.24 by f8.24. */
+int32_t
+dsp_mul_f824 (int32_t a, int32_t b)
+{
+ register int32_t ar asm ("r16"), br asm ("r20");
+ ar = a;
+ br = b;
+ int32_t r;
+ asm (""
+ "clr r2\n\t"
+ /* Low dword (>> 8, with 8 guards). */
+ "mul %A1, %B2\n\t"
+ "movw %A0, r0\n\t"
+ "clr %C0\n\t"
+ "clr %D0\n\t"
+ "mul %A1, %A2\n\t"
+ "add %A0, r1\n\t"
+ "adc %B0, r2\n\t"
+ "adc %C0, r2\n\t"
+ "mul %B1, %A2\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "adc %C0, r2\n\t"
+ "mul %A1, %C2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mul %B1, %B2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mul %C1, %A2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ /* Shift. */
+ "movw %A0, %C0\n\t"
+ /* Upper word. */
+ "mulsu %D2, %C1\n\t"
+ "movw %C0, r0\n\t"
+ "mulsu %D2, %A1\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "adc %C0, r2\n\t"
+ "adc %D0, r2\n\t"
+ "mul %B1, %C2\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "adc %C0, r2\n\t"
+ "adc %D0, r2\n\t"
+ "mul %C1, %B2\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "adc %C0, r2\n\t"
+ "adc %D0, r2\n\t"
+ "mulsu %D1, %A2\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "adc %C0, r2\n\t"
+ "adc %D0, r2\n\t"
+ "mulsu %D2, %B1\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mul %C1, %C2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mulsu %D1, %B2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mulsu %D1, %C2\n\t"
+ "add %C0, r0\n\t"
+ "adc %D0, r1\n\t"
+ "muls %D1, %D2\n\t"
+ "add %D0, r0\n\t"
+ "clr r1\n\t"
+ : "=&r" (r) : "a" (ar), "a" (br) : "r0", "r2");
return r;
}
diff --git a/n/asserv/src/dsp.h b/n/asserv/src/dsp.h
index e498e1d..701711a 100644
--- a/n/asserv/src/dsp.h
+++ b/n/asserv/src/dsp.h
@@ -33,7 +33,7 @@
extern inline int16_t
dsp_add_sat_i16i16 (int16_t a, int16_t b);
-/** Multiply signed 16 by fixed 8.8. */
+/** Multiply signed 16 by fixed unsigned 8.8. */
extern inline int16_t
dsp_mul_i16f88 (int16_t a, uint16_t b);
diff --git a/n/asserv/src/dsp.txt b/n/asserv/src/dsp.txt
new file mode 100644
index 0000000..a20f3db
--- /dev/null
+++ b/n/asserv/src/dsp.txt
@@ -0,0 +1,22 @@
+dsp_mul_f824 (int32_t a, int32_t b)
+
+a: D1.C1 B1 A1
+b: D2.C2 B2 A2
+r: D0.C0 B0 A0
+
+D1xD2. |
+ |D1xC2 |
+ | .D1xB2 |
+ | . D1xA2|
+ |C1xD2 |
+ | .C1xC2 |
+ | . C1xB2|
+ | . C1xA2
+ | .B1xD2 |
+ | . B1xC2|
+ | . B1xB2
+ | . |B1xA2
+ | . A1xD2|
+ | . A1xC2
+ | . |A1xB2
+ | . | A1xA2
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index ae44774..8656644 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -139,6 +139,7 @@ main_loop (void)
counter_right >> 8, counter_right);
motor_stat_counter_cpt = motor_stat_counter;
}
+ /* Misc. */
if ((motor_loop_cpt & 7) == 0)
{
if (rs232_poll ())
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index cb8f2db..54962b3 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -116,7 +116,7 @@ speed_compute_left_pwm (void)
speed_left_int += e; /* 12b = 11b + 10b */
if (speed_left_int > speed_int_max) /* 11b */
speed_left_int = speed_int_max;
- else if (speed_left_int < -speed_left_int)
+ else if (speed_left_int < -speed_int_max)
speed_left_int = -speed_int_max;
/* Compute PI. */ /* 16b = 15b + 15b */
pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 10b * 5.8b */
@@ -137,7 +137,7 @@ speed_compute_right_pwm (void)
speed_right_int += e;
if (speed_right_int > speed_int_max)
speed_right_int = speed_int_max;
- else if (speed_right_int < -speed_right_int)
+ else if (speed_right_int < -speed_int_max)
speed_right_int = -speed_int_max;
/* Compute PI. */
pwm = dsp_mul_i16f88 (e, speed_kp)
diff --git a/n/asserv/src/test_dsp.c b/n/asserv/src/test_dsp.c
index 37f8d2d..dace398 100644
--- a/n/asserv/src/test_dsp.c
+++ b/n/asserv/src/test_dsp.c
@@ -38,8 +38,10 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
int16_t a;
uint16_t b;
int16_t r;
- int16_t i, j;
+ int16_t i, j, k;
uint16_t w = 0xa66a;
+ int32_t al, bl, rl;
+ uint32_t wl[] = { 0xa66a6aa6, 0xffffffff, 0xaff605be };
switch (c | argc << 8)
{
case 'm' | 0 << 8:
@@ -61,6 +63,33 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
}
}
break;
+ case 'M' | 0 << 8:
+ for (k = 0; k < 3; k++)
+ {
+ for (i = 32; i > 0; i--)
+ {
+ al = wl[k] >> i;
+ for (j = 32; j >= 0; j--)
+ {
+ bl = wl[k] >> j;
+ proto_send4 ('A',
+ (al >> 24) & 0xff, (al >> 16) & 0xff,
+ (al >> 8) & 0xff, al & 0xff);
+ proto_send4 ('B',
+ (bl >> 24) & 0xff, (bl >> 16) & 0xff,
+ (bl >> 8) & 0xff, bl & 0xff);
+ rl = dsp_mul_f824 (al, bl);
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ rl = dsp_mul_f824 (-al, bl);
+ proto_send4 ('R',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ }
+ }
+ }
+ break;
case 'z' | 0 << 8:
reset ();
default: