X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Ftourel_beacon%2Fmain.c;h=90fa109cbfba3083376159b601d2098268d9d5f1;hp=97937d432f2edb200cd87d4d327ee3f4579ccd3b;hb=657979175089eef7f2de8918b6bff47dbd08f2e2;hpb=9b20b69a87c9d442cf4610351dc40b28b7f36e9c diff --git a/projects/microb2010/tests/tourel_beacon/main.c b/projects/microb2010/tests/tourel_beacon/main.c index 97937d4..90fa109 100644 --- a/projects/microb2010/tests/tourel_beacon/main.c +++ b/projects/microb2010/tests/tourel_beacon/main.c @@ -204,10 +204,18 @@ ad_to_posxy_one(point_t *pos, tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) - atan2(b0->y-p1.y, b0->x-p1.x); + if (tmp_a01_p1 > M_PI) + tmp_a01_p1 -= 2*M_PI; + if (tmp_a01_p1 < -M_PI) + tmp_a01_p1 += 2*M_PI; 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); + if (tmp_a01_p2 > M_PI) + tmp_a01_p2 -= 2*M_PI; + if (tmp_a01_p2 < -M_PI) + tmp_a01_p2 += 2*M_PI; dprintf("a01-2: %2.2f\n", tmp_a01_p2); /* in some conditions, we already know the position of the @@ -228,64 +236,41 @@ ad_to_posxy_one(point_t *pos, return -1; } +static int8_t +ad_to_posxy_algo(point_t *pos, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd0, const circle_t *cd1, const circle_t *c01, /* circles to intersect */ + double a01, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) -/* 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) + 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) + 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) + 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) + 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) + 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) + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) return 0; break; default: @@ -295,6 +280,59 @@ static int8_t ad_to_posxya(point_t *pos, double *a, int algo, 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 (a01 < -M_PI) + a01 += 2*M_PI; + if (a01 > M_PI) + a01 -= 2*M_PI; + + 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); + + /* get the xy position, depending on algo */ + if (ad_to_posxy_algo(pos, algo, b0, b1, &cd0, &cd1, &c01, a01, d0, d1) < 0) + return -1; + + /* now, we have the xy position, we can get angle thanks to + * the angles a0 and a1. Take the far beacon. */ + if (d0 < d1) + *a = atan2(beacon1.y - pos->y, beacon1.x - pos->x) - a1; + else + *a = atan2(beacon0.y - pos->y, beacon0.x - pos->x) - a0; + if (*a < -M_PI) + *a += 2*M_PI; + if (*a > M_PI) + *a -= 2*M_PI; + return 0; +} + /* 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) @@ -474,6 +512,7 @@ int main(int argc, char **argv) for (x=0; x<300; x++) { for (y=0; y<210; y++) { + pos.x = x*10; pos.y = y*10; posxy_to_angles(pos, &a01, &a12, &a20, @@ -490,14 +529,17 @@ int main(int argc, char **argv) } /* da_error algo errpercent errdeg */ - if (argc == 5 && strcmp(mode, "da_error") == 0) { + if ((argc == 5 && strcmp(mode, "da_error") == 0) || + (argc == 5 && strcmp(mode, "da_error_mm") == 0)) { int x, y, algo; double err_val_deg; double err_val_percent; + double err_val_mm; double err, d0, d1, a; algo = atoi(argv[2]); err_val_percent = atof(argv[3]); /* how many % of error for dist */ + err_val_mm = atof(argv[3]); /* how many mm of error for dist */ err_val_deg = atof(argv[4]); /* how many degrees of error */ for (x=0; x<300; x++) { @@ -508,9 +550,15 @@ int main(int argc, char **argv) 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 (strcmp(mode, "da_error") == 0) { + d0 += d0 * err_val_percent / 100.; + d1 += d1 * err_val_percent / 100.; + } + else { + d0 += err_val_mm; + d1 += err_val_mm; + } if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1, a0, a1, d0, d1) < 0)