#ifdef HOST_VERSION
+#ifdef CONFIG_MODULE_HOSTSIM
#include <hostsim.h>
/* we must use 'flags' to avoid a warning */
#define IRQ_UNLOCK(flags) do { flags=0; /* hostsim_lock(); */ } while(0)
#define IRQ_LOCK(flags) do { flags=0; /* hostsim_unlock(); */ } while(0)
#define GLOBAL_IRQ_ARE_MASKED() hostsim_islocked()
+#else
+#define IRQ_UNLOCK(flags) do { flags=0; } while(0)
+#define IRQ_LOCK(flags) do { flags=0; } while(0)
+#define GLOBAL_IRQ_ARE_MASKED() (0)
+#endif /* CONFIG_MODULE_HOSTSIM */
#else
#include <stdint.h>
#include <math.h>
#include <vect_base.h>
-
+
/* Return scalar product */
-float
+float
vect_pscal(vect_t *v, vect_t *w)
{
return v->x * w->x + v->y * w->y;
}
/* Return Z of vectorial product */
-float
+float
vect_pvect(vect_t *v, vect_t *w)
{
return v->x*w->y - v->y*w->x;
}
/* Return scalar product */
-int8_t
+int8_t
vect_pscal_sign(vect_t *v, vect_t *w)
{
float z;
}
/* Return Z of vectorial product */
-int8_t
+int8_t
vect_pvect_sign(vect_t *v, vect_t *w)
{
float z;
return sqrt(x*x + y*y);
}
-float pt_norm(point_t *p1, point_t *p2)
+float pt_norm(const point_t *p1, const point_t *p2)
{
float x = p2->x - p1->x;
float y = p2->y - p1->y;
}
/* norm of a vector */
-float
-vect_norm(vect_t *v)
+float vect_norm(const vect_t *v)
{
return sqrt(v->x*v->x+v->y*v->y);
}
void vect_rot_trigo(vect_t *v)
{
float s;
-
+
s = v->x;
v->x= -v->y;
v->y = s;
-}
+}
void vect_rot_retro(vect_t *v)
{
float s;
-
+
s = v->x;
v->x= v->y;
v->y = -s;
-}
+}
float vect_get_angle(vect_t *v, vect_t *w)
{
float ps;
float n;
-
+
ps = vect_pscal(v, w);
n = vect_norm(v) * vect_norm(w);
-
+
return acos((float)ps/n);
}
} point_t;
/* Return scalar product */
-float
-vect_pscal(vect_t *v, vect_t *w);
+float vect_pscal(vect_t *v, vect_t *w);
/* Return Z of vectorial product */
-float
-vect_pvect(vect_t *v, vect_t *w);
+float vect_pvect(vect_t *v, vect_t *w);
/* Return scalar product */
-int8_t
-vect_pscal_sign(vect_t *v, vect_t *w);
+int8_t vect_pscal_sign(vect_t *v, vect_t *w);
/* Return Z of vectorial product */
-int8_t
-vect_pvect_sign(vect_t *v, vect_t *w);
+int8_t vect_pvect_sign(vect_t *v, vect_t *w);
/* norm of a vector */
float xy_norm(float x1, float y1, float x2, float y2);
-float pt_norm(point_t *p1, point_t *p2);
-float vect_norm(vect_t *v);
+float pt_norm(const point_t *p1, const point_t *p2);
+float vect_norm(const vect_t *v);
void vect_rot_trigo(vect_t *v);
void vect_rot_retro(vect_t *v);
float vect_get_angle(vect_t *v, vect_t *w);
y = OFFSET_CORN_Y + STEP_CORN_Y/2
j = 0
while y < 2100:
-
+
if waypoints[i][j] == TYPE_WHITE_CORN:
wcorn.append(Circle((x, y), 25))
if waypoints[i][j] == TYPE_BLACK_CORN:
#
# Generation options
#
-# CONFIG_OPTM_0 is not set
+CONFIG_OPTM_0=y
# CONFIG_OPTM_1 is not set
# CONFIG_OPTM_2 is not set
# CONFIG_OPTM_3 is not set
-CONFIG_OPTM_S=y
+# CONFIG_OPTM_S is not set
CONFIG_MATH_LIB=y
# CONFIG_FDEVOPEN_COMPAT is not set
# CONFIG_NO_PRINTF is not set
# CONFIG_MODULE_FIXED_POINT is not set
# CONFIG_MODULE_VECT2 is not set
CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_HOSTSIM is not set
# CONFIG_MODULE_SCHEDULER is not set
# CONFIG_MODULE_SCHEDULER_STATS is not set
# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
#
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
#
# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
# CONFIG_MODULE_POSITION_MANAGER is not set
# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
#
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
#
#
# CONFIG_MINIMAL_PRINTF is not set
CONFIG_STANDARD_PRINTF=y
# CONFIG_ADVANCED_PRINTF is not set
-CONFIG_FORMAT_IHEX=y
+# CONFIG_FORMAT_IHEX is not set
# CONFIG_FORMAT_SREC is not set
-# CONFIG_FORMAT_BINARY is not set
+CONFIG_FORMAT_BINARY=y
#
# Base modules
#
+
+#
+# Enable math library in generation options to see all modules
+#
# CONFIG_MODULE_CIRBUF is not set
# CONFIG_MODULE_CIRBUF_LARGE is not set
# CONFIG_MODULE_FIXED_POINT is not set
# CONFIG_MODULE_VECT2 is not set
CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_HOSTSIM is not set
# CONFIG_MODULE_SCHEDULER is not set
# CONFIG_MODULE_SCHEDULER_STATS is not set
# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
#
# Communication modules
#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
# CONFIG_MODULE_UART is not set
# CONFIG_MODULE_UART_9BITS is not set
# CONFIG_MODULE_UART_CREATE_CONFIG is not set
# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
#
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
#
# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
# CONFIG_MODULE_POSITION_MANAGER is not set
# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
# Control system modules
#
# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+
+#
+# Filters
+#
# CONFIG_MODULE_PID is not set
# CONFIG_MODULE_PID_CREATE_CONFIG is not set
# CONFIG_MODULE_RAMP is not set
#
# Radio devices
#
+
+#
+# Some radio devices require SPI to be activated
+#
# CONFIG_MODULE_CC2420 is not set
# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
#
# Crypto modules
#
+
+#
+# Crypto modules depend on utils module
+#
# CONFIG_MODULE_AES is not set
# CONFIG_MODULE_AES_CTR is not set
# CONFIG_MODULE_MD5 is not set
#
# Encodings modules
#
+
+#
+# Encoding modules depend on utils module
+#
# CONFIG_MODULE_BASE64 is not set
# CONFIG_MODULE_HAMMING is not set
#
# Debug modules
#
+
+#
+# Debug modules depend on utils module
+#
# CONFIG_MODULE_DIAGNOSTIC is not set
# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
CONFIG_MODULE_ERROR=y
TARGET = main
-# repertoire des modules
-AVERSIVE_DIR =../..# VALUE, absolute or relative path : example ../.. #
+AVERSIVE_DIR = ../../../..
# List C source files here. (C dependencies are automatically generated.)
SRC = $(TARGET).c
Path = mpath.Path
FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
INT = "([-+]?[0-9][0-9]*)"
-RANDOM_ERROR = 0.3 # deg
+RANDOM_ERROR_A = 0.5 # deg
+RANDOM_ERROR_D = 0.5 # percent
beacons = [ (0.0, 1050.0), (3000.0, 0.0), (3000.0, 2100.0) ]
def build_poly(ptlist):
def get_angle(ref, b):
"""get angle from robot point of view (ref) of beacon 'b'"""
a = math.atan2(b[1]-ref[1], b[0]-ref[0])
- ea = (math.pi/180.) * RANDOM_ERROR * random.random()
+ ea = (math.pi/180.) * RANDOM_ERROR_A * random.random()
ea = random.choice([ea, -ea])
return a + ea, ea
- alpha = math.atan2(a[1]-ref[1], a[0]-ref[0])
- beta = math.atan2(b[1]-ref[1], b[0]-ref[0])
- gamma = beta-alpha
- if gamma < 0:
- gamma = gamma + 2*math.pi
- return gamma + error, error
+def get_distance(ref, b):
+ """get distance between robot (ref) and beacon 'b'"""
+ d = math.sqrt((b[1]-ref[1])**2 + (b[0]-ref[0])**2)
+ ed = RANDOM_ERROR_D * random.random()
+ ed = random.choice([ed, -ed])
+ return d + (d * ed / 100.), ed
def dist(p1, p2):
return math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2)
-def graph(filename, real_x, real_y):
+# graph position from distance + angle
+def graph_da(filename, real_x, real_y, real_a):
+ pcol = []
+ print "real_pt = %s"%(str((real_x, real_y, real_a)))
real_pt = (real_x, real_y)
# display beacons
patches = []
for b in beacons:
patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
+ pcol.append(PatchCollection(patches, facecolor="yellow", alpha = 1))
- patches += [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
+ patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
+ pcol.append(PatchCollection(patches, facecolor="red", alpha = 1))
+
+ # process angles from robot position
+ a0,ea0 = get_angle((real_x, real_y), beacons[0])
+ 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 += "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 += "d0 = %2.2f (%+2.2f %%)\n"%(d0, ed0)
+ text += "d1 = %2.2f (%+2.2f %%)\n"%(d1, ed1)
+
+ cmd = "./main ad2pos %f %f %f %f"%(a0, a1, d0, d1)
+ print cmd
+ o,i = popen2.popen2(cmd)
+ i.close()
+ s = o.read(1000000)
+ o.close()
+
+ open(filename + ".txt", "w").write(s)
+
+ if len(s) == 1000000:
+ gloupix()
+
+ fig = plt.figure()
+ ax = fig.add_subplot(111)
+ title = "Erreur de position en mm, qd l'erreur "
+ title += "d'angle aleatoire est comprise\n"
+ title += "erreur -%1.1f et %1.1f deg "%(RANDOM_ERROR_A, RANDOM_ERROR_A)
+ title += "et de distance de +/- %2.2f %%"%(RANDOM_ERROR_D)
+ ax.set_title(title)
+
+ # area
+ x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
+ ax.plot(x, y, 'g-')
+
+ result_pt = (-1, -1)
+ patches = []
+ for l in s.split("\n"):
+ m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
+ if m:
+ x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
+ print x,y,r
+ patches += [ Circle((x, y), r, facecolor="none") ]
+ m = re.match("p%s: x=%s y=%s a=%s"%(INT, FLOAT, FLOAT, FLOAT), l)
+ if m:
+ n,x,y,a = (float(m.groups()[0]), float(m.groups()[1]),
+ 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)
+ text += l + "\n"
+
+ pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
+ pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2))
+
+ patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ]
+ pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1))
+
+ # text area, far from the point
+ l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
+ (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))
+ matplotlib.pyplot.text(x, y, text, size=8,
+ ha="center", va="center",
+ bbox = dict(boxstyle="round",
+ ec=(1., 0.5, 0.5),
+ fc=(1., 0.8, 0.8),
+ alpha=0.6,
+ ),
+ alpha=0.8
+ )
+ for p in pcol:
+ ax.add_collection(p)
+
+ ax.grid()
+ ax.set_xlim(-100, 3100)
+ ax.set_ylim(-100, 2200)
+ #ax.set_title('spline paths')
+ #plt.show()
+ fig.savefig(filename)
+
+# graph position from angles
+def graph(filename, real_x, real_y, real_a):
+ pcol = []
+ print "real_pt = %s"%(str((real_x, real_y, real_a)))
+ real_pt = (real_x, real_y)
+
+ # display beacons
+ patches = []
+ for b in beacons:
+ patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
+ pcol.append(PatchCollection(patches, facecolor="yellow", alpha = 1))
+
+ patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
+ pcol.append(PatchCollection(patches, facecolor="red", alpha = 1))
# process angles from robot position
a0,ea0 = get_angle((real_x, real_y), beacons[0])
a1,ea1 = get_angle((real_x, real_y), beacons[1])
a2,ea2 = get_angle((real_x, real_y), beacons[2])
+ a0 -= real_a
+ a1 -= real_a
+ a2 -= 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))
text += "a2 = %2.2f (%+2.2f deg)\n"%(a2, ea2*(180./math.pi))
- a01 = a1-a0
- if a01 < 0:
- a01 += 2*math.pi
- a12 = a2-a1
- if a12 < 0:
- a12 += 2*math.pi
- a20 = a0-a2
- if a20 < 0:
- a20 += 2*math.pi
-
- cmd = "./main angle2pos %f %f %f"%(a01, a12, a20)
+ cmd = "./main angle2pos %f %f %f"%(a0, a1, a2)
+ print cmd
o,i = popen2.popen2(cmd)
i.close()
s = o.read(1000000)
fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_title("Erreur de position en mm lorsqu'on ajoute une erreur de mesure\n"
- "d'angle aleatoire comprise entre - %1.1f et %1.1f deg"%(RANDOM_ERROR,
- RANDOM_ERROR))
+ "d'angle aleatoire comprise entre - %1.1f et %1.1f deg"%(RANDOM_ERROR_A,
+ RANDOM_ERROR_A))
# area
x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
ax.plot(x, y, 'g-')
+ result_pt = (-1, -1)
+ patches = []
for l in s.split("\n"):
m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
if m:
if m:
n,x,y = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
if (n == 0):
- patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
result_pt = (x, y)
text += l + "\n"
- p = PatchCollection(patches, cmap=matplotlib.cm.jet, match_original=True)
+ pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
+ pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2))
+
+ patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ]
+ pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1))
# text area, far from the point
l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
),
alpha=0.8
)
- ax.add_collection(p)
+ for p in pcol:
+ ax.add_collection(p)
ax.grid()
ax.set_xlim(-100, 3100)
def do_random_test():
random.seed(0)
- for i in range(100):
+ for i in range(21):
+ print "---- random %d"%i
x = random.randint(0, 3000)
y = random.randint(0, 2100)
- graph("test%d.png"%i, x, y)
+ a = random.random()*2*math.pi
+ graph("test%d.png"%i, x, y, a)
+ graph_da("test_da%d.png"%i, x, y, a)
def do_graph_2d(data, filename, title):
# Make plot with vertical (default) colorbar
# Add colorbar, make sure to specify tick locations to match desired ticklabels
cbar = fig.colorbar(cax, ticks=[0, 50])
- cbar.ax.set_yticklabels(['0', '50 et plus'])# vertically oriented colorbar
+ cbar.ax.set_yticklabels(['0', '50 and more'])# vertically oriented colorbar
fig.savefig(filename)
def get_data(cmd, sat=0):
def do_graph_2d_simple_error():
for i in range(4):
for j in ["0.1", "0.5", "1.0"]:
+ print "do_graph_2d_simple_error %s %s"%(i, j)
data = get_data("./main simple_error %d %s"%(i,j))
if i != 3:
title = 'Erreur de position en mm, pour une erreur\n'
title += 'de mesure de %s deg sur les 3 balises'%(j)
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 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))
+ 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)
+
def do_graph_2d_move_error():
i = 0
for period in [ 20, 40 ]:
for speed in [ 0.3, 0.7, 1. ]:
angle_deg = 0
+ print "do_graph_2d_move_error %s %s"%(period, speed)
while angle_deg < 360:
angle_rad = angle_deg * (math.pi/180.)
data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad))
"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_random_test()
+do_graph_2d_simple_error()
do_graph_2d_move_error()
+do_graph_2d_ad_error()
static int dprint = 0;
#define dprintf(args...) if (dprint) printf(args)
+#define AREA_X 3000
+#define AREA_Y 2100
+
+#define ANGLE_EPSILON 0.005
+
const point_t beacon0 = { 0, 1050 };
const point_t beacon1 = { 3000, 0 };
const point_t beacon2 = { 3000, 2100 };
+static inline double abs_dbl(double x)
+{
+ if (x > 0)
+ return x;
+ else
+ return -x;
+}
+
/* Fill the 2 circles pointer given as parameter, each of those cross
* both beacons b1 and b2. From any point of these circles (except b1
* and b2), we see b1 and b2 with the angle of a_rad (which must be
vect_t v;
float l, d;
- /* reject negative or too small angles */
- if (a_rad <= 0.01)
+ /* reject too small angles */
+ if (a_rad <= 0.01 && a_rad >= -0.01)
return -1;
/* get position of O */
if (c1) {
c1->x = O.x + v.x;
c1->y = O.y + v.y;
- c1->r = norm(b1->x, b1->y, c1->x, c1->y);
+ c1->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
}
/* get the circle c2 */
if (c2) {
c2->x = O.x - v.x;
c2->y = O.y - v.y;
- c2->r = norm(b1->x, b1->y, c1->x, c1->y);
+ c2->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
}
return 0;
dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
dprintf("p3: x=%2.2f y=%2.2f\n", p3.x, p3.y);
- /* if (norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
- /* norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
- /* norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
+ /* if (xy_norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
+ /* xy_norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
+ /* xy_norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
/* return -1; */
pos->x = (p1.x + p2.x + p3.x) / 3.0;
return 0;
}
-/* get the angles of beacons from xy pos */
-int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
- double *a20, int err_num, float err_val)
+static inline int8_t is_pt_in_area(point_t pt)
{
- double a0, a1, a2;
+ if (pt.x < 0 || pt.y < 0)
+ return 0;
+ if (pt.x > AREA_X || pt.y > AREA_Y)
+ return 0;
+ return 1;
+}
- a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
- a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
- a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
+/* intersect the circle formed by one distance info with the circle
+ * crossing the 2 beacons */
+static int8_t
+ad_to_posxy_one(point_t *pos,
+ const point_t *b0, const point_t *b1, /* beacon position */
+ const circle_t *cd, const circle_t *c01, /* circles to intersect */
+ double a01) /* seen angle of beacons */
+{
+ double tmp_a01_p1, tmp_a01_p2;
+ point_t p1, p2;
+ uint8_t p1_ok=0, p2_ok=0;
+
+ if (circle_intersect(c01, cd, &p1, &p2) == 0)
+ return -1;
+
+ dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
+ dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
+
+ dprintf("real a01: %2.2f\n", a01);
+
+ tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) -
+ atan2(b0->y-p1.y, b0->x-p1.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);
+ dprintf("a01-2: %2.2f\n", tmp_a01_p2);
+
+ /* in some conditions, we already know the position of the
+ * robot at this step */
+ p1_ok = is_pt_in_area(p1) &&
+ abs_dbl(tmp_a01_p1 - a01) < ANGLE_EPSILON;
+
+ p2_ok = is_pt_in_area(p2) &&
+ abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON;
+ if (!p1_ok && p2_ok) {
+ *pos = p2;
+ return 0;
+ }
+ if (p1_ok && !p2_ok) {
+ *pos = p1;
+ return 0;
+ }
+ 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 (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)
+ return 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)
+ return 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)
+ return 0;
+ break;
+ case 2:
+ /* b0 only */
+ if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
+ return 0;
+ break;
+ default:
+ break;
+ }
+ /* not found */
+ return -1;
+}
+
+/* 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)
+{
+ *a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
+ *a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
+ *a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
if (err_num == 0 || err_num == 3)
- a0 += (err_val * M_PI/180.);
+ *a0 += (err_val * M_PI/180.);
if (err_num == 1 || err_num == 3)
- a1 += (err_val * M_PI/180.);
+ *a1 += (err_val * M_PI/180.);
if (err_num == 2 || err_num == 3)
- a2 += (err_val * M_PI/180.);
+ *a2 += (err_val * M_PI/180.);
+
+ return 0;
+}
+
+/* get the angles of beacons from xy pos */
+int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
+ double *a20, int err_num, float err_val)
+{
+ double a0, a1, a2;
+ posxy_to_abs_angles(pos, &a0, &a1, &a2, err_num, err_val);
*a01 = a1-a0;
if (*a01 < 0)
/* whole process is around 3ms on atmega128 at 16Mhz */
int main(int argc, char **argv)
{
+ double a0, a1, a2;
double a01, a12, a20;
point_t pos, tmp;
const char *mode = "nothing";
#else
mode = "angle2pos";
argc = 5;
- a01 = 1.65;
- a12 = 2.12;
- a20 = 2.53;
+ a0 = -1.;
+ a1 = 1.65;
+ a2 = 3.03;
#endif
+ /*
+ * angle2pos a0 a1 a2:
+ * (angles in radian, -pi < a < pi)
+ */
if (argc == 5 && strcmp(mode, "angle2pos") == 0) {
#ifdef HOST_VERSION
dprint = 1;
- a01 = atof(argv[2]);
- a12 = atof(argv[3]);
- a20 = atof(argv[4]);
+ a0 = atof(argv[2]);
+ a1 = atof(argv[3]);
+ a2 = atof(argv[4]);
#endif
+
+ /* */
+ a01 = a1-a0;
+ if (a01 < 0)
+ a01 += 2*M_PI;
+ a12 = a2-a1;
+ if (a12 < 0)
+ a12 += 2*M_PI;
+ a20 = a0-a2;
+ if (a20 < 0)
+ a20 += 2*M_PI;
+
if (angles_to_posxy(&pos, a01, a12, a20) < 0)
return -1;
printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
return 0;
}
+ /*
+ * ad2pos a0 a1 d0 d1:
+ * (angles in radian, -pi < a < pi)
+ * (distances in mm)
+ */
+ if (argc == 6 && strcmp(mode, "ad2pos") == 0) {
+ double a, d0, d1;
+
+ dprint = 1;
+ a0 = atof(argv[2]);
+ a1 = atof(argv[3]);
+ d0 = atof(argv[4]);
+ d1 = atof(argv[5]);
+
+ /* */
+ if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1,
+ a0, a1, d0, d1) < 0)
+ return -1;
+ printf("p0: x=%2.2f y=%2.2f a=%2.2f\n", pos.x, pos.y, a);
+ return 0;
+ }
+
if (argc == 4 && strcmp(mode, "simple_error") == 0) {
int x, y;
int err_num;
return 0;
}
+ /* da_error algo errpercent errdeg */
+ if (argc == 5 && strcmp(mode, "da_error") == 0) {
+ int x, y, algo;
+ double err_val_deg;
+ double err_val_percent;
+ double err, d0, d1, a;
+
+ algo = atoi(argv[2]);
+ err_val_percent = atof(argv[3]); /* how many % of error for dist */
+ err_val_deg = atof(argv[4]); /* how many degrees of error */
+
+ for (x=0; x<300; x++) {
+ for (y=0; y<210; y++) {
+
+ pos.x = x*10;
+ pos.y = y*10;
+ posxy_to_abs_angles(pos, &a0, &a1, &a2,
+ 0, err_val_deg);
+ d0 = pt_norm(&pos, &beacon0);
+ d0 += d0 * err_val_percent / 100.;
+ d1 = pt_norm(&pos, &beacon1);
+ d1 += d1 * err_val_percent / 100.;
+
+ if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1,
+ a0, a1, d0, d1) < 0)
+ continue;
+
+ err = pt_norm(&tmp, &pos);
+ if (err > 50.) /* saturate error to 5cm */
+ err = 50.;
+ printf("%d %d %2.2f\n", x, y, err);
+ }
+ }
+ return 0;
+ }
+
if ((argc == 5 || argc == 7)
&& strcmp(argv[1], "move_error") == 0) {
int x, y;
git.droids-corp.org / aversive.git / commitdiff