add trigo file in beacon
authorOlivier Matz <zer0@droids-corp.org>
Sat, 20 Feb 2010 17:25:34 +0000 (18:25 +0100)
committerOlivier Matz <zer0@droids-corp.org>
Sat, 20 Feb 2010 17:25:34 +0000 (18:25 +0100)
projects/microb2010/tests/beacon_tsop/.config
projects/microb2010/tests/beacon_tsop/Makefile
projects/microb2010/tests/beacon_tsop/commands.c
projects/microb2010/tests/beacon_tsop/trigo.c [new file with mode: 0644]
projects/microb2010/tests/beacon_tsop/trigo.h [new file with mode: 0644]
projects/microb2010/tests/static_beacon/.config.old

index a179a7e..94eb57b 100644 (file)
@@ -61,11 +61,12 @@ CONFIG_QUARTZ=16000000
 # CONFIG_OPTM_2 is not set
 # CONFIG_OPTM_3 is not set
 CONFIG_OPTM_S=y
-# CONFIG_MATH_LIB is not set
+CONFIG_MATH_LIB=y
 # CONFIG_FDEVOPEN_COMPAT is not set
 # CONFIG_NO_PRINTF is not set
 # CONFIG_MINIMAL_PRINTF is not set
 CONFIG_STANDARD_PRINTF=y
+# CONFIG_ADVANCED_PRINTF is not set
 # CONFIG_FORMAT_IHEX is not set
 # CONFIG_FORMAT_SREC is not set
 CONFIG_FORMAT_BINARY=y
@@ -77,7 +78,8 @@ CONFIG_MODULE_CIRBUF=y
 # CONFIG_MODULE_CIRBUF_LARGE is not set
 # CONFIG_MODULE_FIXED_POINT is not set
 # CONFIG_MODULE_VECT2 is not set
-# CONFIG_MODULE_GEOMETRY is not set
+CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_HOSTSIM is not set
 # CONFIG_MODULE_SCHEDULER is not set
 # CONFIG_MODULE_SCHEDULER_STATS is not set
 # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
@@ -160,9 +162,10 @@ CONFIG_MODULE_PARSE=y
 # CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
 
 #
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
 #
 # CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
 # CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
 # CONFIG_MODULE_POSITION_MANAGER is not set
 # CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
index 94f9e8c..6d98f23 100755 (executable)
@@ -3,7 +3,7 @@ TARGET = beacon_tsop
 AVERSIVE_DIR = ../../../..
 
 # List C source files here. (C dependencies are automatically generated.)
-SRC = main.c commands.c cmdline.c
+SRC = main.c commands.c cmdline.c trigo.c
 
 # List Assembler source files here.
 # Make them always end in a capital .S.  Files ending in a lowercase .s
index e938a73..8a98f49 100644 (file)
@@ -146,6 +146,35 @@ parse_pgm_inst_t cmd_debug_speed = {
        },
 };
 
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+       fixed_string_t arg0;
+};
+
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(__attribute__((unused)) void *parsed_result,
+                            __attribute__((unused)) void *data)
+{
+
+}
+
+prog_char str_test_arg0[] = "test";
+parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
+
+prog_char help_test[] = "Test the board";
+parse_pgm_inst_t cmd_test = {
+       .f = cmd_test_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_test,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_test_arg0, 
+               NULL,
+       },
+};
+
 /**********************************************************/
 
 /* in progmem */
@@ -155,5 +184,6 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_reset,
        (parse_pgm_inst_t *)&cmd_debug_frame,
        (parse_pgm_inst_t *)&cmd_debug_speed,
+       (parse_pgm_inst_t *)&cmd_test,
        NULL,
 };
diff --git a/projects/microb2010/tests/beacon_tsop/trigo.c b/projects/microb2010/tests/beacon_tsop/trigo.c
new file mode 100644 (file)
index 0000000..932bb42
--- /dev/null
@@ -0,0 +1,247 @@
+/*
+ *  Copyright Droids Corporation (2010)
+ *
+ *  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
+ *
+ *  Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive.h>
+
+#include <vect_base.h>
+#include <lines.h>
+#include <circles.h>
+
+#define POS_ACCURACY 10.0 /* 1 cm accuracy max */
+#ifndef HOST_VERSION
+#define printf(args...) do {} while(0)
+#endif
+
+static int dprint = 0;
+#define dprintf(args...) if (dprint) printf(args)
+
+const point_t beacon0 = { 0, 1050 };
+const point_t beacon1 = { 3000, 0 };
+const point_t beacon2 = { 3000, 2100 };
+
+/* Fill the 2 circles pointer given as parameter, each of those cross
+ * both beacons b1 and b2. From any point of these circles (except b1
+ * and b2), we see b1 and b2 with the angle of a_rad (which must be
+ * positive). Return 0 on success.
+ *
+ *                              l
+ *                <------------------------->
+ *
+ *              b1              O            b2
+ *               +----------------------------+
+ *             ,' \\            |            /|'\
+ *            /    \ \          | ^        / |   `
+ *           /       \ \   a___ | | d    /   |    `.
+ *          /         \  \ /    | v    /     |     \
+ *         |            \  \    |    /       |      |
+ *         |             \   \  |  /        |       |
+ *         |               \   \|/          |        |
+ *         |                \   * C         |        |
+ *         |                  \             |       .'
+ *         |                   \           |        |
+ *          |                    \         |       .'
+ *           \                    \   a____|       /
+ *            \                     \ /    |     ,'
+ *             `                     \    |     /
+ *              '.                     \  |   ,'
+ *                '-.                   \ |_,'
+ *                   '-._              _,*'
+ *                       '`--......---'     R (the robot)
+ *
+ */
+int8_t angle_to_circles(circle_t *c1, circle_t *c2,
+                        const point_t *b1, const point_t *b2,
+                        double a_rad)
+{
+       point_t O;
+       vect_t v;
+       float l, d;
+
+       /* reject negative or too small angles */
+       if (a_rad <= 0.01)
+               return -1;
+
+       /* get position of O */
+       O.x = (b1->x + b2->x) / 2;
+       O.y = (b1->y + b2->y) / 2;
+
+       /* get the length l */
+       v.x = b2->x - b1->x;
+       v.y = b2->y - b1->y;
+       l = vect_norm(&v);
+
+       /* distance from O to the center of the circle */
+       /* XXX div by 0 when pi */
+       d = l / (2 * tan(a_rad));
+
+       /* get the circle c1 */
+       vect_rot_trigo(&v);
+       vect_resize(&v, d);
+       if (c1) {
+               c1->x = O.x + v.x;
+               c1->y = O.y + v.y;
+               c1->r = norm(b1->x, b1->y, c1->x, c1->y);
+       }
+
+       /* get the circle c2 */
+       if (c2) {
+               c2->x = O.x - v.x;
+               c2->y = O.y - v.y;
+               c2->r = norm(b1->x, b1->y, c1->x, c1->y);
+       }
+
+       return 0;
+}
+
+/* get the position of the robot from the angle of the 3 beacons */
+int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20)
+{
+       circle_t c01, c12, c20;
+       point_t dummy_pt, p1, p2, p3;
+
+       dprintf("a01 = %2.2f\n", a01);
+       dprintf("a12 = %2.2f\n", a12);
+       dprintf("a20 = %2.2f\n", a20);
+
+       if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
+               return -1;
+       dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
+
+       if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
+               return -1;
+       dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
+
+       if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
+               return -1;
+       dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
+
+       if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
+               return -1;
+       if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
+               return -1;
+       if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
+               return -1;
+
+       dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
+       dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
+       dprintf("p3: x=%2.2f y=%2.2f\n", p3.x, p3.y);
+
+       /* if (norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
+       /*     norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
+       /*     norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
+       /*      return -1; */
+
+       pos->x = (p1.x + p2.x + p3.x) / 3.0;
+       pos->y = (p1.y + p2.y + p3.y) / 3.0;
+
+       return 0;
+}
+
+/* get the angles of beacons from xy pos */
+int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
+                      double *a20, int err_num, float err_val)
+{
+       double a0, a1, a2;
+
+       a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
+       a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
+       a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
+
+       if (err_num == 0 || err_num == 3)
+               a0 += (err_val * M_PI/180.);
+       if (err_num == 1 || err_num == 3)
+               a1 += (err_val * M_PI/180.);
+       if (err_num == 2 || err_num == 3)
+               a2 += (err_val * M_PI/180.);
+
+       *a01 = a1-a0;
+       if (*a01 < 0)
+               *a01 += M_PI*2;
+       *a12 = a2-a1;
+       if (*a12 < 0)
+               *a12 += M_PI*2;
+       *a20 = a0-a2;
+       if (*a20 < 0)
+               *a20 += M_PI*2;
+
+       return 0;
+}
+
+int8_t process_move_error(double x, double y, double speed,
+                         double period, double angle, double *err)
+{
+       double a01, a12, a20;
+       point_t pos, tmp;
+       double a0, a1, a2;
+       vect_t u,v;
+       point_t pos2, pos3;
+
+       pos.x = x;
+       pos.y = y;
+
+       /* from start to destination */
+       v.x = cos(angle) * speed * period;
+       v.y = sin(angle) * speed * period;
+
+       /* first process real pos */
+       posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
+
+       /* vector covered during measure of a0 and a1 */
+       u.x = v.x * a01 / (2*M_PI);
+       u.y = v.y * a01 / (2*M_PI);
+       pos2.x = pos.x + u.x;
+       pos2.y = pos.y + u.y;
+
+       /* vector covered during measure of a1 and a2 */
+       u.x = v.x * a12 / (2*M_PI);
+       u.y = v.y * a12 / (2*M_PI);
+       pos3.x = pos2.x + u.x;
+       pos3.y = pos2.y + u.y;
+
+       dprintf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
+       dprintf("p1: x=%2.2f y=%2.2f\n", pos2.x, pos2.y);
+       dprintf("p2: x=%2.2f y=%2.2f\n", pos3.x, pos3.y);
+
+       a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
+       a1 = atan2(beacon1.y-pos2.y, beacon1.x-pos2.x);
+       a2 = atan2(beacon2.y-pos3.y, beacon2.x-pos3.x);
+
+       a01 = a1-a0;
+       if (a01 < 0)
+               a01 += M_PI*2;
+       a12 = a2-a1;
+       if (a12 < 0)
+               a12 += M_PI*2;
+       a20 = a0-a2;
+       if (a20 < 0)
+               a20 += M_PI*2;
+
+       if (angles_to_posxy(&tmp, a01, a12, a20))
+               return -1;
+       *err = pt_norm(&tmp, &pos);
+       if (*err > 50.) /* saturate error to 5cm */
+               *err = 50.;
+       return 0;
+}
diff --git a/projects/microb2010/tests/beacon_tsop/trigo.h b/projects/microb2010/tests/beacon_tsop/trigo.h
new file mode 100644 (file)
index 0000000..1c3178f
--- /dev/null
@@ -0,0 +1,22 @@
+/*
+ *  Copyright Droids Corporation (2010)
+ *
+ *  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
+ *
+ *  Olivier MATZ <zer0@droids-corp.org>
+ */
+
+/* get the position of the robot from the angle of the 3 beacons */
+int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20);
index dd4bd20..b0dbf0d 100644 (file)
@@ -73,7 +73,7 @@ CONFIG_FORMAT_BINARY=y
 #
 # Base modules
 #
-# CONFIG_MODULE_CIRBUF is not set
+CONFIG_MODULE_CIRBUF=y
 # CONFIG_MODULE_CIRBUF_LARGE is not set
 # CONFIG_MODULE_FIXED_POINT is not set
 # CONFIG_MODULE_VECT2 is not set
@@ -92,9 +92,9 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y
 #
 # Communication modules
 #
-# CONFIG_MODULE_UART is not set
+CONFIG_MODULE_UART=y
 # CONFIG_MODULE_UART_9BITS is not set
-# CONFIG_MODULE_UART_CREATE_CONFIG is not set
+CONFIG_MODULE_UART_CREATE_CONFIG=y
 # CONFIG_MODULE_SPI is not set
 # CONFIG_MODULE_SPI_CREATE_CONFIG is not set
 # CONFIG_MODULE_I2C is not set
@@ -214,8 +214,8 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y
 #
 # Programmer options
 #
-# CONFIG_AVRDUDE is not set
-CONFIG_AVARICE=y
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
 
 #
 # Avrdude