clothoid
[aversive.git] / modules / devices / robot / obstacle_avoidance / obstacle_avoidance.h
1 /*  
2  *  Copyright Droids Corporation (2007)
3  * 
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: obstacle_avoidance.h,v 1.1.2.7 2009-05-02 10:00:35 zer0 Exp $
19  *
20  *  Main code and algorithm: Fabrice DESCLAUX <serpilliere@droids-corp.org>
21  *  Integration in Aversive: Olivier MATZ <zer0@droids-corp.org>
22  */
23
24 /*
25  * The algorithm is based on the "visible point" algorithm.
26  * There are 3 inputs:
27  *   - the play ground (basically the table, here a rectangle)
28  *   - the objects to avoid, represented by polygones 
29  *   - start/stop points (A, B)
30  *
31  * The algorithm will first find every ray formed by 2 points that can
32  * "see" each others. Basically, if a polygon is between two points,
33  * they cannot see each others. A side of a polygon is composed by 2
34  * points that can se each others.
35  *
36  * From all these rays, we can create a graph. We affect for each ray
37  * a weight with its own length.
38  *
39  * The algorithm executes Dijkstra to find the shortest path to go
40  * from A to B.
41  */
42
43 /*
44  * As we run on 4Ko ram uC, we have static structures arrays to store:
45  *  - MAX_POLY => represent the maximum polygons to avoid in the area.
46  *  - MAX_PTS => maximize the sum of every polygons vertices.
47  *  - MAX_RAYS => maximum number of rays. 
48  *  - MAX_CHKPOINTS => maximum accepted checkpoints in the resulting path.
49  *  - PLAYGROUND XXX => dimensions of the playground.
50  */
51
52 #ifndef _OBSTACLE_AVOIDANCE_H_
53 #define _OBSTACLE_AVOIDANCE_H_
54
55 #include <obstacle_avoidance_config.h>
56
57 struct obstacle_avoidance {
58         poly_t polys[MAX_POLY];  /* tab of polygons (obstacles) */
59         point_t points[MAX_PTS]; /* tab of points, referenced by polys */
60         uint8_t valid[MAX_PTS];
61         int32_t pweight[MAX_PTS];
62         uint8_t p[MAX_PTS];
63         uint8_t pt[MAX_PTS];
64
65
66         
67         uint8_t ray_n;
68         uint8_t cur_poly_idx;
69         uint8_t cur_pt_idx;
70
71         uint16_t weight[MAX_RAYS];
72         union {
73                 uint8_t rays[MAX_RAYS*2];
74                 point_t res[MAX_CHKPOINTS];
75         } u;
76 };
77
78 /* To save memory space here is the moemory representation of
79  *   polygons/points:
80  *
81  *   We have an array of points (oa_ext_point_t points):  
82  *  _____ _____ _____ _____ _____ _____ _____ _____ _____
83  * |     |     |     |     |     |     |     |     |     |
84  * | p0  | p1  | p0  | p1  | p2  | p3  | p0  | p1  | p2  |
85  * |_____|_____|_____|_____|_____|_____|_____|_____|_____|
86  *
87  *
88  *  ^            ^                       ^
89  *  |            |                       |
90  *  -polygon 0   -polygon 1              -polygon 2
91  *  -2 vertices  -4 vertices             -3 vertices
92  *
93  *
94  * And each polygon is represented by the sub array starting with the
95  * point represented by oa_ext_point_t * pts and composed of uint8_t l; 
96  * (in the oa_poly_t structure)
97  */
98
99 /* reset oa without cleaning points */
100 void oa_reset(void);
101
102
103 /** Init the oa structure */
104 void oa_init(void);
105
106 /** 
107  * Set the start and destination point.
108  */
109 void oa_start_end_points(int32_t st_x, int32_t st_y, int32_t en_x, int32_t en_y);
110
111 /**
112  * Create a new obstacle polygon. Return NULL on error.
113  */
114 poly_t *oa_new_poly(uint8_t size);
115
116
117 /**
118  * Dump status
119  */
120 void oa_dump(void);
121
122 /**
123  * set a point of the polygon.
124  */
125 void oa_poly_set_point(poly_t *pol, int32_t x, int32_t y, uint8_t i);
126
127
128 /**
129  * process the path from start to end. Return 0 on sucess.
130  */
131 int8_t oa_process(void);
132
133 /**
134  * get the result after a call to oa_process()
135  */
136 point_t * oa_get_path(void);
137
138 #endif /* _OBSTACLE_AVOIDANCE_H_ */