From 9b20b69a87c9d442cf4610351dc40b28b7f36e9c Mon Sep 17 00:00:00 2001 From: zer0 Date: Sun, 28 Feb 2010 23:32:27 +0100 Subject: [PATCH] work on beacon --- include/aversive/irq_lock.h | 6 + modules/base/math/geometry/vect_base.c | 27 +- modules/base/math/geometry/vect_base.h | 16 +- projects/microb2010/tests/oa/graph.py | 2 +- .../microb2010/tests/tourel_beacon/.config | 8 +- .../tests/tourel_beacon/.config.old | 38 ++- .../microb2010/tests/tourel_beacon/Makefile | 3 +- .../microb2010/tests/tourel_beacon/graph.py | 184 ++++++++++-- .../microb2010/tests/tourel_beacon/main.c | 264 ++++++++++++++++-- 9 files changed, 460 insertions(+), 88 deletions(-) diff --git a/include/aversive/irq_lock.h b/include/aversive/irq_lock.h index 12a46bb..055c247 100644 --- a/include/aversive/irq_lock.h +++ b/include/aversive/irq_lock.h @@ -42,12 +42,18 @@ #ifdef HOST_VERSION +#ifdef CONFIG_MODULE_HOSTSIM #include /* we must use 'flags' to avoid a warning */ #define IRQ_UNLOCK(flags) do { flags=0; /* hostsim_lock(); */ } while(0) #define IRQ_LOCK(flags) do { flags=0; /* hostsim_unlock(); */ } while(0) #define GLOBAL_IRQ_ARE_MASKED() hostsim_islocked() +#else +#define IRQ_UNLOCK(flags) do { flags=0; } while(0) +#define IRQ_LOCK(flags) do { flags=0; } while(0) +#define GLOBAL_IRQ_ARE_MASKED() (0) +#endif /* CONFIG_MODULE_HOSTSIM */ #else diff --git a/modules/base/math/geometry/vect_base.c b/modules/base/math/geometry/vect_base.c index b8b0d31..6008440 100755 --- a/modules/base/math/geometry/vect_base.c +++ b/modules/base/math/geometry/vect_base.c @@ -22,23 +22,23 @@ #include #include #include - + /* Return scalar product */ -float +float vect_pscal(vect_t *v, vect_t *w) { return v->x * w->x + v->y * w->y; } /* Return Z of vectorial product */ -float +float vect_pvect(vect_t *v, vect_t *w) { return v->x*w->y - v->y*w->x; } /* Return scalar product */ -int8_t +int8_t vect_pscal_sign(vect_t *v, vect_t *w) { float z; @@ -49,7 +49,7 @@ vect_pscal_sign(vect_t *v, vect_t *w) } /* Return Z of vectorial product */ -int8_t +int8_t vect_pvect_sign(vect_t *v, vect_t *w) { float z; @@ -66,7 +66,7 @@ float xy_norm(float x1, float y1, float x2, float y2) return sqrt(x*x + y*y); } -float pt_norm(point_t *p1, point_t *p2) +float pt_norm(const point_t *p1, const point_t *p2) { float x = p2->x - p1->x; float y = p2->y - p1->y; @@ -74,8 +74,7 @@ float pt_norm(point_t *p1, point_t *p2) } /* norm of a vector */ -float -vect_norm(vect_t *v) +float vect_norm(const vect_t *v) { return sqrt(v->x*v->x+v->y*v->y); } @@ -83,30 +82,30 @@ vect_norm(vect_t *v) void vect_rot_trigo(vect_t *v) { float s; - + s = v->x; v->x= -v->y; v->y = s; -} +} void vect_rot_retro(vect_t *v) { float s; - + s = v->x; v->x= v->y; v->y = -s; -} +} float vect_get_angle(vect_t *v, vect_t *w) { float ps; float n; - + ps = vect_pscal(v, w); n = vect_norm(v) * vect_norm(w); - + return acos((float)ps/n); } diff --git a/modules/base/math/geometry/vect_base.h b/modules/base/math/geometry/vect_base.h index de3a2c2..50a5910 100755 --- a/modules/base/math/geometry/vect_base.h +++ b/modules/base/math/geometry/vect_base.h @@ -33,25 +33,21 @@ typedef struct _point_t { } point_t; /* Return scalar product */ -float -vect_pscal(vect_t *v, vect_t *w); +float vect_pscal(vect_t *v, vect_t *w); /* Return Z of vectorial product */ -float -vect_pvect(vect_t *v, vect_t *w); +float vect_pvect(vect_t *v, vect_t *w); /* Return scalar product */ -int8_t -vect_pscal_sign(vect_t *v, vect_t *w); +int8_t vect_pscal_sign(vect_t *v, vect_t *w); /* Return Z of vectorial product */ -int8_t -vect_pvect_sign(vect_t *v, vect_t *w); +int8_t vect_pvect_sign(vect_t *v, vect_t *w); /* norm of a vector */ float xy_norm(float x1, float y1, float x2, float y2); -float pt_norm(point_t *p1, point_t *p2); -float vect_norm(vect_t *v); +float pt_norm(const point_t *p1, const point_t *p2); +float vect_norm(const vect_t *v); void vect_rot_trigo(vect_t *v); void vect_rot_retro(vect_t *v); float vect_get_angle(vect_t *v, vect_t *w); diff --git a/projects/microb2010/tests/oa/graph.py b/projects/microb2010/tests/oa/graph.py index 0b304bc..d5946f1 100644 --- a/projects/microb2010/tests/oa/graph.py +++ b/projects/microb2010/tests/oa/graph.py @@ -205,7 +205,7 @@ def build_area(ax): y = OFFSET_CORN_Y + STEP_CORN_Y/2 j = 0 while y < 2100: - + if waypoints[i][j] == TYPE_WHITE_CORN: wcorn.append(Circle((x, y), 25)) if waypoints[i][j] == TYPE_BLACK_CORN: diff --git a/projects/microb2010/tests/tourel_beacon/.config b/projects/microb2010/tests/tourel_beacon/.config index 9a16be2..0182964 100644 --- a/projects/microb2010/tests/tourel_beacon/.config +++ b/projects/microb2010/tests/tourel_beacon/.config @@ -56,11 +56,11 @@ CONFIG_QUARTZ=16000000 # # Generation options # -# CONFIG_OPTM_0 is not set +CONFIG_OPTM_0=y # CONFIG_OPTM_1 is not set # CONFIG_OPTM_2 is not set # CONFIG_OPTM_3 is not set -CONFIG_OPTM_S=y +# CONFIG_OPTM_S is not set CONFIG_MATH_LIB=y # CONFIG_FDEVOPEN_COMPAT is not set # CONFIG_NO_PRINTF is not set @@ -79,6 +79,7 @@ CONFIG_FORMAT_BINARY=y # CONFIG_MODULE_FIXED_POINT is not set # CONFIG_MODULE_VECT2 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 @@ -161,9 +162,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=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/tourel_beacon/.config.old b/projects/microb2010/tests/tourel_beacon/.config.old index 0a9b697..b47777e 100644 --- a/projects/microb2010/tests/tourel_beacon/.config.old +++ b/projects/microb2010/tests/tourel_beacon/.config.old @@ -1,5 +1,5 @@ # -# Automatically generated by make menuconfig: don't edit +# Automatically generated make config: don't edit # # @@ -67,18 +67,23 @@ CONFIG_MATH_LIB=y # CONFIG_MINIMAL_PRINTF is not set CONFIG_STANDARD_PRINTF=y # CONFIG_ADVANCED_PRINTF is not set -CONFIG_FORMAT_IHEX=y +# CONFIG_FORMAT_IHEX is not set # CONFIG_FORMAT_SREC is not set -# CONFIG_FORMAT_BINARY is not set +CONFIG_FORMAT_BINARY=y # # Base modules # + +# +# Enable math library in generation options to see all modules +# # CONFIG_MODULE_CIRBUF is not set # CONFIG_MODULE_CIRBUF_LARGE is not set # CONFIG_MODULE_FIXED_POINT is not set # CONFIG_MODULE_VECT2 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 @@ -93,6 +98,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Communication modules # + +# +# uart needs circular buffer, mf2 client may need scheduler +# # CONFIG_MODULE_UART is not set # CONFIG_MODULE_UART_9BITS is not set # CONFIG_MODULE_UART_CREATE_CONFIG is not set @@ -161,9 +170,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=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 @@ -176,6 +186,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # Control system modules # # CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set + +# +# Filters +# # CONFIG_MODULE_PID is not set # CONFIG_MODULE_PID_CREATE_CONFIG is not set # CONFIG_MODULE_RAMP is not set @@ -186,12 +200,20 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Radio devices # + +# +# Some radio devices require SPI to be activated +# # CONFIG_MODULE_CC2420 is not set # CONFIG_MODULE_CC2420_CREATE_CONFIG is not set # # Crypto modules # + +# +# Crypto modules depend on utils module +# # CONFIG_MODULE_AES is not set # CONFIG_MODULE_AES_CTR is not set # CONFIG_MODULE_MD5 is not set @@ -201,12 +223,20 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Encodings modules # + +# +# Encoding modules depend on utils module +# # CONFIG_MODULE_BASE64 is not set # CONFIG_MODULE_HAMMING is not set # # Debug modules # + +# +# Debug modules depend on utils module +# # CONFIG_MODULE_DIAGNOSTIC is not set # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set CONFIG_MODULE_ERROR=y diff --git a/projects/microb2010/tests/tourel_beacon/Makefile b/projects/microb2010/tests/tourel_beacon/Makefile index 6d5d8e2..639fab0 100644 --- a/projects/microb2010/tests/tourel_beacon/Makefile +++ b/projects/microb2010/tests/tourel_beacon/Makefile @@ -1,7 +1,6 @@ TARGET = main -# repertoire des modules -AVERSIVE_DIR =../..# VALUE, absolute or relative path : example ../.. # +AVERSIVE_DIR = ../../../.. # List C source files here. (C dependencies are automatically generated.) SRC = $(TARGET).c diff --git a/projects/microb2010/tests/tourel_beacon/graph.py b/projects/microb2010/tests/tourel_beacon/graph.py index 23193b7..f1e175e 100644 --- a/projects/microb2010/tests/tourel_beacon/graph.py +++ b/projects/microb2010/tests/tourel_beacon/graph.py @@ -13,7 +13,8 @@ import popen2, random Path = mpath.Path FLOAT = "([-+]?[0-9]*\.?[0-9]+)" INT = "([-+]?[0-9][0-9]*)" -RANDOM_ERROR = 0.3 # deg +RANDOM_ERROR_A = 0.5 # deg +RANDOM_ERROR_D = 0.5 # percent beacons = [ (0.0, 1050.0), (3000.0, 0.0), (3000.0, 2100.0) ] def build_poly(ptlist): @@ -40,50 +41,149 @@ def build_path(ptlist): def get_angle(ref, b): """get angle from robot point of view (ref) of beacon 'b'""" a = math.atan2(b[1]-ref[1], b[0]-ref[0]) - ea = (math.pi/180.) * RANDOM_ERROR * random.random() + ea = (math.pi/180.) * RANDOM_ERROR_A * random.random() ea = random.choice([ea, -ea]) return a + ea, ea - alpha = math.atan2(a[1]-ref[1], a[0]-ref[0]) - beta = math.atan2(b[1]-ref[1], b[0]-ref[0]) - gamma = beta-alpha - if gamma < 0: - gamma = gamma + 2*math.pi - return gamma + error, error +def get_distance(ref, b): + """get distance between robot (ref) and beacon 'b'""" + d = math.sqrt((b[1]-ref[1])**2 + (b[0]-ref[0])**2) + ed = RANDOM_ERROR_D * random.random() + ed = random.choice([ed, -ed]) + return d + (d * ed / 100.), ed def dist(p1, p2): return math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2) -def graph(filename, real_x, real_y): +# graph position from distance + angle +def graph_da(filename, real_x, real_y, real_a): + pcol = [] + print "real_pt = %s"%(str((real_x, real_y, real_a))) real_pt = (real_x, real_y) # display beacons patches = [] for b in beacons: patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ] + pcol.append(PatchCollection(patches, facecolor="yellow", alpha = 1)) - patches += [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ] + patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ] + pcol.append(PatchCollection(patches, facecolor="red", alpha = 1)) + + # process angles from robot position + a0,ea0 = get_angle((real_x, real_y), beacons[0]) + a1,ea1 = get_angle((real_x, real_y), beacons[1]) + a0 -= real_a + a1 -= real_a + text = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi)) + text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi)) + d0,ed0 = get_distance((real_x, real_y), beacons[0]) + d1,ed1 = get_distance((real_x, real_y), beacons[1]) + text += "d0 = %2.2f (%+2.2f %%)\n"%(d0, ed0) + text += "d1 = %2.2f (%+2.2f %%)\n"%(d1, ed1) + + cmd = "./main ad2pos %f %f %f %f"%(a0, a1, d0, d1) + print cmd + o,i = popen2.popen2(cmd) + i.close() + s = o.read(1000000) + o.close() + + open(filename + ".txt", "w").write(s) + + if len(s) == 1000000: + gloupix() + + fig = plt.figure() + ax = fig.add_subplot(111) + title = "Erreur de position en mm, qd l'erreur " + title += "d'angle aleatoire est comprise\n" + title += "erreur -%1.1f et %1.1f deg "%(RANDOM_ERROR_A, RANDOM_ERROR_A) + title += "et de distance de +/- %2.2f %%"%(RANDOM_ERROR_D) + ax.set_title(title) + + # area + x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)]) + ax.plot(x, y, 'g-') + + result_pt = (-1, -1) + patches = [] + for l in s.split("\n"): + m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l) + if m: + x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2])) + print x,y,r + patches += [ Circle((x, y), r, facecolor="none") ] + m = re.match("p%s: x=%s y=%s a=%s"%(INT, FLOAT, FLOAT, FLOAT), l) + if m: + n,x,y,a = (float(m.groups()[0]), float(m.groups()[1]), + float(m.groups()[2]), float(m.groups()[3])) + if (n == 0): + patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ] + result_pt = (x, y) + text += l + "\n" + + pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6)) + pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2)) + + patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ] + pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1)) + + # text area, far from the point + l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.), + (2200., 1800.), (2200., 500.)] + l.sort(cmp=lambda p1,p2: (dist(p1,real_pt) 2cm (pour un deplacement\n" 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period)) -#do_random_test() -#do_graph_2d_simple_error() +do_random_test() +do_graph_2d_simple_error() do_graph_2d_move_error() +do_graph_2d_ad_error() diff --git a/projects/microb2010/tests/tourel_beacon/main.c b/projects/microb2010/tests/tourel_beacon/main.c index 38ab0a7..97937d4 100644 --- a/projects/microb2010/tests/tourel_beacon/main.c +++ b/projects/microb2010/tests/tourel_beacon/main.c @@ -38,10 +38,23 @@ static int dprint = 0; #define dprintf(args...) if (dprint) printf(args) +#define AREA_X 3000 +#define AREA_Y 2100 + +#define ANGLE_EPSILON 0.005 + const point_t beacon0 = { 0, 1050 }; const point_t beacon1 = { 3000, 0 }; const point_t beacon2 = { 3000, 2100 }; +static inline double abs_dbl(double x) +{ + if (x > 0) + return x; + else + return -x; +} + /* 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 @@ -80,8 +93,8 @@ int8_t angle_to_circles(circle_t *c1, circle_t *c2, vect_t v; float l, d; - /* reject negative or too small angles */ - if (a_rad <= 0.01) + /* reject too small angles */ + if (a_rad <= 0.01 && a_rad >= -0.01) return -1; /* get position of O */ @@ -103,14 +116,14 @@ int8_t angle_to_circles(circle_t *c1, circle_t *c2, 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); + c1->r = xy_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); + c2->r = xy_norm(b1->x, b1->y, c1->x, c1->y); } return 0; @@ -149,9 +162,9 @@ int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20) 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) */ + /* if (xy_norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */ + /* xy_norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */ + /* xy_norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */ /* return -1; */ pos->x = (p1.x + p2.x + p3.x) / 3.0; @@ -160,22 +173,152 @@ int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20) 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) +static inline int8_t is_pt_in_area(point_t pt) { - double a0, a1, a2; + if (pt.x < 0 || pt.y < 0) + return 0; + if (pt.x > AREA_X || pt.y > AREA_Y) + return 0; + return 1; +} - 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); +/* intersect the circle formed by one distance info with the circle + * crossing the 2 beacons */ +static int8_t +ad_to_posxy_one(point_t *pos, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd, const circle_t *c01, /* circles to intersect */ + double a01) /* seen angle of beacons */ +{ + double tmp_a01_p1, tmp_a01_p2; + point_t p1, p2; + uint8_t p1_ok=0, p2_ok=0; + + if (circle_intersect(c01, cd, &p1, &p2) == 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("real a01: %2.2f\n", a01); + + tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) - + atan2(b0->y-p1.y, b0->x-p1.x); + dprintf("a01-1: %2.2f\n", tmp_a01_p1); + + tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) - + atan2(b0->y-p2.y, b0->x-p2.x); + dprintf("a01-2: %2.2f\n", tmp_a01_p2); + + /* in some conditions, we already know the position of the + * robot at this step */ + p1_ok = is_pt_in_area(p1) && + abs_dbl(tmp_a01_p1 - a01) < ANGLE_EPSILON; + + p2_ok = is_pt_in_area(p2) && + abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON; + if (!p1_ok && p2_ok) { + *pos = p2; + return 0; + } + if (p1_ok && !p2_ok) { + *pos = p1; + return 0; + } + return -1; +} + + +/* get the position and angle of the robot from the angle of the 2 + * beacons, and the distance of 2 beacons */ +static int8_t ad_to_posxya(point_t *pos, double *a, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + double a0, double a1, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) +{ + circle_t cd0, cd1, c01; + double a01; + + dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n", + algo, a0, a1, d0, d1); + + /* the first 2 circles depends on distance between robot and + * beacons */ + cd0.x = b0->x; + cd0.y = b0->y; + cd0.r = d0; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r); + cd1.x = b1->x; + cd1.y = b1->y; + cd1.r = d1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r); + + /* the third circle depends on angle between b0 and b1 */ + a01 = a1-a0; + if (angle_to_circles(&c01, NULL, b0, b1, a01)) + return -1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r); + + switch (algo) { + + /* take the closer beacon first */ + case 0: + if (d0 < d1) { + if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + return 0; + if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + return 0; + return -1; + } + else { + if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + return 0; + if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + return 0; + return -1; + } + break; + case 1: + /* b0 only */ + if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + return 0; + break; + case 2: + /* b0 only */ + if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + return 0; + break; + default: + break; + } + /* not found */ + return -1; +} + +/* get the angles of beacons from xy pos */ +int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1, + double *a2, int err_num, float err_val) +{ + *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.); + *a0 += (err_val * M_PI/180.); if (err_num == 1 || err_num == 3) - a1 += (err_val * M_PI/180.); + *a1 += (err_val * M_PI/180.); if (err_num == 2 || err_num == 3) - a2 += (err_val * M_PI/180.); + *a2 += (err_val * M_PI/180.); + + 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; + posxy_to_abs_angles(pos, &a0, &a1, &a2, err_num, err_val); *a01 = a1-a0; if (*a01 < 0) @@ -250,6 +393,7 @@ int8_t process_move_error(double x, double y, double speed, /* whole process is around 3ms on atmega128 at 16Mhz */ int main(int argc, char **argv) { + double a0, a1, a2; double a01, a12, a20; point_t pos, tmp; const char *mode = "nothing"; @@ -263,24 +407,62 @@ int main(int argc, char **argv) #else mode = "angle2pos"; argc = 5; - a01 = 1.65; - a12 = 2.12; - a20 = 2.53; + a0 = -1.; + a1 = 1.65; + a2 = 3.03; #endif + /* + * angle2pos a0 a1 a2: + * (angles in radian, -pi < a < pi) + */ if (argc == 5 && strcmp(mode, "angle2pos") == 0) { #ifdef HOST_VERSION dprint = 1; - a01 = atof(argv[2]); - a12 = atof(argv[3]); - a20 = atof(argv[4]); + a0 = atof(argv[2]); + a1 = atof(argv[3]); + a2 = atof(argv[4]); #endif + + /* */ + a01 = a1-a0; + if (a01 < 0) + a01 += 2*M_PI; + a12 = a2-a1; + if (a12 < 0) + a12 += 2*M_PI; + a20 = a0-a2; + if (a20 < 0) + a20 += 2*M_PI; + if (angles_to_posxy(&pos, a01, a12, a20) < 0) return -1; printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y); return 0; } + /* + * ad2pos a0 a1 d0 d1: + * (angles in radian, -pi < a < pi) + * (distances in mm) + */ + if (argc == 6 && strcmp(mode, "ad2pos") == 0) { + double a, d0, d1; + + dprint = 1; + a0 = atof(argv[2]); + a1 = atof(argv[3]); + d0 = atof(argv[4]); + d1 = atof(argv[5]); + + /* */ + if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, + a0, a1, d0, d1) < 0) + return -1; + printf("p0: x=%2.2f y=%2.2f a=%2.2f\n", pos.x, pos.y, a); + return 0; + } + if (argc == 4 && strcmp(mode, "simple_error") == 0) { int x, y; int err_num; @@ -307,6 +489,42 @@ int main(int argc, char **argv) return 0; } + /* da_error algo errpercent errdeg */ + if (argc == 5 && strcmp(mode, "da_error") == 0) { + int x, y, algo; + double err_val_deg; + double err_val_percent; + double err, d0, d1, a; + + algo = atoi(argv[2]); + err_val_percent = atof(argv[3]); /* how many % of error for dist */ + err_val_deg = atof(argv[4]); /* how many degrees of error */ + + for (x=0; x<300; x++) { + for (y=0; y<210; y++) { + + pos.x = x*10; + pos.y = y*10; + posxy_to_abs_angles(pos, &a0, &a1, &a2, + 0, err_val_deg); + d0 = pt_norm(&pos, &beacon0); + d0 += d0 * err_val_percent / 100.; + d1 = pt_norm(&pos, &beacon1); + d1 += d1 * err_val_percent / 100.; + + if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1, + a0, a1, d0, d1) < 0) + continue; + + err = pt_norm(&tmp, &pos); + if (err > 50.) /* saturate error to 5cm */ + err = 50.; + printf("%d %d %2.2f\n", x, y, err); + } + } + return 0; + } + if ((argc == 5 || argc == 7) && strcmp(argv[1], "move_error") == 0) { int x, y; -- 2.39.5