/* 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; }