laser-nav.h File Reference

This graph shows which files directly or indirectly include this file:

Classes
struct	pln_point
struct	pln_line
struct	pln_compl
struct	pln_circle
struct	pln_fquad_2var
struct	pln_fquad
struct	pln_circ
struct	pln_pos_state
struct	pln_time_state
struct	pln_aangle_state
struct	pln_angle_state
struct	pln_angle_eval
Defines
#define	DEBUG   1
	D=S2 F C=R3 +-------------+-------------+ | | | | | +H | | | | | R1+ +L=S1 =K| | | +r | | +G | | | | | +-------------+-------------+ A=S3 E B=R2.
#define	FWIDTH   3.0
	Program parameters.
#define	FHEIGHT   2.1
#define	SET_POINT_A(p)   SET_POINT((p), 0.0, 0.0)
#define	SET_POINT_B(p)   SET_POINT((p), FWIDTH, 0.0)
#define	SET_POINT_C(p)   SET_POINT((p), FWIDTH, FHEIGHT)
#define	SET_POINT_D(p)   SET_POINT((p), 0.0, FHEIGHT)
#define	SET_POINT_E(p)   SET_POINT((p), FWIDTH/2, 0)
#define	SET_POINT_F(p)   SET_POINT((p), FWIDTH/2, FHEIGHT)
#define	SET_POINT_G(p)   SET_POINT((p), FWIDTH/2, FHEIGHT/4)
#define	SET_POINT_H(p)   SET_POINT((p), FWIDTH/2, FHEIGHT/4*3)
#define	SET_POINT_K(p)   SET_POINT((p), 0, FHEIGHT/2)
#define	SET_POINT_L(p)   SET_POINT((p), FWIDTH, FHEIGHT/2)
#define	SET_POINT_R1(p)   SET_POINT_K(p)
#define	SET_POINT_R2(p)   SET_POINT_B(p)
#define	SET_POINT_R3(p)   SET_POINT_C(p)
#define	TIME2ANGLE(p, t)   (((double)t)/((double)p)*2*M_PI)
	Other macros.
#define	SET_POINT(p, a, b)   ({(p).x=(a); (p).y=(b);})
Functions
void	pln_get_position (struct pln_angle_state as, struct pln_pos_state *pos)
	FUNCTION PROTOTYPES.
void	pln_set_points ()
	Set values of points we already know for later use.
void	pln_pos2ang (struct pln_pos_state ps, struct pln_aangle_state *aas)
	Convert position to angles.
void	pln_coordinate (struct pln_pos_state estpos, struct pln_aangle_state aas, struct pln_angle_state *as)
	Angle coordinations.
void	pln_sel_angles (struct pln_pos_state estpos, double *angles, int angcount, struct pln_aangle_state *maas)
	Select angles most probable from all measured angles.
void	pln_cal_position (unsigned int *times, unsigned int timecnt, struct pln_pos_state act_pos, struct pln_pos_state *est_pos)
	Calculate position from measured times and from the actual position.
int	cmpd (const void *a, const void *b)
	Compare doubles.
void	test_angles (struct pln_point *positions, int *poscount)
	Test all possible positions.
void	test_coordination ()
	Test pln_coordinate() function.
void	test_sel_angles ()
	Generate random angles and test if the function pln_sel_angles() works correctly.
void	test_cal_position (struct pln_point *positions, int *poscount)
	Test this library.
Variables
struct pln_point A B C D E F G H K L R1 R2	R3

---

Define Documentation

#define DEBUG   1

D=S2 F C=R3 +-------------+-------------+ | | | | | +H | | | | | R1+ +L=S1 =K| | | +r | | +G | | | | | +-------------+-------------+ A=S3 E B=R2.

Fixed points (known points) [x,y] A - [0,0] B - [width,0] C - [width,height] D - [0,height] E - [width/2,0] F - [width/2,height] G - [width/2,height/4] H - [width/2,height/4*3] K - [0,height/2] L - [width,height/2] Our reflectors R1, R2, R3 Opponent`s reflectors S1, S2, S3

Unknown point r - Position of the robot

Angles: theta1 = from robot`s head to the first reflector theta2 = from robot`s head to the second reflector theta3 = from robot`s head to the third reflector alfa = angle R3-r-R2 (S2-r-S3) beta = angle R1-r-R3 (S3-r-S1) gama = angle R1-r-R2 (S1-r-S2)

Angle`s sense - counterclockwise

Terminology:

• absolute angle - angle between head and reflector (theta1,theta2,..)
• relative angle - angle between reflectors (alfa, beta, ...)

#define FHEIGHT   2.1

#define FWIDTH   3.0

Program parameters.

Playground size and known points

#define SET_POINT	(		p,
			a,
			b
	)			({(p).x=(a); (p).y=(b);})

#define SET_POINT_A

(

p

)

SET_POINT((p), 0.0, 0.0)

#define SET_POINT_B

(

p

)

SET_POINT((p), FWIDTH, 0.0)

#define SET_POINT_C

(

p

)

SET_POINT((p), FWIDTH, FHEIGHT)

#define SET_POINT_D

(

p

)

SET_POINT((p), 0.0, FHEIGHT)

#define SET_POINT_E

(

p

)

SET_POINT((p), FWIDTH/2, 0)

#define SET_POINT_F

(

p

)

SET_POINT((p), FWIDTH/2, FHEIGHT)

#define SET_POINT_G

(

p

)

SET_POINT((p), FWIDTH/2, FHEIGHT/4)

#define SET_POINT_H

(

p

)

SET_POINT((p), FWIDTH/2, FHEIGHT/4*3)

#define SET_POINT_K

(

p

)

SET_POINT((p), 0, FHEIGHT/2)

#define SET_POINT_L

(

p

)

SET_POINT((p), FWIDTH, FHEIGHT/2)

#define SET_POINT_R1

(

p

)

SET_POINT_K(p)

#define SET_POINT_R2

(

p

)

SET_POINT_B(p)

#define SET_POINT_R3

(

p

)

SET_POINT_C(p)

#define TIME2ANGLE	(		p,
			t
	)			(((double)t)/((double)p)*2*M_PI)

Other macros.

---

Function Documentation

int cmpd	(	const void *	a,
		const void *	b
	)

Compare doubles.

Used by qsort().

Here is the caller graph for this function:

void pln_cal_position	(	unsigned int *	times,
		unsigned int	timecnt,
		struct pln_pos_state	act_pos,
		struct pln_pos_state *	est_pos
	)

Calculate position from measured times and from the actual position.

Parameters:

	times	measured times. The period is expected on the 1st position.
	timecnt	number of measured times
	act_pos	actual position
	est_pos	calculated position

Here is the call graph for this function:

Here is the caller graph for this function:

void pln_coordinate	(	struct pln_pos_state	estpos,
		struct pln_aangle_state	aas,
		struct pln_angle_state *	as
	)

Angle coordinations.

From the estimated position (x,y,head) and measured times determine correctly angles needed to count the position by measured angles.

Parameters:

	estpos	estimated position
	aas	measured angles (after selection)
	as	angles between reflectors (correctly aligned)

Here is the call graph for this function:

Here is the caller graph for this function:

void pln_get_position	(	struct pln_angle_state	as,
		struct pln_pos_state *	pos
	)

FUNCTION PROTOTYPES.

FUNCTION PROTOTYPES.

Parameters:

	as	angles between reflectors
	pos	enumerated position

Here is the call graph for this function:

Here is the caller graph for this function:

void pln_pos2ang	(	struct pln_pos_state	ps,
		struct pln_aangle_state *	aas
	)

Convert position to angles.

Parameters:

	ps	position to be converted
	aas	3 angles between head and reflectors

Here is the call graph for this function:

Here is the caller graph for this function:

void pln_sel_angles	(	struct pln_pos_state	estpos,
		double *	angles,
		int	angcount,
		struct pln_aangle_state *	maas
	)

Select angles most probable from all measured angles.

Use estimated position to evaluate them.

Parameters:

	estpos	estimated position
	angles	measured angles (times)
	angcount	amount of measured angles
	maas	selected angles

Here is the call graph for this function:

Here is the caller graph for this function:

void pln_set_points

(

)

Set values of points we already know for later use.

Here is the caller graph for this function:

void test_angles	(	struct pln_point *	positions,
		int *	poscount
	)

Test all possible positions.

Convert positions to angles and then do the reverse conversion.

Parameters:

	positions	positions of wanted points
	poscount	count of positions

Here is the call graph for this function:

Here is the caller graph for this function:

void test_cal_position	(	struct pln_point *	positions,
		int *	poscount
	)

Test this library.

Set some actual position, give it measured times and hope that everything will works correctly.

Parameters:

	positions	positions of wanted points
	poscount	count of positions

Here is the call graph for this function:

Here is the caller graph for this function:

void test_coordination

(

)

Test pln_coordinate() function.

For any value of head`s angle it should return the same alfa, beta and gama.

Here is the call graph for this function:

Here is the caller graph for this function:

void test_sel_angles

(

)

Generate random angles and test if the function pln_sel_angles() works correctly.

Here is the call graph for this function:

Here is the caller graph for this function:

---

Variable Documentation

struct pln_point A B C D E F G H K L R1 R2 R3