aversive_10-03-12/modules/devices/robot/obstacle_avoidance/obstacle_avoidance.c File Reference

#include <aversive.h>
#include <aversive/error.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <obstacle_avoidance.h>

Go to the source code of this file.

Defines
#define	debug_printf(args...)
#define	MAX_COEF   5000
#define	IS_IN_PLAYGROUND(pt)
Functions
void	oa_init (void)
void	oa_start_end_points (int32_t st_x, int32_t st_y, int32_t en_x, int32_t en_y)
oa_poly_t *	oa_new_poly (uint8_t size)
void	oa_poly_set_point (oa_poly_t *pol, int32_t x, int32_t y, uint8_t i)
oa_point_t *	oa_get_path (void)
void	pts2line (const oa_ext_point_t *p1, const oa_ext_point_t *p2, oa_line_t *l)
uint8_t	intersect_line (const oa_line_t *l1, const oa_line_t *l2, oa_ext_point_t *p)
uint8_t	intersect_segment (const oa_ext_point_t *s1, const oa_ext_point_t *s2, const oa_ext_point_t *t1, const oa_ext_point_t *t2, oa_ext_point_t *p)
uint8_t	is_point_in_poly (oa_poly_t *pol, int16_t x, int16_t y)
uint8_t	is_crossing_poly (oa_ext_point_t p1, oa_ext_point_t p2, oa_poly_t *pol)
uint8_t	calc_rays (oa_poly_t *polys, uint8_t npolys, uint8_t *rays)
void	calc_rays_weight (oa_poly_t *polys, uint8_t npolys, uint8_t *rays, uint8_t ray_n, uint16_t *weight)
void	dijkstra (uint8_t start_p, uint8_t start)
int8_t	get_path (oa_poly_t *polys, uint8_t *rays)
int8_t	oa_process (void)

---

Define Documentation

#define debug_printf

(

args...

)

Definition at line 34 of file obstacle_avoidance.c.

#define IS_IN_PLAYGROUND

(

pt

)

Value:

( (pt).x >= PLAYGROUND_X_MIN && \
                (pt).x<=PLAYGROUND_X_MAX && \
                (pt).y >= PLAYGROUND_Y_MIN && (pt).y<=PLAYGROUND_Y_MAX )

Definition at line 443 of file obstacle_avoidance.c.

Referenced by calc_rays().

#define MAX_COEF   5000

Definition at line 170 of file obstacle_avoidance.c.

Referenced by pts2line().

---

Function Documentation

uint8_t calc_rays	(	oa_poly_t *	polys,
		uint8_t	npolys,
		uint8_t *	rays
	)

Definition at line 465 of file obstacle_avoidance.c.

References debug_printf, is_crossing_poly(), IS_IN_PLAYGROUND, _poly::l, parse_symbols::l, and obstacle_avoidance::ray_n.

Referenced by oa_process().

void calc_rays_weight	(	oa_poly_t *	polys,
		uint8_t	npolys,
		uint8_t *	rays,
		uint8_t	ray_n,
		uint16_t *	weight
	)

Definition at line 565 of file obstacle_avoidance.c.

Referenced by oa_process().

void dijkstra	(	uint8_t	start_p,
		uint8_t	start
	)

Definition at line 593 of file obstacle_avoidance.c.

References _poly::l, MAX_POLY, _ext_point::p, obstacle_avoidance::polys, _ext_point::pt, _poly::pts, obstacle_avoidance::ray_n, obstacle_avoidance::rays, obstacle_avoidance::u, _ext_point::valid, _ext_point::weight, and obstacle_avoidance::weight.

Referenced by oa_process().

int8_t get_path	(	oa_poly_t *	polys,
		uint8_t *	rays
	)

Definition at line 654 of file obstacle_avoidance.c.

References DEBUG, E_OA, MAX_CHKPOINTS, _ext_point::p, _ext_point::pt, _poly::pts, obstacle_avoidance::res, obstacle_avoidance::u, _ext_point::x, _point::x, _ext_point::y, and _point::y.

Referenced by oa_process().

uint8_t intersect_line	(	const oa_line_t *	l1,
		const oa_line_t *	l2,
		oa_ext_point_t *	p
	)

Definition at line 208 of file obstacle_avoidance.c.

References _line::a, _line::b, _line::c, debug_printf, _ext_point::x, and _ext_point::y.

Referenced by intersect_segment().

uint8_t intersect_segment	(	const oa_ext_point_t *	s1,
		const oa_ext_point_t *	s2,
		const oa_ext_point_t *	t1,
		const oa_ext_point_t *	t2,
		oa_ext_point_t *	p
	)

Definition at line 269 of file obstacle_avoidance.c.

References debug_printf, intersect_line(), pts2line(), _ext_point::x, and _ext_point::y.

Referenced by is_crossing_poly().

uint8_t is_crossing_poly	(	oa_ext_point_t	p1,
		oa_ext_point_t	p2,
		oa_poly_t *	pol
	)

Definition at line 377 of file obstacle_avoidance.c.

References debug_printf, intersect_segment(), _poly::l, _poly::pts, _ext_point::x, and _ext_point::y.

Referenced by calc_rays().

uint8_t is_point_in_poly	(	oa_poly_t *	pol,
		int16_t	x,
		int16_t	y
	)

return

Definition at line 361 of file obstacle_avoidance.c.

References _ext_point::x, and _ext_point::y.

oa_point_t* oa_get_path

(

void

)

get the result after a call to oa_process()

Definition at line 112 of file obstacle_avoidance.c.

References obstacle_avoidance::res, and obstacle_avoidance::u.

void oa_init

(

void

)

Init the oa structure. Note: In the algorithm, the first polygon is a dummy one, and is used to represent the START and END points (so it has 2 vertices)

Definition at line 42 of file obstacle_avoidance.c.

References _poly::l, oa_start_end_points(), obstacle_avoidance::points, obstacle_avoidance::polys, and _poly::pts.

oa_poly_t* oa_new_poly

(

uint8_t

size

)

Create a new obstacle polygon. Return NULL on error.

Definition at line 84 of file obstacle_avoidance.c.

References obstacle_avoidance::cur_poly_idx, obstacle_avoidance::cur_pt_idx, _poly::l, MAX_POLY, MAX_PTS, obstacle_avoidance::points, obstacle_avoidance::polys, and _poly::pts.

void oa_poly_set_point	(	oa_poly_t *	pol,
		int32_t	x,
		int32_t	y,
		uint8_t	i
	)

Add a point to the polygon.

Definition at line 101 of file obstacle_avoidance.c.

References DEBUG, E_OA, _poly::pts, _ext_point::valid, _ext_point::weight, _ext_point::x, and _ext_point::y.

int8_t oa_process

(

void

)

process the path from start to end. Return 0 on sucess.

Definition at line 687 of file obstacle_avoidance.c.

References calc_rays(), calc_rays_weight(), obstacle_avoidance::cur_poly_idx, DEBUG, dijkstra(), E_OA, get_path(), obstacle_avoidance::polys, _poly::pts, obstacle_avoidance::ray_n, obstacle_avoidance::rays, obstacle_avoidance::u, obstacle_avoidance::weight, _ext_point::x, and _ext_point::y.

void oa_start_end_points	(	int32_t	st_x,
		int32_t	st_y,
		int32_t	en_x,
		int32_t	en_y
	)

Set the start and destination point. Return 0 on sucess

Definition at line 56 of file obstacle_avoidance.c.

References obstacle_avoidance::cur_poly_idx, obstacle_avoidance::cur_pt_idx, obstacle_avoidance::points, _ext_point::valid, _ext_point::weight, _ext_point::x, and _ext_point::y.

Referenced by oa_init().

void pts2line	(	const oa_ext_point_t *	p1,
		const oa_ext_point_t *	p2,
		oa_line_t *	l
	)

Definition at line 183 of file obstacle_avoidance.c.

References _line::a, ABS, _line::b, _line::c, MAX_COEF, _ext_point::x, and _ext_point::y.

Referenced by intersect_segment().