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_ */