projects/microb2010/tests/beacon_tsop/trigo.c

   1 /*
   2  *  Copyright Droids Corporation (2009)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include <aversive.h>
  28
  29 #include <vect_base.h>
  30 #include <lines.h>
  31 #include <circles.h>
  32
  33 #define POS_ACCURACY 10.0 /* 1 cm accuracy max */
  34 #ifndef HOST_VERSION
  35 #define printf(args...) do {} while(0)
  36 #endif
  37
  38 static int dprint = 0;
  39 #define dprintf(args...) if (dprint) printf(args)
  40
  41 #define AREA_X 3000
  42 #define AREA_Y 2100
  43
  44 #define ANGLE_EPSILON 0.005
  45
  46 const point_t beacon0 = { 0, 1050 };
  47 const point_t beacon1 = { 3000, 0 };
  48 const point_t beacon2 = { 3000, 2100 };
  49
  50 static inline double abs_dbl(double x)
  51 {
  52         if (x > 0)
  53                 return x;
  54         else
  55                 return -x;
  56 }
  57
  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.
  62  *
  63  *                              l
  64  *                <------------------------->
  65  *
  66  *              b1              O            b2
  67  *               +----------------------------+
  68  *             ,' \\            |            /|'\
  69  *            /    \ \          | ^        / |   `
  70  *           /       \ \   a___ | | d    /   |    `.
  71  *          /         \  \ /    | v    /     |     \
  72  *         |            \  \    |    /       |      |
  73  *         |             \   \  |  /        |       |
  74  *         |               \   \|/          |        |
  75  *         |                \   * C         |        |
  76  *         |                  \             |       .'
  77  *         |                   \           |        |
  78  *          |                    \         |       .'
  79  *           \                    \   a____|       /
  80  *            \                     \ /    |     ,'
  81  *             `                     \    |     /
  82  *              '.                     \  |   ,'
  83  *                '-.                   \ |_,'
  84  *                   '-._              _,*'
  85  *                       '`--......---'     R (the robot)
  86  *
  87  */
  88 int8_t angle_to_circles(circle_t *c1, circle_t *c2,
  89                          const point_t *b1, const point_t *b2,
  90                          double a_rad)
  91 {
  92         point_t O;
  93         vect_t v;
  94         float l, d;
  95
  96         /* reject too small angles */
  97         if (a_rad <= 0.01 && a_rad >= -0.01)
  98                 return -1;
  99
 100         /* get position of O */
 101         O.x = (b1->x + b2->x) / 2;
 102         O.y = (b1->y + b2->y) / 2;
 103
 104         /* get the length l */
 105         v.x = b2->x - b1->x;
 106         v.y = b2->y - b1->y;
 107         l = vect_norm(&v);
 108
 109         /* distance from O to the center of the circle */
 110         /* XXX div by 0 when pi */
 111         d = l / (2 * tan(a_rad));
 112
 113         /* get the circle c1 */
 114         vect_rot_trigo(&v);
 115         vect_resize(&v, d);
 116         if (c1) {
 117                 c1->x = O.x + v.x;
 118                 c1->y = O.y + v.y;
 119                 c1->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
 120         }
 121
 122         /* get the circle c2 */
 123         if (c2) {
 124                 c2->x = O.x - v.x;
 125                 c2->y = O.y - v.y;
 126                 c2->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
 127         }
 128
 129         return 0;
 130 }
 131
 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)
 134 {
 135         circle_t c01, c12, c20;
 136         point_t dummy_pt, p1, p2, p3;
 137
 138         dprintf("a01 = %2.2f\n", a01);
 139         dprintf("a12 = %2.2f\n", a12);
 140         dprintf("a20 = %2.2f\n", a20);
 141
 142         if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
 143                 return -1;
 144         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
 145
 146         if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
 147                 return -1;
 148         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
 149
 150         if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
 151                 return -1;
 152         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
 153
 154         if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
 155                 return -1;
 156         if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
 157                 return -1;
 158         if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
 159                 return -1;
 160
 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);
 164
 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) */
 168         /*      return -1; */
 169
 170         pos->x = (p1.x + p2.x + p3.x) / 3.0;
 171         pos->y = (p1.y + p2.y + p3.y) / 3.0;
 172
 173         return 0;
 174 }
 175
 176 static inline int8_t is_pt_in_area(point_t pt)
 177 {
 178         if (pt.x < 0 || pt.y < 0)
 179                 return 0;
 180         if (pt.x > AREA_X || pt.y > AREA_Y)
 181                 return 0;
 182         return 1;
 183 }
 184
 185 /* intersect the circle formed by one distance info with the circle
 186  * crossing the 2 beacons */
 187 static int8_t
 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 */
 192 {
 193         double tmp_a01_p1, tmp_a01_p2;
 194         point_t p1, p2;
 195         uint8_t p1_ok=0, p2_ok=0;
 196
 197         if (circle_intersect(c01, cd, &p1, &p2) == 0)
 198                 return -1;
 199
 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);
 202
 203         dprintf("real a01: %2.2f\n", a01);
 204
 205         tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) -
 206                 atan2(b0->y-p1.y, b0->x-p1.x);
 207         if (tmp_a01_p1 > M_PI)
 208                 tmp_a01_p1 -= 2*M_PI;
 209         if (tmp_a01_p1 < -M_PI)
 210                 tmp_a01_p1 += 2*M_PI;
 211         dprintf("a01-1: %2.2f\n", tmp_a01_p1);
 212
 213         tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) -
 214                 atan2(b0->y-p2.y, b0->x-p2.x);
 215         if (tmp_a01_p2 > M_PI)
 216                 tmp_a01_p2 -= 2*M_PI;
 217         if (tmp_a01_p2 < -M_PI)
 218                 tmp_a01_p2 += 2*M_PI;
 219         dprintf("a01-2: %2.2f\n", tmp_a01_p2);
 220
 221         /* in some conditions, we already know the position of the
 222          * robot at this step */
 223         p1_ok = is_pt_in_area(p1) &&
 224                 abs_dbl(tmp_a01_p1 - a01) < ANGLE_EPSILON;
 225
 226         p2_ok = is_pt_in_area(p2) &&
 227                 abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON;
 228         if (!p1_ok && p2_ok) {
 229                 *pos = p2;
 230                 return 0;
 231         }
 232         if (p1_ok && !p2_ok) {
 233                 *pos = p1;
 234                 return 0;
 235         }
 236         return -1;
 237 }
 238
 239 static int8_t
 240 ad_to_posxy_algo(point_t *pos, int algo,
 241                  const point_t *b0, const point_t *b1, /* beacon position */
 242                  const circle_t *cd0, const circle_t *cd1, const circle_t *c01, /* circles to intersect */
 243                  double a01, /* seen angle of beacons */
 244                  double d0, double d1 /* distance to beacons */ )
 245
 246 {
 247         switch (algo) {
 248
 249                 /* take the closer beacon first */
 250         case 0:
 251                 if (d0 < d1) {
 252                         if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0)
 253                                 return 0;
 254                         if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0)
 255                                 return 0;
 256                         return -1;
 257                 }
 258                 else {
 259                         if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0)
 260                                 return 0;
 261                         if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0)
 262                                 return 0;
 263                         return -1;
 264                 }
 265                 break;
 266         case 1:
 267                 /* b0 only */
 268                 if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0)
 269                         return 0;
 270                 break;
 271         case 2:
 272                 /* b0 only */
 273                 if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0)
 274                         return 0;
 275                 break;
 276         default:
 277                 break;
 278         }
 279         /* not found */
 280         return -1;
 281 }
 282
 283 /* get the position and angle of the robot from the angle of the 2
 284  * beacons, and the distance of 2 beacons */
 285 int8_t
 286 ad_to_posxya(point_t *pos, double *a, int algo,
 287              const point_t *b0, const point_t *b1, /* beacon position */
 288              double a0, double a1, /* seen angle of beacons */
 289              double d0, double d1 /* distance to beacons */ )
 290 {
 291         circle_t cd0, cd1, c01;
 292         double a01;
 293
 294         dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n",
 295                 algo, a0, a1, d0, d1);
 296
 297         /* the first 2 circles depends on distance between robot and
 298          * beacons */
 299         cd0.x = b0->x;
 300         cd0.y = b0->y;
 301         cd0.r = d0;
 302         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r);
 303         cd1.x = b1->x;
 304         cd1.y = b1->y;
 305         cd1.r = d1;
 306         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r);
 307
 308         /* the third circle depends on angle between b0 and b1 */
 309         a01 = a1-a0;
 310         if (a01 < -M_PI)
 311                 a01 += 2*M_PI;
 312         if (a01 > M_PI)
 313                 a01 -= 2*M_PI;
 314
 315         if (angle_to_circles(&c01, NULL, b0, b1, a01))
 316                 return -1;
 317         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
 318
 319         /* get the xy position, depending on algo */
 320         if (ad_to_posxy_algo(pos, algo, b0, b1, &cd0, &cd1, &c01, a01, d0, d1) < 0)
 321                 return -1;
 322
 323         /* now, we have the xy position, we can get angle thanks to
 324          * the angles a0 and a1. Take the far beacon. */
 325         if (d0 < d1)
 326                 *a = atan2(beacon1.y - pos->y, beacon1.x - pos->x) - a1;
 327         else
 328                 *a = atan2(beacon0.y - pos->y, beacon0.x - pos->x) - a0;
 329         if (*a < -M_PI)
 330                 *a += 2*M_PI;
 331         if (*a > M_PI)
 332                 *a -= 2*M_PI;
 333         return 0;
 334 }
 335
 336 /* get the angles of beacons from xy pos */
 337 int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1,
 338                            double *a2, int err_num, float err_val)
 339 {
 340         *a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
 341         *a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
 342         *a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
 343
 344         if (err_num == 0 || err_num == 3)
 345                 *a0 += (err_val * M_PI/180.);
 346         if (err_num == 1 || err_num == 3)
 347                 *a1 += (err_val * M_PI/180.);
 348         if (err_num == 2 || err_num == 3)
 349                 *a2 += (err_val * M_PI/180.);
 350
 351         return 0;
 352 }
 353
 354 /* get the angles of beacons from xy pos */
 355 int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
 356                        double *a20, int err_num, float err_val)
 357 {
 358         double a0, a1, a2;
 359         posxy_to_abs_angles(pos, &a0, &a1, &a2, err_num, err_val);
 360
 361         *a01 = a1-a0;
 362         if (*a01 < 0)
 363                 *a01 += M_PI*2;
 364         *a12 = a2-a1;
 365         if (*a12 < 0)
 366                 *a12 += M_PI*2;
 367         *a20 = a0-a2;
 368         if (*a20 < 0)
 369                 *a20 += M_PI*2;
 370
 371         return 0;
 372 }
 373
 374 int8_t process_move_error(double x, double y, double speed,
 375                           double period, double angle, double *err)
 376 {
 377         double a01, a12, a20;
 378         point_t pos, tmp;
 379         double a0, a1, a2;
 380         vect_t u,v;
 381         point_t pos2, pos3;
 382
 383         pos.x = x;
 384         pos.y = y;
 385
 386         /* from start to destination */
 387         v.x = cos(angle) * speed * period;
 388         v.y = sin(angle) * speed * period;
 389
 390         /* first process real pos */
 391         posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
 392
 393         /* vector covered during measure of a0 and a1 */
 394         u.x = v.x * a01 / (2*M_PI);
 395         u.y = v.y * a01 / (2*M_PI);
 396         pos2.x = pos.x + u.x;
 397         pos2.y = pos.y + u.y;
 398
 399         /* vector covered during measure of a1 and a2 */
 400         u.x = v.x * a12 / (2*M_PI);
 401         u.y = v.y * a12 / (2*M_PI);
 402         pos3.x = pos2.x + u.x;
 403         pos3.y = pos2.y + u.y;
 404
 405         dprintf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
 406         dprintf("p1: x=%2.2f y=%2.2f\n", pos2.x, pos2.y);
 407         dprintf("p2: x=%2.2f y=%2.2f\n", pos3.x, pos3.y);
 408
 409         a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
 410         a1 = atan2(beacon1.y-pos2.y, beacon1.x-pos2.x);
 411         a2 = atan2(beacon2.y-pos3.y, beacon2.x-pos3.x);
 412
 413         a01 = a1-a0;
 414         if (a01 < 0)
 415                 a01 += M_PI*2;
 416         a12 = a2-a1;
 417         if (a12 < 0)
 418                 a12 += M_PI*2;
 419         a20 = a0-a2;
 420         if (a20 < 0)
 421                 a20 += M_PI*2;
 422
 423         if (angles_to_posxy(&tmp, a01, a12, a20))
 424                 return -1;
 425         *err = pt_norm(&tmp, &pos);
 426         if (*err > 50.) /* saturate error to 5cm */
 427                 *err = 50.;
 428         return 0;
 429 }
 430
 431 #if 0
 432 /* whole process is around 3ms on atmega128 at 16Mhz */
 433 int main(int argc, char **argv)
 434 {
 435         double a0, a1, a2;
 436         double a01, a12, a20;
 437         point_t pos, tmp;
 438         const char *mode = "nothing";
 439
 440 #ifdef HOST_VERSION
 441         if (argc < 2) {
 442                 printf("bad args\n");
 443                 return -1;
 444         }
 445         mode = argv[1];
 446 #else
 447         mode = "angle2pos";
 448         argc = 5;
 449         a0 = -1.;
 450         a1 = 1.65;
 451         a2 = 3.03;
 452 #endif
 453
 454         /*
 455          * angle2pos a0 a1 a2:
 456          *   (angles in radian, -pi < a < pi)
 457          */
 458         if (argc == 5 && strcmp(mode, "angle2pos") == 0) {
 459 #ifdef HOST_VERSION
 460                 dprint = 1;
 461                 a0 = atof(argv[2]);
 462                 a1 = atof(argv[3]);
 463                 a2 = atof(argv[4]);
 464 #endif
 465
 466                 /* */
 467                 a01 = a1-a0;
 468                 if (a01 < 0)
 469                         a01 += 2*M_PI;
 470                 a12 = a2-a1;
 471                 if (a12 < 0)
 472                         a12 += 2*M_PI;
 473                 a20 = a0-a2;
 474                 if (a20 < 0)
 475                         a20 += 2*M_PI;
 476
 477                 if (angles_to_posxy(&pos, a01, a12, a20) < 0)
 478                         return -1;
 479                 printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
 480                 return 0;
 481         }
 482
 483         /*
 484          * ad2pos a0 a1 d0 d1:
 485          *   (angles in radian, -pi < a < pi)
 486          *   (distances in mm)
 487          */
 488         if (argc == 6 && strcmp(mode, "ad2pos") == 0) {
 489                 double a, d0, d1;
 490
 491                 dprint = 1;
 492                 a0 = atof(argv[2]);
 493                 a1 = atof(argv[3]);
 494                 d0 = atof(argv[4]);
 495                 d1 = atof(argv[5]);
 496
 497                 /* */
 498                 if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1,
 499                                  a0, a1, d0, d1) < 0)
 500                         return -1;
 501                 printf("p0: x=%2.2f y=%2.2f a=%2.2f\n", pos.x, pos.y, a);
 502                 return 0;
 503         }
 504
 505         if (argc == 4 && strcmp(mode, "simple_error") == 0) {
 506                 int x, y;
 507                 int err_num;
 508                 double err_val_deg;
 509                 double err;
 510
 511                 err_num = atof(argv[2]); /* which beacon sees an error */
 512                 err_val_deg = atof(argv[3]); /* how many degrees of error */
 513
 514                 for (x=0; x<300; x++) {
 515                         for (y=0; y<210; y++) {
 516
 517                                 pos.x = x*10;
 518                                 pos.y = y*10;
 519                                 posxy_to_angles(pos, &a01, &a12, &a20,
 520                                                 err_num, err_val_deg);
 521                                 if (angles_to_posxy(&tmp, a01, a12, a20))
 522                                         continue;
 523                                 err = pt_norm(&tmp, &pos);
 524                                 if (err > 50.) /* saturate error to 5cm */
 525                                         err = 50.;
 526                                 printf("%d %d %2.2f\n", x, y, err);
 527                         }
 528                 }
 529                 return 0;
 530         }
 531
 532         /* da_error algo errpercent errdeg */
 533         if ((argc == 5 && strcmp(mode, "da_error") == 0) ||
 534             (argc == 5 && strcmp(mode, "da_error_mm") == 0)) {
 535                 int x, y, algo;
 536                 double err_val_deg;
 537                 double err_val_percent;
 538                 double err_val_mm;
 539                 double err, d0, d1, a;
 540
 541                 algo = atoi(argv[2]);
 542                 err_val_percent = atof(argv[3]); /* how many % of error for dist */
 543                 err_val_mm = atof(argv[3]); /* how many mm of error for dist */
 544                 err_val_deg = atof(argv[4]); /* how many degrees of error */
 545
 546                 for (x=0; x<300; x++) {
 547                         for (y=0; y<210; y++) {
 548
 549                                 pos.x = x*10;
 550                                 pos.y = y*10;
 551                                 posxy_to_abs_angles(pos, &a0, &a1, &a2,
 552                                                     0, err_val_deg);
 553                                 d0 = pt_norm(&pos, &beacon0);
 554                                 d1 = pt_norm(&pos, &beacon1);
 555                                 if (strcmp(mode, "da_error") == 0) {
 556                                         d0 += d0 * err_val_percent / 100.;
 557                                         d1 += d1 * err_val_percent / 100.;
 558                                 }
 559                                 else {
 560                                         d0 += err_val_mm;
 561                                         d1 += err_val_mm;
 562                                 }
 563
 564                                 if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1,
 565                                                  a0, a1, d0, d1) < 0)
 566                                         continue;
 567
 568                                 err = pt_norm(&tmp, &pos);
 569                                 if (err > 50.) /* saturate error to 5cm */
 570                                         err = 50.;
 571                                 printf("%d %d %2.2f\n", x, y, err);
 572                         }
 573                 }
 574                 return 0;
 575         }
 576
 577         if ((argc == 5 || argc == 7)
 578             && strcmp(argv[1], "move_error") == 0) {
 579                 int x, y;
 580                 double angle, speed, period, err;
 581
 582                 speed = atof(argv[2]); /* speed in m/s ( = mm/ms) */
 583                 period = atof(argv[3]); /* period of turret in ms */
 584                 angle = atof(argv[4]); /* direction of moving */
 585                 if (argc == 7) {
 586                         dprint = 1;
 587                         process_move_error(atof(argv[5]), atof(argv[6]),
 588                                            speed, period, angle, &err);
 589                         printf("%2.2f %2.2f %2.2f\n", atof(argv[5]),
 590                                atof(argv[6]), err);
 591                         return 0;
 592                 }
 593
 594                 for (x=0; x<300; x++) {
 595                         for (y=0; y<210; y++) {
 596                                 pos.x = x*10;
 597                                 pos.y = y*10;
 598                                 if (process_move_error(pos.x, pos.y,
 599                                                        speed, period, angle,
 600                                                        &err) < 0)
 601                                         continue;
 602                                 printf("%d %d %2.2f\n", x, y, err);
 603                         }
 604                 }
 605                 return 0;
 606         }
 607
 608         printf("bad args\n");
 609         return -1;
 610 }
 611 #endif