2 * Copyright Droids Corporation (2009)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
29 #include <vect_base.h>
33 #define POS_ACCURACY 10.0 /* 1 cm accuracy max */
35 #define printf(args...) do {} while(0)
38 static int dprint = 0;
39 #define dprintf(args...) if (dprint) printf(args)
44 #define ANGLE_EPSILON 0.005
46 const point_t beacon0 = { 0, 1050 };
47 const point_t beacon1 = { 3000, 0 };
48 const point_t beacon2 = { 3000, 2100 };
50 static inline double abs_dbl(double x)
58 /* Fill the 2 circles pointer given as parameter, each of those cross
59 * both beacons b1 and b2. From any point of these circles (except b1
60 * and b2), we see b1 and b2 with the angle of a_rad (which must be
61 * positive). Return 0 on success.
64 * <------------------------->
67 * +----------------------------+
70 * / \ \ a___ | | d / | `.
85 * '`--......---' R (the robot)
88 int8_t angle_to_circles(circle_t *c1, circle_t *c2,
89 const point_t *b1, const point_t *b2,
96 /* reject too small angles */
97 if (a_rad <= 0.01 && a_rad >= -0.01)
100 /* get position of O */
101 O.x = (b1->x + b2->x) / 2;
102 O.y = (b1->y + b2->y) / 2;
104 /* get the length l */
109 /* distance from O to the center of the circle */
110 /* XXX div by 0 when pi */
111 d = l / (2 * tan(a_rad));
113 /* get the circle c1 */
119 c1->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
122 /* get the circle c2 */
126 c2->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
132 /* get the position of the robot from the angle of the 3 beacons */
133 int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20)
135 circle_t c01, c12, c20;
136 point_t dummy_pt, p1, p2, p3;
138 dprintf("a01 = %2.2f\n", a01);
139 dprintf("a12 = %2.2f\n", a12);
140 dprintf("a20 = %2.2f\n", a20);
142 if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
144 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
146 if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
148 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
150 if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
152 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
154 if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
156 if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
158 if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
161 dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
162 dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
163 dprintf("p3: x=%2.2f y=%2.2f\n", p3.x, p3.y);
165 /* if (xy_norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
166 /* xy_norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
167 /* xy_norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
170 pos->x = (p1.x + p2.x + p3.x) / 3.0;
171 pos->y = (p1.y + p2.y + p3.y) / 3.0;
176 static inline int8_t is_pt_in_area(point_t pt)
178 if (pt.x < 0 || pt.y < 0)
180 if (pt.x > AREA_X || pt.y > AREA_Y)
185 /* intersect the circle formed by one distance info with the circle
186 * crossing the 2 beacons */
188 ad_to_posxy_one(point_t *pos,
189 const point_t *b0, const point_t *b1, /* beacon position */
190 const circle_t *cd, const circle_t *c01, /* circles to intersect */
191 double a01) /* seen angle of beacons */
193 double tmp_a01_p1, tmp_a01_p2;
195 uint8_t p1_ok=0, p2_ok=0;
197 if (circle_intersect(c01, cd, &p1, &p2) == 0)
200 dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
201 dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
203 dprintf("real a01: %2.2f\n", a01);
205 tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) -
206 atan2(b0->y-p1.y, b0->x-p1.x);
207 dprintf("a01-1: %2.2f\n", tmp_a01_p1);
209 tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) -
210 atan2(b0->y-p2.y, b0->x-p2.x);
211 dprintf("a01-2: %2.2f\n", tmp_a01_p2);
213 /* in some conditions, we already know the position of the
214 * robot at this step */
215 p1_ok = is_pt_in_area(p1) &&
216 abs_dbl(tmp_a01_p1 - a01) < ANGLE_EPSILON;
218 p2_ok = is_pt_in_area(p2) &&
219 abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON;
220 if (!p1_ok && p2_ok) {
224 if (p1_ok && !p2_ok) {
232 /* get the position and angle of the robot from the angle of the 2
233 * beacons, and the distance of 2 beacons */
234 static int8_t ad_to_posxya(point_t *pos, double *a, int algo,
235 const point_t *b0, const point_t *b1, /* beacon position */
236 double a0, double a1, /* seen angle of beacons */
237 double d0, double d1 /* distance to beacons */ )
239 circle_t cd0, cd1, c01;
242 dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n",
243 algo, a0, a1, d0, d1);
245 /* the first 2 circles depends on distance between robot and
250 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r);
254 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r);
256 /* the third circle depends on angle between b0 and b1 */
258 if (angle_to_circles(&c01, NULL, b0, b1, a01))
260 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
264 /* take the closer beacon first */
267 if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
269 if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
274 if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
276 if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
283 if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
288 if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
298 /* get the angles of beacons from xy pos */
299 int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1,
300 double *a2, int err_num, float err_val)
302 *a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
303 *a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
304 *a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
306 if (err_num == 0 || err_num == 3)
307 *a0 += (err_val * M_PI/180.);
308 if (err_num == 1 || err_num == 3)
309 *a1 += (err_val * M_PI/180.);
310 if (err_num == 2 || err_num == 3)
311 *a2 += (err_val * M_PI/180.);
316 /* get the angles of beacons from xy pos */
317 int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
318 double *a20, int err_num, float err_val)
321 posxy_to_abs_angles(pos, &a0, &a1, &a2, err_num, err_val);
336 int8_t process_move_error(double x, double y, double speed,
337 double period, double angle, double *err)
339 double a01, a12, a20;
348 /* from start to destination */
349 v.x = cos(angle) * speed * period;
350 v.y = sin(angle) * speed * period;
352 /* first process real pos */
353 posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
355 /* vector covered during measure of a0 and a1 */
356 u.x = v.x * a01 / (2*M_PI);
357 u.y = v.y * a01 / (2*M_PI);
358 pos2.x = pos.x + u.x;
359 pos2.y = pos.y + u.y;
361 /* vector covered during measure of a1 and a2 */
362 u.x = v.x * a12 / (2*M_PI);
363 u.y = v.y * a12 / (2*M_PI);
364 pos3.x = pos2.x + u.x;
365 pos3.y = pos2.y + u.y;
367 dprintf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
368 dprintf("p1: x=%2.2f y=%2.2f\n", pos2.x, pos2.y);
369 dprintf("p2: x=%2.2f y=%2.2f\n", pos3.x, pos3.y);
371 a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
372 a1 = atan2(beacon1.y-pos2.y, beacon1.x-pos2.x);
373 a2 = atan2(beacon2.y-pos3.y, beacon2.x-pos3.x);
385 if (angles_to_posxy(&tmp, a01, a12, a20))
387 *err = pt_norm(&tmp, &pos);
388 if (*err > 50.) /* saturate error to 5cm */
393 /* whole process is around 3ms on atmega128 at 16Mhz */
394 int main(int argc, char **argv)
397 double a01, a12, a20;
399 const char *mode = "nothing";
403 printf("bad args\n");
416 * angle2pos a0 a1 a2:
417 * (angles in radian, -pi < a < pi)
419 if (argc == 5 && strcmp(mode, "angle2pos") == 0) {
438 if (angles_to_posxy(&pos, a01, a12, a20) < 0)
440 printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
445 * ad2pos a0 a1 d0 d1:
446 * (angles in radian, -pi < a < pi)
449 if (argc == 6 && strcmp(mode, "ad2pos") == 0) {
459 if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1,
462 printf("p0: x=%2.2f y=%2.2f a=%2.2f\n", pos.x, pos.y, a);
466 if (argc == 4 && strcmp(mode, "simple_error") == 0) {
472 err_num = atof(argv[2]); /* which beacon sees an error */
473 err_val_deg = atof(argv[3]); /* how many degrees of error */
475 for (x=0; x<300; x++) {
476 for (y=0; y<210; y++) {
479 posxy_to_angles(pos, &a01, &a12, &a20,
480 err_num, err_val_deg);
481 if (angles_to_posxy(&tmp, a01, a12, a20))
483 err = pt_norm(&tmp, &pos);
484 if (err > 50.) /* saturate error to 5cm */
486 printf("%d %d %2.2f\n", x, y, err);
492 /* da_error algo errpercent errdeg */
493 if (argc == 5 && strcmp(mode, "da_error") == 0) {
496 double err_val_percent;
497 double err, d0, d1, a;
499 algo = atoi(argv[2]);
500 err_val_percent = atof(argv[3]); /* how many % of error for dist */
501 err_val_deg = atof(argv[4]); /* how many degrees of error */
503 for (x=0; x<300; x++) {
504 for (y=0; y<210; y++) {
508 posxy_to_abs_angles(pos, &a0, &a1, &a2,
510 d0 = pt_norm(&pos, &beacon0);
511 d0 += d0 * err_val_percent / 100.;
512 d1 = pt_norm(&pos, &beacon1);
513 d1 += d1 * err_val_percent / 100.;
515 if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1,
519 err = pt_norm(&tmp, &pos);
520 if (err > 50.) /* saturate error to 5cm */
522 printf("%d %d %2.2f\n", x, y, err);
528 if ((argc == 5 || argc == 7)
529 && strcmp(argv[1], "move_error") == 0) {
531 double angle, speed, period, err;
533 speed = atof(argv[2]); /* speed in m/s ( = mm/ms) */
534 period = atof(argv[3]); /* period of turret in ms */
535 angle = atof(argv[4]); /* direction of moving */
538 process_move_error(atof(argv[5]), atof(argv[6]),
539 speed, period, angle, &err);
540 printf("%2.2f %2.2f %2.2f\n", atof(argv[5]),
545 for (x=0; x<300; x++) {
546 for (y=0; y<210; y++) {
549 if (process_move_error(pos.x, pos.y,
550 speed, period, angle,
553 printf("%d %d %2.2f\n", x, y, err);
559 printf("bad args\n");