static beacon

diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c
index c34838c..c8e54c0 100755 (executable)
--- 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))))
 
 #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
 
 /* in ms */
 #define INTER_LASER_TIME 10


@@ -152,59 +151,43 @@ static void xmit_bits(uint32_t frame, uint8_t nbit)
        xmit_0();
 }
 
        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);
 
        /* 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;
        }
                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;
 
 
        diff = time2 - time1;
        if (diff < MIN_INTER_TIME)
                return -1;
 

+       *when = time1 + (diff/2) + IR_DELAY;
+

        /* laser ok */
        return 0;
 }
        /* laser ok */
        return 0;
 }


@@ -216,7 +199,7 @@ int main(void)
        /* must be odd */
        uint32_t frame = FRAME;
        int8_t ret;
        /* 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);
 
        /* LEDS */
        LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT);


@@ -228,6 +211,7 @@ int main(void)
 
 #if 0
        while (1) {
 
 #if 0
        while (1) {

+               int16_t c;

                c = uart_recv_nowait(0);
                if (c != -1)
                        printf("%c", (char)(c+1));
                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 0
        while (1) {

-               if (READ_PHOTO() & _BV(PHOTO1_BIT))
+               if (READ_PHOTO())

                        LED1_ON();
                else
                        LED1_OFF();
                        LED1_ON();
                else
                        LED1_OFF();


@@ -261,13 +245,16 @@ int main(void)
        while (1) {
 
 #ifdef WAIT_LASER
        while (1) {
 
 #ifdef WAIT_LASER

-               ret = wait_laser();
+               ret = wait_laser(&when);

 
                LED1_OFF();
                LED2_OFF();
 
                if (ret)
                        continue;
 
                LED1_OFF();
                LED2_OFF();
 
                if (ret)
                        continue;

+
+               /* wait before IR xmit */
+               while ((int16_t)(when-TCNT1) > 0);

 #endif
 
 #if 1
 #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 (file)
--- 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
     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])
     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)
     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)
     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") ]
                         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))
             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)<dist(p2,real_pt)) and 1 or -1)
     x,y = l[0]
          (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",
     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)
         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)
 
         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):
     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:
     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():
             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)
             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)
                 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))
 
                 "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()
 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 (file)
--- 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);
 
        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);
        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
        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;
 }
 
        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) {
        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;
                                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 {
                                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;
                                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 */
                                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 */
                        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 0;
                break;
        default:


@@ -295,6 +280,59 @@ static int8_t ad_to_posxya(point_t *pos, double *a, int algo,
        return -1;
 }
 
        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)
 /* 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++) {
 
                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,
                                pos.x = x*10;
                                pos.y = y*10;
                                posxy_to_angles(pos, &a01, &a12, &a20,