trajectory optimizations
[aversive.git] / 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