From 5787cfdb9987e3e5cf650e6ad1586663b191af20 Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 6 Mar 2010 15:18:17 +0100 Subject: [PATCH] static beacon --- .../tests/static_beacon/static_beacon.c | 79 ++++++------- .../microb2010/tests/tourel_beacon/graph.py | 34 ++++-- .../microb2010/tests/tourel_beacon/main.c | 109 ++++++++++++------ 3 files changed, 129 insertions(+), 93 deletions(-) diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c index c34838c..c8e54c0 100755 --- a/projects/microb2010/tests/static_beacon/static_beacon.c +++ b/projects/microb2010/tests/static_beacon/static_beacon.c @@ -79,12 +79,11 @@ #define PHOTO_BIT 0 #define READ_PHOTO() (!!(PHOTO_PIN & (_BV(PHOTO_BIT)))) -/* in cycles/64 (unit is 4 us at 16Mhz) */ -#define MAX_PHOTO_TIME ((uint8_t)25) /* t=100us len=5mm rps=40Hz dist=20cm */ +#define MIN_INTER_TIME (50*16) /* t~=50us dist=350cm */ +#define MAX_INTER_TIME (2000*16) /* t=2ms dist=20cm */ -/* XXX to be recalculated */ -#define MIN_INTER_TIME ((uint8_t)12) /* t=50us len=50mm rps=40Hz dist=350cm */ -#define MAX_INTER_TIME ((uint8_t)250) /* t=1000us len=50mm rps=40Hz dist=20cm */ +/* xmit 2ms after virtual laser: must be < 32768 */ +#define IR_DELAY (2000*16) /* in ms */ #define INTER_LASER_TIME 10 @@ -152,59 +151,43 @@ static void xmit_bits(uint32_t frame, uint8_t nbit) xmit_0(); } -/* */ -static inline int8_t wait_laser(void) +/* Wait 2 consecutive rising edges on photodiode. Return 0 on success, + * in this case, the 'when' pointed area is assigned to the time when + * IR signal should be sent. */ +static inline int8_t wait_laser(uint16_t *when) { - uint8_t photos; - uint8_t time1, time2; - uint8_t diff; - - /* - wait photo on - wait photo off - wait photo on or timeout - wait photo off - should return the amount of time to wait - */ + uint16_t time1, time2; + uint16_t diff; + + /* set timer to 16Mhz, we will use ICP */ + TCCR1A = 0; + TCCR1B = _BV(CS10); /* wait until all is off */ while (READ_PHOTO() != 0); + TIFR = _BV(ICF1); - /* wait until photo is on */ - while (READ_PHOTO() != 1); - time1 = TCNT0; - LED1_ON(); - - /* wait until photo is off, if it takes too long time, - * return. */ - while (READ_PHOTO() != 0) { - diff = TCNT0 - time1; - if (diff > MAX_PHOTO_TIME) - return -1; - } + /* wait rising edge */ + while ((TIFR & _BV(ICF1)) == 0); + time1 = ICR1; + TIFR = _BV(ICF1); - /* wait photo on. */ - while (READ_PHOTO() != 1) { - diff = TCNT0 - time1; + /* wait next rising edge + timeout */ + while ((TIFR & _BV(ICF1)) == 0) { + diff = TCNT1 - time1; if (diff > MAX_INTER_TIME) return -1; } - time2 = TCNT0; - /* wait until photo is off, if it takes too long time, - * return. */ - while (READ_PHOTO() != 0) { - diff = TCNT0 - time2; - if (diff > MAX_PHOTO_TIME) - return -1; - } - - LED2_ON(); + time2 = ICR1; + TIFR = _BV(ICF1); diff = time2 - time1; if (diff < MIN_INTER_TIME) return -1; + *when = time1 + (diff/2) + IR_DELAY; + /* laser ok */ return 0; } @@ -216,7 +199,7 @@ int main(void) /* must be odd */ uint32_t frame = FRAME; int8_t ret; - int16_t c; + uint16_t when = 0; /* LEDS */ LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT); @@ -228,6 +211,7 @@ int main(void) #if 0 while (1) { + int16_t c; c = uart_recv_nowait(0); if (c != -1) printf("%c", (char)(c+1)); @@ -240,7 +224,7 @@ int main(void) #if 0 while (1) { - if (READ_PHOTO() & _BV(PHOTO1_BIT)) + if (READ_PHOTO()) LED1_ON(); else LED1_OFF(); @@ -261,13 +245,16 @@ int main(void) while (1) { #ifdef WAIT_LASER - ret = wait_laser(); + ret = wait_laser(&when); LED1_OFF(); LED2_OFF(); if (ret) continue; + + /* wait before IR xmit */ + while ((int16_t)(when-TCNT1) > 0); #endif #if 1 diff --git a/projects/microb2010/tests/tourel_beacon/graph.py b/projects/microb2010/tests/tourel_beacon/graph.py index f1e175e..bee4540 100644 --- a/projects/microb2010/tests/tourel_beacon/graph.py +++ b/projects/microb2010/tests/tourel_beacon/graph.py @@ -76,7 +76,8 @@ def graph_da(filename, real_x, real_y, real_a): a1,ea1 = get_angle((real_x, real_y), beacons[1]) a0 -= real_a a1 -= real_a - text = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi)) + text = "real_pt = %2.2f, %2.2f, %2.2f\n"%(real_x, real_y, real_a) + text += "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi)) text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi)) d0,ed0 = get_distance((real_x, real_y), beacons[0]) d1,ed1 = get_distance((real_x, real_y), beacons[1]) @@ -108,6 +109,7 @@ def graph_da(filename, real_x, real_y, real_a): ax.plot(x, y, 'g-') result_pt = (-1, -1) + result_x, result_y, result_a = -1, -1, -1 patches = [] for l in s.split("\n"): m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l) @@ -121,7 +123,9 @@ def graph_da(filename, real_x, real_y, real_a): float(m.groups()[2]), float(m.groups()[3])) if (n == 0): patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ] - result_pt = (x, y) + result_x, result_y = (x, y) + result_pt = (x,y) + result_a = a text += l + "\n" pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6)) @@ -135,8 +139,14 @@ def graph_da(filename, real_x, real_y, real_a): (2200., 1800.), (2200., 500.)] l.sort(cmp=lambda p1,p2: (dist(p1,real_pt) math.pi: + error_a -= 2*math.pi + if error_a < -math.pi: + error_a += 2*math.pi + text += "error = %2.2f mm, %2.2f deg"%(error_dist, error_a * 180. / math.pi) matplotlib.pyplot.text(x, y, text, size=8, ha="center", va="center", bbox = dict(boxstyle="round", @@ -257,7 +267,7 @@ def do_random_test(): print "---- random %d"%i x = random.randint(0, 3000) y = random.randint(0, 2100) - a = random.random()*2*math.pi + a = random.random()*2*math.pi - math.pi graph("test%d.png"%i, x, y, a) graph_da("test_da%d.png"%i, x, y, a) @@ -275,7 +285,7 @@ def do_graph_2d(data, filename, title): fig.savefig(filename) def get_data(cmd, sat=0): - data = np.array([[0.]*210]*300) + data = np.array([[50.]*210]*300) oo,ii = popen2.popen2(cmd) ii.close() while True: @@ -313,11 +323,11 @@ def do_graph_2d_simple_error(): do_graph_2d(data, "error_a%d_%s.png"%(i,j), title) def do_graph_2d_ad_error(): - for d in ["0.0", "0.1", "0.5"]: - for a in ["0.0", "0.1", "0.5"]: + for d in ["0.0", "0.1", "0.5", "1.0"]: + for a in ["0.0", "0.1", "0.5", "1.0"]: for i in ["0", "1", "2"]: print "do_graph_2d_ad_error %s %s %s"%(i, d, a) - data = get_data("./main da_error %s %s %s"%(i, d, a)) + data = get_data("./main da_error %s %s -%s"%(i, d, a)) title = 'Erreur de position en mm, pour une erreur\n' title += "d'angle de %s deg et dist de %s %% (algo %s)"%(a, d, i) do_graph_2d(data, "error_da_%s_%s_%s.png"%(i, d, a), title) @@ -345,7 +355,7 @@ def do_graph_2d_move_error(): "En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n" 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period)) -do_random_test() -do_graph_2d_simple_error() -do_graph_2d_move_error() +#do_random_test() +#do_graph_2d_simple_error() +#do_graph_2d_move_error() do_graph_2d_ad_error() diff --git a/projects/microb2010/tests/tourel_beacon/main.c b/projects/microb2010/tests/tourel_beacon/main.c index 97937d4..8c14f41 100644 --- a/projects/microb2010/tests/tourel_beacon/main.c +++ b/projects/microb2010/tests/tourel_beacon/main.c @@ -204,10 +204,18 @@ ad_to_posxy_one(point_t *pos, tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) - atan2(b0->y-p1.y, b0->x-p1.x); + if (tmp_a01_p1 > M_PI) + tmp_a01_p1 -= 2*M_PI; + if (tmp_a01_p1 < -M_PI) + tmp_a01_p1 += 2*M_PI; dprintf("a01-1: %2.2f\n", tmp_a01_p1); tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) - atan2(b0->y-p2.y, b0->x-p2.x); + if (tmp_a01_p2 > M_PI) + tmp_a01_p2 -= 2*M_PI; + if (tmp_a01_p2 < -M_PI) + tmp_a01_p2 += 2*M_PI; dprintf("a01-2: %2.2f\n", tmp_a01_p2); /* in some conditions, we already know the position of the @@ -228,64 +236,41 @@ ad_to_posxy_one(point_t *pos, return -1; } +static int8_t +ad_to_posxy_algo(point_t *pos, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd0, const circle_t *cd1, const circle_t *c01, /* circles to intersect */ + double a01, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) -/* get the position and angle of the robot from the angle of the 2 - * beacons, and the distance of 2 beacons */ -static int8_t ad_to_posxya(point_t *pos, double *a, int algo, - const point_t *b0, const point_t *b1, /* beacon position */ - double a0, double a1, /* seen angle of beacons */ - double d0, double d1 /* distance to beacons */ ) { - circle_t cd0, cd1, c01; - double a01; - - dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n", - algo, a0, a1, d0, d1); - - /* the first 2 circles depends on distance between robot and - * beacons */ - cd0.x = b0->x; - cd0.y = b0->y; - cd0.r = d0; - dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r); - cd1.x = b1->x; - cd1.y = b1->y; - cd1.r = d1; - dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r); - - /* the third circle depends on angle between b0 and b1 */ - a01 = a1-a0; - if (angle_to_circles(&c01, NULL, b0, b1, a01)) - return -1; - dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r); - switch (algo) { /* take the closer beacon first */ case 0: if (d0 < d1) { - if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) return 0; - if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) return 0; return -1; } else { - if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) return 0; - if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) return 0; return -1; } break; case 1: /* b0 only */ - if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) return 0; break; case 2: /* b0 only */ - if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) return 0; break; default: @@ -295,6 +280,59 @@ static int8_t ad_to_posxya(point_t *pos, double *a, int algo, return -1; } +/* get the position and angle of the robot from the angle of the 2 + * beacons, and the distance of 2 beacons */ +static int8_t +ad_to_posxya(point_t *pos, double *a, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + double a0, double a1, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) +{ + circle_t cd0, cd1, c01; + double a01; + + dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n", + algo, a0, a1, d0, d1); + + /* the first 2 circles depends on distance between robot and + * beacons */ + cd0.x = b0->x; + cd0.y = b0->y; + cd0.r = d0; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r); + cd1.x = b1->x; + cd1.y = b1->y; + cd1.r = d1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r); + + /* the third circle depends on angle between b0 and b1 */ + a01 = a1-a0; + if (a01 < -M_PI) + a01 += 2*M_PI; + if (a01 > M_PI) + a01 -= 2*M_PI; + + if (angle_to_circles(&c01, NULL, b0, b1, a01)) + return -1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r); + + /* get the xy position, depending on algo */ + if (ad_to_posxy_algo(pos, algo, b0, b1, &cd0, &cd1, &c01, a01, d0, d1) < 0) + return -1; + + /* now, we have the xy position, we can get angle thanks to + * the angles a0 and a1. Take the far beacon. */ + if (d0 < d1) + *a = atan2(beacon1.y - pos->y, beacon1.x - pos->x) - a1; + else + *a = atan2(beacon0.y - pos->y, beacon0.x - pos->x) - a0; + if (*a < -M_PI) + *a += 2*M_PI; + if (*a > M_PI) + *a -= 2*M_PI; + return 0; +} + /* get the angles of beacons from xy pos */ int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1, double *a2, int err_num, float err_val) @@ -474,6 +512,7 @@ int main(int argc, char **argv) for (x=0; x<300; x++) { for (y=0; y<210; y++) { + pos.x = x*10; pos.y = y*10; posxy_to_angles(pos, &a01, &a12, &a20, -- 2.39.5