From e7503d02686ce5b4bf01bd8b55e2eb2cda1c3ea6 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 6 Nov 2004 19:38:15 +0000 Subject: Ajout de mul_f824 Test de cos --- n/asserv/src/Makefile | 8 +++ n/asserv/src/cos_check.c | 157 +++++++++++++++++++++++++++++++++++++++++++++++ n/asserv/src/dsp.c | 86 +++++++++++++++++++++++++- n/asserv/src/dsp.h | 2 +- n/asserv/src/dsp.txt | 22 +++++++ n/asserv/src/main.c | 1 + n/asserv/src/speed.c | 4 +- n/asserv/src/test_dsp.c | 31 +++++++++- 8 files changed, 305 insertions(+), 6 deletions(-) create mode 100644 n/asserv/src/cos_check.c create mode 100644 n/asserv/src/dsp.txt (limited to 'n/asserv/src') 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 +#include +#include +#include + +/* +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: -- cgit v1.2.3