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 const point_t beacon0 = { 0, 1050 };
  42 const point_t beacon1 = { 3000, 0 };
  43 const point_t beacon2 = { 3000, 2100 };
  44
  45 /* Fill the 2 circles pointer given as parameter, each of those cross
  46  * both beacons b1 and b2. From any point of these circles (except b1
  47  * and b2), we see b1 and b2 with the angle of a_rad (which must be
  48  * positive). Return 0 on success.
  49  *
  50  *                              l
  51  *                <------------------------->
  52  *
  53  *              b1              O            b2
  54  *               +----------------------------+
  55  *             ,' \\            |            /|'\
  56  *            /    \ \          | ^        / |   `
  57  *           /       \ \   a___ | | d    /   |    `.
  58  *          /         \  \ /    | v    /     |     \
  59  *         |            \  \    |    /       |      |
  60  *         |             \   \  |  /        |       |
  61  *         |               \   \|/          |        |
  62  *         |                \   * C         |        |
  63  *         |                  \             |       .'
  64  *         |                   \           |        |
  65  *          |                    \         |       .'
  66  *           \                    \   a____|       /
  67  *            \                     \ /    |     ,'
  68  *             `                     \    |     /
  69  *              '.                     \  |   ,'
  70  *                '-.                   \ |_,'
  71  *                   '-._              _,*'
  72  *                       '`--......---'     R (the robot)
  73  *
  74  */
  75 int8_t angle_to_circles(circle_t *c1, circle_t *c2,
  76                          const point_t *b1, const point_t *b2,
  77                          double a_rad)
  78 {
  79         point_t O;
  80         vect_t v;
  81         float l, d;
  82
  83         /* reject negative or too small angles */
  84         if (a_rad <= 0.01)
  85                 return -1;
  86
  87         /* get position of O */
  88         O.x = (b1->x + b2->x) / 2;
  89         O.y = (b1->y + b2->y) / 2;
  90
  91         /* get the length l */
  92         v.x = b2->x - b1->x;
  93         v.y = b2->y - b1->y;
  94         l = vect_norm(&v);
  95
  96         /* distance from O to the center of the circle */
  97         /* XXX div by 0 when pi */
  98         d = l / (2 * tan(a_rad));
  99
 100         /* get the circle c1 */
 101         vect_rot_trigo(&v);
 102         vect_resize(&v, d);
 103         if (c1) {
 104                 c1->x = O.x + v.x;
 105                 c1->y = O.y + v.y;
 106                 c1->r = norm(b1->x, b1->y, c1->x, c1->y);
 107         }
 108
 109         /* get the circle c2 */
 110         if (c2) {
 111                 c2->x = O.x - v.x;
 112                 c2->y = O.y - v.y;
 113                 c2->r = norm(b1->x, b1->y, c1->x, c1->y);
 114         }
 115
 116         return 0;
 117 }
 118
 119 /* get the position of the robot from the angle of the 3 beacons */
 120 int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20)
 121 {
 122         circle_t c01, c12, c20;
 123         point_t dummy_pt, p1, p2, p3;
 124
 125         dprintf("a01 = %2.2f\n", a01);
 126         dprintf("a12 = %2.2f\n", a12);
 127         dprintf("a20 = %2.2f\n", a20);
 128
 129         if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
 130                 return -1;
 131         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
 132
 133         if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
 134                 return -1;
 135         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
 136
 137         if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
 138                 return -1;
 139         dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
 140
 141         if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
 142                 return -1;
 143         if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
 144                 return -1;
 145         if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
 146                 return -1;
 147
 148         dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
 149         dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
 150         dprintf("p3: x=%2.2f y=%2.2f\n", p3.x, p3.y);
 151
 152         /* if (norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
 153         /*     norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
 154         /*     norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
 155         /*      return -1; */
 156
 157         pos->x = (p1.x + p2.x + p3.x) / 3.0;
 158         pos->y = (p1.y + p2.y + p3.y) / 3.0;
 159
 160         return 0;
 161 }
 162
 163 /* get the angles of beacons from xy pos */
 164 int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
 165                        double *a20, int err_num, float err_val)
 166 {
 167         double a0, a1, a2;
 168
 169         a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
 170         a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
 171         a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
 172
 173         if (err_num == 0 || err_num == 3)
 174                 a0 += (err_val * M_PI/180.);
 175         if (err_num == 1 || err_num == 3)
 176                 a1 += (err_val * M_PI/180.);
 177         if (err_num == 2 || err_num == 3)
 178                 a2 += (err_val * M_PI/180.);
 179
 180         *a01 = a1-a0;
 181         if (*a01 < 0)
 182                 *a01 += M_PI*2;
 183         *a12 = a2-a1;
 184         if (*a12 < 0)
 185                 *a12 += M_PI*2;
 186         *a20 = a0-a2;
 187         if (*a20 < 0)
 188                 *a20 += M_PI*2;
 189
 190         return 0;
 191 }
 192
 193 int8_t process_move_error(double x, double y, double speed,
 194                           double period, double angle, double *err)
 195 {
 196         double a01, a12, a20;
 197         point_t pos, tmp;
 198         double a0, a1, a2;
 199         vect_t u,v;
 200         point_t pos2, pos3;
 201
 202         pos.x = x;
 203         pos.y = y;
 204
 205         /* from start to destination */
 206         v.x = cos(angle) * speed * period;
 207         v.y = sin(angle) * speed * period;
 208
 209         /* first process real pos */
 210         posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
 211
 212         /* vector covered during measure of a0 and a1 */
 213         u.x = v.x * a01 / (2*M_PI);
 214         u.y = v.y * a01 / (2*M_PI);
 215         pos2.x = pos.x + u.x;
 216         pos2.y = pos.y + u.y;
 217
 218         /* vector covered during measure of a1 and a2 */
 219         u.x = v.x * a12 / (2*M_PI);
 220         u.y = v.y * a12 / (2*M_PI);
 221         pos3.x = pos2.x + u.x;
 222         pos3.y = pos2.y + u.y;
 223
 224         dprintf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
 225         dprintf("p1: x=%2.2f y=%2.2f\n", pos2.x, pos2.y);
 226         dprintf("p2: x=%2.2f y=%2.2f\n", pos3.x, pos3.y);
 227
 228         a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
 229         a1 = atan2(beacon1.y-pos2.y, beacon1.x-pos2.x);
 230         a2 = atan2(beacon2.y-pos3.y, beacon2.x-pos3.x);
 231
 232         a01 = a1-a0;
 233         if (a01 < 0)
 234                 a01 += M_PI*2;
 235         a12 = a2-a1;
 236         if (a12 < 0)
 237                 a12 += M_PI*2;
 238         a20 = a0-a2;
 239         if (a20 < 0)
 240                 a20 += M_PI*2;
 241
 242         if (angles_to_posxy(&tmp, a01, a12, a20))
 243                 return -1;
 244         *err = pt_norm(&tmp, &pos);
 245         if (*err > 50.) /* saturate error to 5cm */
 246                 *err = 50.;
 247         return 0;
 248 }
 249
 250 /* whole process is around 3ms on atmega128 at 16Mhz */
 251 int main(int argc, char **argv)
 252 {
 253         double a01, a12, a20;
 254         point_t pos, tmp;
 255         const char *mode = "nothing";
 256
 257 #ifdef HOST_VERSION
 258         if (argc < 2) {
 259                 printf("bad args\n");
 260                 return -1;
 261         }
 262         mode = argv[1];
 263 #else
 264         mode = "angle2pos";
 265         argc = 5;
 266         a01 = 1.65;
 267         a12 = 2.12;
 268         a20 = 2.53;
 269 #endif
 270
 271         if (argc == 5 && strcmp(mode, "angle2pos") == 0) {
 272 #ifdef HOST_VERSION
 273                 dprint = 1;
 274                 a01 = atof(argv[2]);
 275                 a12 = atof(argv[3]);
 276                 a20 = atof(argv[4]);
 277 #endif
 278                 if (angles_to_posxy(&pos, a01, a12, a20) < 0)
 279                         return -1;
 280                 printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
 281                 return 0;
 282         }
 283
 284         if (argc == 4 && strcmp(mode, "simple_error") == 0) {
 285                 int x, y;
 286                 int err_num;
 287                 double err_val_deg;
 288                 double err;
 289
 290                 err_num = atof(argv[2]); /* which beacon sees an error */
 291                 err_val_deg = atof(argv[3]); /* how many degrees of error */
 292
 293                 for (x=0; x<300; x++) {
 294                         for (y=0; y<210; y++) {
 295                                 pos.x = x*10;
 296                                 pos.y = y*10;
 297                                 posxy_to_angles(pos, &a01, &a12, &a20,
 298                                                 err_num, err_val_deg);
 299                                 if (angles_to_posxy(&tmp, a01, a12, a20))
 300                                         continue;
 301                                 err = pt_norm(&tmp, &pos);
 302                                 if (err > 50.) /* saturate error to 5cm */
 303                                         err = 50.;
 304                                 printf("%d %d %2.2f\n", x, y, err);
 305                         }
 306                 }
 307                 return 0;
 308         }
 309
 310         if ((argc == 5 || argc == 7)
 311             && strcmp(argv[1], "move_error") == 0) {
 312                 int x, y;
 313                 double angle, speed, period, err;
 314
 315                 speed = atof(argv[2]); /* speed in m/s ( = mm/ms) */
 316                 period = atof(argv[3]); /* period of turret in ms */
 317                 angle = atof(argv[4]); /* direction of moving */
 318                 if (argc == 7) {
 319                         dprint = 1;
 320                         process_move_error(atof(argv[5]), atof(argv[6]),
 321                                            speed, period, angle, &err);
 322                         printf("%2.2f %2.2f %2.2f\n", atof(argv[5]),
 323                                atof(argv[6]), err);
 324                         return 0;
 325                 }
 326
 327                 for (x=0; x<300; x++) {
 328                         for (y=0; y<210; y++) {
 329                                 pos.x = x*10;
 330                                 pos.y = y*10;
 331                                 if (process_move_error(pos.x, pos.y,
 332                                                        speed, period, angle,
 333                                                        &err) < 0)
 334                                         continue;
 335                                 printf("%d %d %2.2f\n", x, y, err);
 336                         }
 337                 }
 338                 return 0;
 339         }
 340
 341         printf("bad args\n");
 342         return -1;
 343 }