#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
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;
}
/* 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);
#if 0
while (1) {
+ int16_t c;
c = uart_recv_nowait(0);
if (c != -1)
printf("%c", (char)(c+1));
#if 0
while (1) {
- if (READ_PHOTO() & _BV(PHOTO1_BIT))
+ if (READ_PHOTO())
LED1_ON();
else
LED1_OFF();
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
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])
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)
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))
(2200., 1800.), (2200., 500.)]
l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
x,y = l[0]
- text += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
- text += "error = %2.2f mm"%(dist(real_pt, result_pt))
+ text += "result_pt: x=%2.2f, y=%2.2f, a=%2.2f\n"%(result_x, result_y, result_a)
+ error_dist = dist(real_pt, result_pt)
+ error_a = result_a - real_a
+ if error_a > 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",
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)
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:
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)
"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()
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
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:
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)
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,
git.droids-corp.org / aversive.git / commitdiff