2 * Copyright Droids Corporation (2009)
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.
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.
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
18 * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
29 #include <vect_base.h>
33 #define POS_ACCURACY 10.0 /* 1 cm accuracy max */
35 #define printf(args...) do {} while(0)
38 static int dprint = 0;
39 #define dprintf(args...) if (dprint) printf(args)
41 const point_t beacon0 = { 0, 1050 };
42 const point_t beacon1 = { 3000, 0 };
43 const point_t beacon2 = { 3000, 2100 };
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.
51 * <------------------------->
54 * +----------------------------+
57 * / \ \ a___ | | d / | `.
72 * '`--......---' R (the robot)
75 int8_t angle_to_circles(circle_t *c1, circle_t *c2,
76 const point_t *b1, const point_t *b2,
83 /* reject negative or too small angles */
87 /* get position of O */
88 O.x = (b1->x + b2->x) / 2;
89 O.y = (b1->y + b2->y) / 2;
91 /* get the length l */
96 /* distance from O to the center of the circle */
97 /* XXX div by 0 when pi */
98 d = l / (2 * tan(a_rad));
100 /* get the circle c1 */
106 c1->r = norm(b1->x, b1->y, c1->x, c1->y);
109 /* get the circle c2 */
113 c2->r = norm(b1->x, b1->y, c1->x, c1->y);
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)
122 circle_t c01, c12, c20;
123 point_t dummy_pt, p1, p2, p3;
125 dprintf("a01 = %2.2f\n", a01);
126 dprintf("a12 = %2.2f\n", a12);
127 dprintf("a20 = %2.2f\n", a20);
129 if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
131 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
133 if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
135 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
137 if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
139 dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
141 if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
143 if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
145 if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
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);
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) */
157 pos->x = (p1.x + p2.x + p3.x) / 3.0;
158 pos->y = (p1.y + p2.y + p3.y) / 3.0;
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)
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);
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.);
193 int8_t process_move_error(double x, double y, double speed,
194 double period, double angle, double *err)
196 double a01, a12, a20;
205 /* from start to destination */
206 v.x = cos(angle) * speed * period;
207 v.y = sin(angle) * speed * period;
209 /* first process real pos */
210 posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
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;
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;
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);
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);
242 if (angles_to_posxy(&tmp, a01, a12, a20))
244 *err = pt_norm(&tmp, &pos);
245 if (*err > 50.) /* saturate error to 5cm */
250 /* whole process is around 3ms on atmega128 at 16Mhz */
251 int main(int argc, char **argv)
253 double a01, a12, a20;
255 const char *mode = "nothing";
259 printf("bad args\n");
271 if (argc == 5 && strcmp(mode, "angle2pos") == 0) {
278 if (angles_to_posxy(&pos, a01, a12, a20) < 0)
280 printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
284 if (argc == 4 && strcmp(mode, "simple_error") == 0) {
290 err_num = atof(argv[2]); /* which beacon sees an error */
291 err_val_deg = atof(argv[3]); /* how many degrees of error */
293 for (x=0; x<300; x++) {
294 for (y=0; y<210; y++) {
297 posxy_to_angles(pos, &a01, &a12, &a20,
298 err_num, err_val_deg);
299 if (angles_to_posxy(&tmp, a01, a12, a20))
301 err = pt_norm(&tmp, &pos);
302 if (err > 50.) /* saturate error to 5cm */
304 printf("%d %d %2.2f\n", x, y, err);
310 if ((argc == 5 || argc == 7)
311 && strcmp(argv[1], "move_error") == 0) {
313 double angle, speed, period, err;
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 */
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]),
327 for (x=0; x<300; x++) {
328 for (y=0; y<210; y++) {
331 if (process_move_error(pos.x, pos.y,
332 speed, period, angle,
335 printf("%d %d %2.2f\n", x, y, err);
341 printf("bad args\n");