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