projects/microb2010/tests/tourel_beacon/main.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         dprintf("a01-1: %2.2f\n", tmp_a01_p1);
 208
 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);
 212
 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;
 217
 218         p2_ok = is_pt_in_area(p2) &&
 219                 abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON;
 220         if (!p1_ok && p2_ok) {
 221                 *pos = p2;
 222                 return 0;
 223         }
 224         if (p1_ok && !p2_ok) {
 225                 *pos = p1;
 226                 return 0;
 227         }
 228         return -1;
 229 }
 230
 231
 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 */ )
 238 {
 239         circle_t cd0, cd1, c01;
 240         double a01;
 241
 242         dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n",
 243                 algo, a0, a1, d0, d1);
 244
 245         /* the first 2 circles depends on distance between robot and
 246          * beacons */
 247         cd0.x = b0->x;
 248         cd0.y = b0->y;
 249         cd0.r = d0;
 250         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r);
 251         cd1.x = b1->x;
 252         cd1.y = b1->y;
 253         cd1.r = d1;
 254         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r);
 255
 256         /* the third circle depends on angle between b0 and b1 */
 257         a01 = a1-a0;
 258         if (angle_to_circles(&c01, NULL, b0, b1, a01))
 259                 return -1;
 260         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
 261
 262         switch (algo) {
 263
 264                 /* take the closer beacon first */
 265         case 0:
 266                 if (d0 < d1) {
 267                         if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
 268                                 return 0;
 269                         if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
 270                                 return 0;
 271                         return -1;
 272                 }
 273                 else {
 274                         if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
 275                                 return 0;
 276                         if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
 277                                 return 0;
 278                         return -1;
 279                 }
 280                 break;
 281         case 1:
 282                 /* b0 only */
 283                 if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
 284                         return 0;
 285                 break;
 286         case 2:
 287                 /* b0 only */
 288                 if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
 289                         return 0;
 290                 break;
 291         default:
 292                 break;
 293         }
 294         /* not found */
 295         return -1;
 296 }
 297
 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)
 301 {
 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);
 305
 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.);
 312
 313         return 0;
 314 }
 315
 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)
 319 {
 320         double a0, a1, a2;
 321         posxy_to_abs_angles(pos, &a0, &a1, &a2, err_num, err_val);
 322
 323         *a01 = a1-a0;
 324         if (*a01 < 0)
 325                 *a01 += M_PI*2;
 326         *a12 = a2-a1;
 327         if (*a12 < 0)
 328                 *a12 += M_PI*2;
 329         *a20 = a0-a2;
 330         if (*a20 < 0)
 331                 *a20 += M_PI*2;
 332
 333         return 0;
 334 }
 335
 336 int8_t process_move_error(double x, double y, double speed,
 337                           double period, double angle, double *err)
 338 {
 339         double a01, a12, a20;
 340         point_t pos, tmp;
 341         double a0, a1, a2;
 342         vect_t u,v;
 343         point_t pos2, pos3;
 344
 345         pos.x = x;
 346         pos.y = y;
 347
 348         /* from start to destination */
 349         v.x = cos(angle) * speed * period;
 350         v.y = sin(angle) * speed * period;
 351
 352         /* first process real pos */
 353         posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
 354
 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;
 360
 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;
 366
 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);
 370
 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);
 374
 375         a01 = a1-a0;
 376         if (a01 < 0)
 377                 a01 += M_PI*2;
 378         a12 = a2-a1;
 379         if (a12 < 0)
 380                 a12 += M_PI*2;
 381         a20 = a0-a2;
 382         if (a20 < 0)
 383                 a20 += M_PI*2;
 384
 385         if (angles_to_posxy(&tmp, a01, a12, a20))
 386                 return -1;
 387         *err = pt_norm(&tmp, &pos);
 388         if (*err > 50.) /* saturate error to 5cm */
 389                 *err = 50.;
 390         return 0;
 391 }
 392
 393 /* whole process is around 3ms on atmega128 at 16Mhz */
 394 int main(int argc, char **argv)
 395 {
 396         double a0, a1, a2;
 397         double a01, a12, a20;
 398         point_t pos, tmp;
 399         const char *mode = "nothing";
 400
 401 #ifdef HOST_VERSION
 402         if (argc < 2) {
 403                 printf("bad args\n");
 404                 return -1;
 405         }
 406         mode = argv[1];
 407 #else
 408         mode = "angle2pos";
 409         argc = 5;
 410         a0 = -1.;
 411         a1 = 1.65;
 412         a2 = 3.03;
 413 #endif
 414
 415         /*
 416          * angle2pos a0 a1 a2:
 417          *   (angles in radian, -pi < a < pi)
 418          */
 419         if (argc == 5 && strcmp(mode, "angle2pos") == 0) {
 420 #ifdef HOST_VERSION
 421                 dprint = 1;
 422                 a0 = atof(argv[2]);
 423                 a1 = atof(argv[3]);
 424                 a2 = atof(argv[4]);
 425 #endif
 426
 427                 /* */
 428                 a01 = a1-a0;
 429                 if (a01 < 0)
 430                         a01 += 2*M_PI;
 431                 a12 = a2-a1;
 432                 if (a12 < 0)
 433                         a12 += 2*M_PI;
 434                 a20 = a0-a2;
 435                 if (a20 < 0)
 436                         a20 += 2*M_PI;
 437
 438                 if (angles_to_posxy(&pos, a01, a12, a20) < 0)
 439                         return -1;
 440                 printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
 441                 return 0;
 442         }
 443
 444         /*
 445          * ad2pos a0 a1 d0 d1:
 446          *   (angles in radian, -pi < a < pi)
 447          *   (distances in mm)
 448          */
 449         if (argc == 6 && strcmp(mode, "ad2pos") == 0) {
 450                 double a, d0, d1;
 451
 452                 dprint = 1;
 453                 a0 = atof(argv[2]);
 454                 a1 = atof(argv[3]);
 455                 d0 = atof(argv[4]);
 456                 d1 = atof(argv[5]);
 457
 458                 /* */
 459                 if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1,
 460                                  a0, a1, d0, d1) < 0)
 461                         return -1;
 462                 printf("p0: x=%2.2f y=%2.2f a=%2.2f\n", pos.x, pos.y, a);
 463                 return 0;
 464         }
 465
 466         if (argc == 4 && strcmp(mode, "simple_error") == 0) {
 467                 int x, y;
 468                 int err_num;
 469                 double err_val_deg;
 470                 double err;
 471
 472                 err_num = atof(argv[2]); /* which beacon sees an error */
 473                 err_val_deg = atof(argv[3]); /* how many degrees of error */
 474
 475                 for (x=0; x<300; x++) {
 476                         for (y=0; y<210; y++) {
 477                                 pos.x = x*10;
 478                                 pos.y = y*10;
 479                                 posxy_to_angles(pos, &a01, &a12, &a20,
 480                                                 err_num, err_val_deg);
 481                                 if (angles_to_posxy(&tmp, a01, a12, a20))
 482                                         continue;
 483                                 err = pt_norm(&tmp, &pos);
 484                                 if (err > 50.) /* saturate error to 5cm */
 485                                         err = 50.;
 486                                 printf("%d %d %2.2f\n", x, y, err);
 487                         }
 488                 }
 489                 return 0;
 490         }
 491
 492         /* da_error algo errpercent errdeg */
 493         if (argc == 5 && strcmp(mode, "da_error") == 0) {
 494                 int x, y, algo;
 495                 double err_val_deg;
 496                 double err_val_percent;
 497                 double err, d0, d1, a;
 498
 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 */
 502
 503                 for (x=0; x<300; x++) {
 504                         for (y=0; y<210; y++) {
 505
 506                                 pos.x = x*10;
 507                                 pos.y = y*10;
 508                                 posxy_to_abs_angles(pos, &a0, &a1, &a2,
 509                                                     0, err_val_deg);
 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.;
 514
 515                                 if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1,
 516                                                  a0, a1, d0, d1) < 0)
 517                                         continue;
 518
 519                                 err = pt_norm(&tmp, &pos);
 520                                 if (err > 50.) /* saturate error to 5cm */
 521                                         err = 50.;
 522                                 printf("%d %d %2.2f\n", x, y, err);
 523                         }
 524                 }
 525                 return 0;
 526         }
 527
 528         if ((argc == 5 || argc == 7)
 529             && strcmp(argv[1], "move_error") == 0) {
 530                 int x, y;
 531                 double angle, speed, period, err;
 532
 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 */
 536                 if (argc == 7) {
 537                         dprint = 1;
 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]),
 541                                atof(argv[6]), err);
 542                         return 0;
 543                 }
 544
 545                 for (x=0; x<300; x++) {
 546                         for (y=0; y<210; y++) {
 547                                 pos.x = x*10;
 548                                 pos.y = y*10;
 549                                 if (process_move_error(pos.x, pos.y,
 550                                                        speed, period, angle,
 551                                                        &err) < 0)
 552                                         continue;
 553                                 printf("%d %d %2.2f\n", x, y, err);
 554                         }
 555                 }
 556                 return 0;
 557         }
 558
 559         printf("bad args\n");
 560         return -1;
 561 }