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
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:
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)
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,
}
/* 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++) {
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)
git.droids-corp.org / aversive.git / blobdiff