From a56bf7c398eb727f67ed0843c6ebef948cc0dd61 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Sat, 20 Feb 2010 18:25:34 +0100 Subject: [PATCH] add trigo file in beacon --- projects/microb2010/tests/beacon_tsop/.config | 9 +- .../microb2010/tests/beacon_tsop/Makefile | 2 +- .../microb2010/tests/beacon_tsop/commands.c | 30 +++ projects/microb2010/tests/beacon_tsop/trigo.c | 247 ++++++++++++++++++ projects/microb2010/tests/beacon_tsop/trigo.h | 22 ++ .../tests/static_beacon/.config.old | 10 +- 6 files changed, 311 insertions(+), 9 deletions(-) create mode 100644 projects/microb2010/tests/beacon_tsop/trigo.c create mode 100644 projects/microb2010/tests/beacon_tsop/trigo.h diff --git a/projects/microb2010/tests/beacon_tsop/.config b/projects/microb2010/tests/beacon_tsop/.config index a179a7e..94eb57b 100644 --- a/projects/microb2010/tests/beacon_tsop/.config +++ b/projects/microb2010/tests/beacon_tsop/.config @@ -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 diff --git a/projects/microb2010/tests/beacon_tsop/Makefile b/projects/microb2010/tests/beacon_tsop/Makefile index 94f9e8c..6d98f23 100755 --- a/projects/microb2010/tests/beacon_tsop/Makefile +++ b/projects/microb2010/tests/beacon_tsop/Makefile @@ -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 diff --git a/projects/microb2010/tests/beacon_tsop/commands.c b/projects/microb2010/tests/beacon_tsop/commands.c index e938a73..8a98f49 100644 --- a/projects/microb2010/tests/beacon_tsop/commands.c +++ b/projects/microb2010/tests/beacon_tsop/commands.c @@ -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 index 0000000..932bb42 --- /dev/null +++ b/projects/microb2010/tests/beacon_tsop/trigo.c @@ -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 + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#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 index 0000000..1c3178f --- /dev/null +++ b/projects/microb2010/tests/beacon_tsop/trigo.h @@ -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 + */ + +/* 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); diff --git a/projects/microb2010/tests/static_beacon/.config.old b/projects/microb2010/tests/static_beacon/.config.old index dd4bd20..b0dbf0d 100644 --- a/projects/microb2010/tests/static_beacon/.config.old +++ b/projects/microb2010/tests/static_beacon/.config.old @@ -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 -- 2.39.5