summaryrefslogtreecommitdiff
path: root/n/asserv/src/cos_check.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/cos_check.c')
-rw-r--r--n/asserv/src/cos_check.c157
1 files changed, 157 insertions, 0 deletions
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;
+}