laser-nav.c File Reference

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <robomath.h>
#include "laser-nav.h"

Include dependency graph for laser-nav.c:

Functions
void	time2angle (struct pln_time_state ts, struct pln_aangle_state *aas)
	D=S2 F C=R3 +-------------+-------------+ | | | | | +H | | | | | R1+ +L=S1 =K| | | +r | | +G | | | | | +-------------+-------------+ A=S3 E B=R2.
void	aangle2angle (struct pln_aangle_state aas, struct pln_angle_state *as)
	Convert absolute angles to relative angles (to angles between reflectors).
void	pln_get_line (struct pln_point a, struct pln_point b, struct pln_line *line)
	Get line parametrization from 2 points.
void	pln_get_oline (struct pln_line line, struct pln_point c, struct pln_line *oline)
	Get the ortogonal line if we know 1 line and the cross point.
void	pln_solve_quad (struct pln_fquad quad, struct pln_compl *sol1, struct pln_compl *sol2)
	Solve quadratic equation.
void	pln_solve_2quad (struct pln_fquad_2var form1, struct pln_fquad_2var form2, struct pln_point *sol1, struct pln_point *sol2)
	Solve 2 quadratic equations with 2 unknown variables.
void	pln_circ_line_cross (struct pln_circle circle, struct pln_line line, struct pln_point *sol1, struct pln_point *sol2)
	Find cross points of the line and the circle (condisering the line cross center of the circle).
void	pln_get_circle2 (struct pln_circ c, struct pln_circle *circle)
	Get parameters of the circle if we know 1 angle and 3 points.
void	pln_get_circle (struct pln_circ c, struct pln_circle *circle)
	Get parameters of the circle if we know 1 angle and 3 points.
void	pln_circ2form (struct pln_circle circle, struct pln_fquad_2var *form)
	Get parameters of the quadratic equations from the circle parameters.
void	pln_get_position (struct pln_angle_state as, struct pln_pos_state *pos)
	Get position from the measured angles.
void	pln_set_points ()
	Set values of points we already know for later use.
int	cmpang (const void *a, const void *b)
	Compare evaluation of angles.
void	pln_pos2ang (struct pln_pos_state ps, struct pln_aangle_state *aas)
	Convert position to angles.
void	test_angles (struct pln_point *positions, int *poscount)
	Test all possible positions.
void	pln_coordinate (struct pln_pos_state estpos, struct pln_aangle_state aas, struct pln_angle_state *as)
	Angle coordinations.
void	test_coordination ()
	Test pln_coordinate() 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.
void	test_sel_angles ()
	Generate random angles and test if the function pln_sel_angles() works correctly.
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.
void	test_cal_position (struct pln_point *positions, int *poscount)
	Test this library.

---

Function Documentation

void aangle2angle	(	struct pln_aangle_state	aas,
		struct pln_angle_state *	as
	)

Convert absolute angles to relative angles (to angles between reflectors).

Parameters:

	aas	absolute angles
	as	relative angles

Here is the caller graph for this function:

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

Compare evaluation of angles.

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_circ2form	(	struct pln_circle	circle,
		struct pln_fquad_2var *	form
	)

Get parameters of the quadratic equations from the circle parameters.

Parameters:

	circle	parameters of the circle
	form	quadratic formular

Here is the caller graph for this function:

void pln_circ_line_cross	(	struct pln_circle	circle,
		struct pln_line	line,
		struct pln_point *	sol1,
		struct pln_point *	sol2
	)

Find cross points of the line and the circle (condisering the line cross center of the circle).

Parameters:

	circle	parameters of the circle
	line	parameters of the line
	sol1	cross point 1
	sol2	cross point 2

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_circle	(	struct pln_circ	c,
		struct pln_circle *	circle
	)

Get parameters of the circle if we know 1 angle and 3 points.

Parameters:

	c	angle and points that we already know
	circle	output circle

Here is the call graph for this function:

Here is the caller graph for this function:

void pln_get_circle2	(	struct pln_circ	c,
		struct pln_circle *	circle
	)

Get parameters of the circle if we know 1 angle and 3 points.

This function is similar to another one in this program, but it works only with ortogonal sides.

Parameters:

	c	angle and points that we already know
	circle	output circle

Here is the call graph for this function:

void pln_get_line	(	struct pln_point	a,
		struct pln_point	b,
		struct pln_line *	line
	)

Get line parametrization from 2 points.

Parameters:

	a	point A
	b	point B
	line	output line

Here is the caller graph for this function:

void pln_get_oline	(	struct pln_line	line,
		struct pln_point	c,
		struct pln_line *	oline
	)

Get the ortogonal line if we know 1 line and the cross point.

Parameters:

	line	the line that we know
	c	the cross point between 2 lines
	oline	ortogonal output line

Here is the caller graph for this function:

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

Get position from the measured angles.

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 pln_solve_2quad	(	struct pln_fquad_2var	form1,
		struct pln_fquad_2var	form2,
		struct pln_point *	sol1,
		struct pln_point *	sol2
	)

Solve 2 quadratic equations with 2 unknown variables.

Parameters:

	form1	equation 1
	form2	equation 2
	sol1	solution 1
	sol2	solution 2

Here is the call graph for this function:

Here is the caller graph for this function:

void pln_solve_quad	(	struct pln_fquad	quad,
		struct pln_compl *	sol1,
		struct pln_compl *	sol2
	)

Solve quadratic equation.

Parameters:

	quad	parameters of the quadratic equation
	sol1	solution 1
	sol2	solution 2

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:

void time2angle	(	struct pln_time_state	ts,
		struct pln_aangle_state *	aas
	)

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, ...) Convert times to angles

  Parameters:

  	ts	measured times and period
  	aas	absolute angles