projects/microb2010/tests/beacon_tsop/trigo.c

   1 /*
   2  *  Copyright Droids Corporation (2010)
   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  *  Olivier MATZ <zer0@droids-corp.org>
  19  */
  20
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include <aversive.h>
  27
  28 #include <vect_base.h>
  29 #include <lines.h>
  30 #include <circles.h>
  31
  32 #define POS_ACCURACY 10.0 /* 1 cm accuracy max */
  33 #ifndef HOST_VERSION
  34 #define printf(args...) do {} while(0)
  35 #endif
  36
  37 static int dprint = 0;
  38 #define dprintf(args...) if (dprint) printf(args)
  39
  40 const point_t beacon0 = { 0, 1050 };
  41 const point_t beacon1 = { 3000, 0 };
  42 const point_t beacon2 = { 3000, 2100 };
  43
  44 /* Fill the 2 circles pointer given as parameter, each of those cross
  45  * both beacons b1 and b2. From any point of these circles (except b1
  46  * and b2), we see b1 and b2 with the angle of a_rad (which must be
  47  * positive). Return 0 on success.
  48  *
  49  *                              l
  50  *                <------------------------->
  51  *
  52  *              b1              O            b2
  53  *               +----------------------------+
  54  *             ,' \\            |            /|'\
  55  *            /    \ \          | ^        / |   `
  56  *           /       \ \   a___ | | d    /   |    `.
  57  *          /         \  \ /    | v    /     |     \
  58  *         |            \  \    |    /       |      |
  59  *         |             \   \  |  /        |       |
  60  *         |               \   \|/          |        |
  61  *         |                \   * C         |        |
  62  *         |                  \             |       .'
  63  *         |                   \           |        |
  64  *          |                    \         |       .'
  65  *           \                    \   a____|       /
  66  *            \                     \ /    |     ,'
  67  *             `                     \    |     /
  68  *              '.                     \  |   ,'
  69  *                '-.                   \ |_,'
  70  *                   '-._              _,*'
  71  *                       '`--......---'     R (the robot)
  72  *
  73  */
  74 int8_t angle_to_circles(circle_t *c1, circle_t *c2,
  75                          const point_t *b1, const point_t *b2,
  76                          double a_rad)
  77 {
  78         point_t O;
  79         vect_t v;
  80         float l, d;
  81
  82         /* reject negative or too small angles */
  83         if (a_rad <= 0.01)
  84                 return -1;
  85
  86         /* get position of O */
  87         O.x = (b1->x + b2->x) / 2;
  88         O.y = (b1->y + b2->y) / 2;
  89
  90         /* get the length l */
  91         v.x = b2->x - b1->x;
  92         v.y = b2->y - b1->y;
  93         l = vect_norm(&v);
  94
  95         /* distance from O to the center of the circle */
  96         /* XXX div by 0 when pi */
  97         d = l / (2 * tan(a_rad));
  98
  99         /* get the circle c1 */
 100         vect_rot_trigo(&v);
 101         vect_resize(&v, d);
 102         if (c1) {
 103                 c1->x = O.x + v.x;
 104                 c1->y = O.y + v.y;
 105                 c1->r = norm(b1->x, b1->y, c1->x, c1->y);
 106         }
 107
 108         /* get the circle c2 */
 109         if (c2) {
 110                 c2->x = O.x - v.x;
 111                 c2->y = O.y - v.y;
 112                 c2->r = norm(b1->x, b1->y, c1->x, c1->y);
 113         }
 114
 115         return 0;
 116 }
 117
 118 /* get the position of the robot from the angle of the 3 beacons */
 119 int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20)
 120 {
 121         circle_t c01, c12, c20;
 122         point_t dummy_pt, p1, p2, p3;
 123
 124         dprintf("a01 = %2.2f\n", a01);
 125         dprintf("a12 = %2.2f\n", a12);
 126         dprintf("a20 = %2.2f\n", a20);
 127
 128         if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
 129                 return -1;
 130         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
 131
 132         if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
 133                 return -1;
 134         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
 135
 136         if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
 137                 return -1;
 138         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
 139
 140         if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
 141                 return -1;
 142         if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
 143                 return -1;
 144         if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
 145                 return -1;
 146
 147         dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
 148         dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
 149         dprintf("p3: x=%2.2f y=%2.2f\n", p3.x, p3.y);
 150
 151         /* if (norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
 152         /*     norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
 153         /*     norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
 154         /*      return -1; */
 155
 156         pos->x = (p1.x + p2.x + p3.x) / 3.0;
 157         pos->y = (p1.y + p2.y + p3.y) / 3.0;
 158
 159         return 0;
 160 }
 161
 162 /* get the angles of beacons from xy pos */
 163 int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
 164                        double *a20, int err_num, float err_val)
 165 {
 166         double a0, a1, a2;
 167
 168         a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
 169         a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
 170         a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
 171
 172         if (err_num == 0 || err_num == 3)
 173                 a0 += (err_val * M_PI/180.);
 174         if (err_num == 1 || err_num == 3)
 175                 a1 += (err_val * M_PI/180.);
 176         if (err_num == 2 || err_num == 3)
 177                 a2 += (err_val * M_PI/180.);
 178
 179         *a01 = a1-a0;
 180         if (*a01 < 0)
 181                 *a01 += M_PI*2;
 182         *a12 = a2-a1;
 183         if (*a12 < 0)
 184                 *a12 += M_PI*2;
 185         *a20 = a0-a2;
 186         if (*a20 < 0)
 187                 *a20 += M_PI*2;
 188
 189         return 0;
 190 }
 191
 192 int8_t process_move_error(double x, double y, double speed,
 193                           double period, double angle, double *err)
 194 {
 195         double a01, a12, a20;
 196         point_t pos, tmp;
 197         double a0, a1, a2;
 198         vect_t u,v;
 199         point_t pos2, pos3;
 200
 201         pos.x = x;
 202         pos.y = y;
 203
 204         /* from start to destination */
 205         v.x = cos(angle) * speed * period;
 206         v.y = sin(angle) * speed * period;
 207
 208         /* first process real pos */
 209         posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
 210
 211         /* vector covered during measure of a0 and a1 */
 212         u.x = v.x * a01 / (2*M_PI);
 213         u.y = v.y * a01 / (2*M_PI);
 214         pos2.x = pos.x + u.x;
 215         pos2.y = pos.y + u.y;
 216
 217         /* vector covered during measure of a1 and a2 */
 218         u.x = v.x * a12 / (2*M_PI);
 219         u.y = v.y * a12 / (2*M_PI);
 220         pos3.x = pos2.x + u.x;
 221         pos3.y = pos2.y + u.y;
 222
 223         dprintf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
 224         dprintf("p1: x=%2.2f y=%2.2f\n", pos2.x, pos2.y);
 225         dprintf("p2: x=%2.2f y=%2.2f\n", pos3.x, pos3.y);
 226
 227         a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
 228         a1 = atan2(beacon1.y-pos2.y, beacon1.x-pos2.x);
 229         a2 = atan2(beacon2.y-pos3.y, beacon2.x-pos3.x);
 230
 231         a01 = a1-a0;
 232         if (a01 < 0)
 233                 a01 += M_PI*2;
 234         a12 = a2-a1;
 235         if (a12 < 0)
 236                 a12 += M_PI*2;
 237         a20 = a0-a2;
 238         if (a20 < 0)
 239                 a20 += M_PI*2;
 240
 241         if (angles_to_posxy(&tmp, a01, a12, a20))
 242                 return -1;
 243         *err = pt_norm(&tmp, &pos);
 244         if (*err > 50.) /* saturate error to 5cm */
 245                 *err = 50.;
 246         return 0;
 247 }