00001 /* 00002 * Copyright Droids Corporation (2007) 00003 * 00004 * This program is free software; you can redistribute it and/or modify 00005 * it under the terms of the GNU General Public License as published by 00006 * the Free Software Foundation; either version 2 of the License, or 00007 * (at your option) any later version. 00008 * 00009 * This program is distributed in the hope that it will be useful, 00010 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 * GNU General Public License for more details. 00013 * 00014 * You should have received a copy of the GNU General Public License 00015 * along with this program; if not, write to the Free Software 00016 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00017 * 00018 * Revision : $Id: obstacle_avoidance.h,v 1.5 2009-03-15 21:51:18 zer0 Exp $ 00019 * 00020 * Main code and algorithm: Fabrice DESCLAUX <serpilliere@droids-corp.org> 00021 * Integration in Aversive: Olivier MATZ <zer0@droids-corp.org> 00022 */ 00023 00024 /* 00025 * The algorithm is based on the "visible point" algorithm. 00026 * There are 3 inputs: 00027 * - the play ground (basically the table, here a rectangle) 00028 * - the objects to avoid, represented by polygones 00029 * - start/stop points (A, B) 00030 * 00031 * The algorithm will first find every ray formed by 2 points that can 00032 * "see" each others. Basically, if a polygon is between two points, 00033 * they cannot see each others. A side of a polygon is composed by 2 00034 * points that can se each others. 00035 * 00036 * From all these rays, we can create a graph. We affect for each ray 00037 * a weight with its own length. 00038 * 00039 * The algorithm executes Dijkstra to find the shortest path to go 00040 * from A to B. 00041 */ 00042 00043 /* 00044 * As we run on 4Ko ram uC, we have static structures arrays to store: 00045 * - MAX_POLY => represent the maximum polygons to avoid in the area. 00046 * - MAX_PTS => maximize the sum of every polygons vertices. 00047 * - MAX_RAYS => maximum number of rays. 00048 * - MAX_CHKPOINTS => maximum accepted checkpoints in the resulting path. 00049 * - PLAYGROUND XXX => dimensions of the playground. 00050 */ 00051 00052 #ifndef _OBSTACLE_AVOIDANCE_H_ 00053 #define _OBSTACLE_AVOIDANCE_H_ 00054 00055 /* XXX this should be set in obstacle_avoidance_config.h !! */ 00056 #define MAX_POLY 3 00057 #define MAX_PTS 10 00058 #define MAX_RAYS 100 00059 #define MAX_CHKPOINTS 5 00060 00061 #define PLAYGROUND_X_MIN 0 // 30 00062 #define PLAYGROUND_X_MAX 300 // 270 00063 00064 #define PLAYGROUND_Y_MIN 0 // 30 00065 #define PLAYGROUND_Y_MAX 210 // 180 00066 00067 /* point used in processing */ 00068 typedef struct _ext_point { 00069 int32_t x; 00070 int32_t y; 00071 int32_t weight; 00072 00073 /* used for dijkstra */ 00074 uint8_t p; 00075 uint8_t pt; 00076 00077 /* Used to determine if the destination point is reachable */ 00078 uint8_t valid; 00079 00080 } oa_ext_point_t; 00081 00082 /* point used for result */ 00083 typedef struct _point { 00084 int16_t x; 00085 int16_t y; 00086 } oa_point_t; 00087 00088 /* A line is represented by the equation: 00089 * a*x + b*y + c = 0 00090 * 00091 * This is better than classic a*x + b = y : 00092 * here we can handle vertical (a*x + 0*y + c = 0) 00093 * and horizontal lines (0*x + b*y + c = 0) */ 00094 typedef struct _line { 00095 int32_t a; 00096 int32_t b; 00097 int32_t c; 00098 } oa_line_t; 00099 00100 typedef struct _poly { 00101 oa_ext_point_t * pts; 00102 uint8_t l; 00103 } oa_poly_t; 00104 00105 00106 struct obstacle_avoidance { 00107 oa_poly_t polys[MAX_POLY]; /* tab of polygons (obstacles) */ 00108 oa_ext_point_t points[MAX_PTS]; /* tab of points, referenced by polys */ 00109 00110 uint8_t ray_n; 00111 uint8_t cur_poly_idx; 00112 uint8_t cur_pt_idx; 00113 00114 uint16_t weight[MAX_RAYS]; 00115 union { 00116 uint8_t rays[MAX_RAYS*2]; 00117 oa_point_t res[MAX_CHKPOINTS]; 00118 } u; 00119 }; 00120 00121 /* To save memory space here is the moemory representation of 00122 * polygons/points: 00123 * 00124 * We have an array of points (oa_ext_point_t points): 00125 * _____ _____ _____ _____ _____ _____ _____ _____ _____ 00126 * | | | | | | | | | | 00127 * | p0 | p1 | p0 | p1 | p2 | p3 | p0 | p1 | p2 | 00128 * |_____|_____|_____|_____|_____|_____|_____|_____|_____| 00129 * 00130 * 00131 * ^ ^ ^ 00132 * | | | 00133 * -polygon 0 -polygon 1 -polygon 2 00134 * -2 vertices -4 vertices -3 vertices 00135 * 00136 * 00137 * And each polygon is represented by the sub array starting with the 00138 * point represented by oa_ext_point_t * pts and composed of uint8_t l; 00139 * (in the oa_poly_t structure) 00140 */ 00141 00143 void oa_init(void); 00144 00148 void oa_start_end_points(int32_t st_x, int32_t st_y, int32_t en_x, int32_t en_y); 00149 00153 oa_poly_t *oa_new_poly(uint8_t size); 00154 00155 00159 void oa_poly_set_point(oa_poly_t *pol, int32_t x, int32_t y, uint8_t i); 00160 00161 00165 int8_t oa_process(void); 00166 00170 oa_point_t * oa_get_path(void); 00171 00175 uint8_t is_point_in_poly(oa_poly_t *pol, int16_t x, int16_t y); 00176 00177 #endif /* _OBSTACLE_AVOIDANCE_H_ */
1.5.6