projects/microb2010/mainboard/strat_avoid.c

   1 /*
   2  *  Copyright Droids, Microb Technology (2010)
   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: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23
  24 #include <stdio.h>
  25 #include <stdlib.h>
  26 #include <stdint.h>
  27 #include <math.h>
  28
  29 #include <aversive.h>
  30 #include <aversive/pgmspace.h>
  31
  32 #include <ax12.h>
  33 #include <uart.h>
  34 #include <pwm_ng.h>
  35 #include <clock_time.h>
  36 #include <spi.h>
  37
  38 #include <pid.h>
  39 #include <quadramp.h>
  40 #include <control_system_manager.h>
  41 #include <trajectory_manager.h>
  42 #include <trajectory_manager_utils.h>
  43 #include <trajectory_manager_core.h>
  44 #include <vect_base.h>
  45 #include <lines.h>
  46 #include <polygon.h>
  47 #include <obstacle_avoidance.h>
  48 #include <blocking_detection_manager.h>
  49 #include <robot_system.h>
  50 #include <position_manager.h>
  51
  52 #include <diagnostic.h>
  53
  54 #include <rdline.h>
  55 #include <parse.h>
  56
  57 #include "../common/i2c_commands.h"
  58 #include "i2c_protocol.h"
  59 #include "main.h"
  60 #include "strat.h"
  61 #include "strat_base.h"
  62 #include "strat_corn.h"
  63 #include "strat_utils.h"
  64 #include "sensor.h"
  65 #include "actuator.h"
  66
  67 /* XXX TODO
  68 static
  69 const
  70 change x,y -> i,j to avoid confusion with coords
  71 could be optimized in mem space: it is not needed to store the x,y coord,
  72    we can process it from idx. however it will be less optimized for speed
  73
  74 */
  75
  76 #define OFFSET_CORN_X 150
  77 #define OFFSET_CORN_Y 222
  78 #define STEP_CORN_X 225
  79 #define STEP_CORN_Y 250
  80
  81 #define CORN_NB 18
  82
  83 #define WAYPOINTS_NBX 13
  84 #define WAYPOINTS_NBY 8
  85
  86 /* enum is better */
  87 #define TYPE_WAYPOINT   0
  88 #define TYPE_DANGEROUS  1
  89 #define TYPE_WHITE_CORN 2
  90 #define TYPE_BLACK_CORN 3
  91 #define TYPE_OBSTACLE   4
  92 #define TYPE_UNKNOWN    5
  93
  94 /* XXX enum possible ? else just rename */
  95 #define START      0
  96 #define UP         1
  97 #define UP_RIGHT   2
  98 #define DOWN_RIGHT 3
  99 #define DOWN       4
 100 #define DOWN_LEFT  5
 101 #define UP_LEFT    6
 102 #define END        7
 103
 104 struct djpoint {
 105         uint16_t weight;
 106         struct djpoint *parent;
 107
 108         uint8_t type:3;
 109         uint8_t parent_pos:3;
 110         uint8_t updated:1;
 111         uint8_t todo:1;
 112 };
 113
 114 uint8_t corn_table[CORN_NB];
 115
 116 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
 117
 118 /* return index from neigh pointer */
 119 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
 120
 121 void dump(void)
 122 {
 123         int8_t i, j;
 124         struct djpoint *pt;
 125
 126         printf_P(PSTR("         "));
 127         for (i=0; i<WAYPOINTS_NBX; i++) {
 128                 printf_P(PSTR(" %2d "), i);
 129         }
 130         printf_P(PSTR("\r\n"));
 131
 132         for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
 133                 printf_P(PSTR("%3d   "), j/2);
 134
 135                 if ((j&1) == 0)
 136                         printf_P(PSTR("    "));
 137
 138                 for (i=0; i<WAYPOINTS_NBX; i++) {
 139                         pt = &djpoints[i][j/2];
 140
 141                         if (((i+j) & 1) == 0)
 142                                 continue;
 143
 144                         if (pt->type == TYPE_OBSTACLE)
 145                                 printf_P(PSTR("     X  "));
 146                         else if (pt->type == TYPE_DANGEROUS)
 147                                 printf_P(PSTR("     D  "));
 148                         else if (pt->type == TYPE_WHITE_CORN)
 149                                 printf_P(PSTR("     W  "));
 150                         else if (pt->type == TYPE_BLACK_CORN)
 151                                 printf_P(PSTR("     B  "));
 152                         else if (pt->type == TYPE_WAYPOINT)
 153                                 printf_P(PSTR(" %5d  "), pt->weight);
 154                         else
 155                                 printf_P(PSTR("     ?  "));
 156                 }
 157                 printf_P(PSTR("\r\n"));
 158         }
 159 }
 160
 161 static inline uint8_t opposite_position(uint8_t pos)
 162 {
 163         pos += 3;
 164         if (pos > UP_LEFT)
 165                 pos -= 6;
 166         return pos;
 167 }
 168
 169 /* return coord of the entry in the table from the pointer */
 170 static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
 171 {
 172         int8_t idx = PT2IDX(pt);
 173         *x = idx / WAYPOINTS_NBY;
 174         *y = idx % WAYPOINTS_NBY;
 175 }
 176
 177 /* get the neighbour of the point at specified position */
 178 static struct djpoint *get_neigh(struct djpoint *pt,
 179                                  uint8_t position)
 180 {
 181         int8_t i,j;
 182         struct djpoint *neigh;
 183
 184         djpoint2ij(pt, &i, &j);
 185
 186         switch (position) {
 187         case UP:
 188                 j++;
 189                 break;
 190         case UP_RIGHT:
 191                 if (!(i & 1)) j++;
 192                 i++;
 193                 break;
 194         case DOWN_RIGHT:
 195                 if (i & 1) j--;
 196                 i++;
 197                 break;
 198         case DOWN:
 199                 j--;
 200                 break;
 201         case DOWN_LEFT:
 202                 if (i & 1) j--;
 203                 i--;
 204                 break;
 205         case UP_LEFT:
 206                 if (!(i & 1)) j++;
 207                 i--;
 208                 break;
 209         default:
 210                 return NULL;
 211         }
 212         if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 213                 return NULL;
 214
 215         neigh = &djpoints[i][j];
 216
 217         if (neigh->type != TYPE_WAYPOINT)
 218                 return NULL;
 219
 220         return neigh;
 221 }
 222
 223 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
 224 {
 225         struct djpoint *neigh = NULL;
 226         uint8_t pos = *position + 1;
 227
 228         for (pos = *position + 1; pos < END; pos++) {
 229                 neigh = get_neigh(pt, pos);
 230                 if (neigh != NULL)
 231                         break;
 232         }
 233
 234         *position = pos;
 235         return neigh;
 236 }
 237
 238 /* browse all points */
 239 #define POINT_FOREACH(cur)                                              \
 240         for (cur = &djpoints[0][0];                                     \
 241              cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];             \
 242              cur ++)
 243
 244 /* XXX comment */
 245 #define NEIGH_FOREACH(neigh, pos, point)                        \
 246         for (pos = START, neigh = get_next_neigh(point, &pos);  \
 247              neigh;                                             \
 248              neigh = get_next_neigh(point, &pos))
 249
 250 int dijkstra_init(void)
 251 {
 252         return 0;
 253 }
 254
 255 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
 256 {
 257         double vx, vy;
 258         vx = p2->pos.x - p1->pos.x;
 259         vy = p2->pos.y - p1->pos.y;
 260         return sqrt(vx * vx + vy * vy);
 261 }
 262
 263 void dijkstra_process_neighs(struct djpoint *pt)
 264 {
 265         struct djpoint *neigh;
 266         uint8_t pos, parent_pos;
 267         uint16_t weight;
 268         int8_t i,j;
 269
 270         djpoint2ij(pt, &i, &j);
 271         printf_P(PSTR("at %d %d:\r\n"), i, j);
 272
 273         NEIGH_FOREACH(neigh, pos, pt) {
 274                 weight = pt->weight + dist(pt, neigh);
 275                 parent_pos = opposite_position(pos);
 276
 277                 /* bonus if we keep the same direction */
 278                 if (parent_pos == pt->parent_pos ||
 279                     pt->parent_pos == END)
 280                         weight = weight - 1;
 281
 282                 printf_P(PSTR("  pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
 283                        pt->parent_pos, parent_pos,
 284                        neigh->weight, weight);
 285                 if (neigh->weight == 0 || weight < neigh->weight) {
 286                         djpoint2ij(neigh, &i, &j);
 287                         //printf_P(PSTR("    %d,%d updated\r\n"), i, j);
 288                         neigh->weight = weight;
 289                         neigh->parent_pos = parent_pos;
 290                         neigh->updated = 1;
 291                 }
 292         }
 293 }
 294
 295 int dijkstra(struct djpoint *start)
 296 {
 297         struct djpoint *cur;
 298         uint8_t todolist = 1;
 299
 300         start->todo = 1;
 301
 302         while (todolist) {
 303                 printf_P(PSTR("\r\n"));
 304                 dump();
 305                 /* process all neighbours of todo list */
 306                 POINT_FOREACH(cur) {
 307                         if (!cur->todo)
 308                                 continue;
 309                         dijkstra_process_neighs(cur);
 310                         cur->todo = 0;
 311                 }
 312
 313                 /* convert updated list in todo list */
 314                 todolist = 0;
 315                 POINT_FOREACH(cur) {
 316                         if (!cur->updated)
 317                                 continue;
 318                         todolist = 1;
 319                         cur->todo = 1;
 320                         cur->updated = 0;
 321                 }
 322         }
 323         return 0; /* XXX */
 324 }
 325
 326 /* init waypoints position */
 327 void init_djpoints(void)
 328 {
 329         int8_t i, j;
 330         struct djpoint *pt;
 331
 332         for (i=0; i<WAYPOINTS_NBX; i++) {
 333
 334                 for (j=0; j<WAYPOINTS_NBY; j++) {
 335                         pt = &djpoints[i][j];
 336
 337                         pt->type = TYPE_WAYPOINT;
 338                         pt->parent_pos = END;
 339                         pt->updated = 0;
 340                         pt->todo = 0;
 341                 }
 342         }
 343 }
 344
 345 /* update the type and weight of waypoints, before starting
 346  * dijkstra */
 347 void update_djpoints(void)
 348 {
 349         int8_t i, j, c;
 350         struct djpoint *pt;
 351
 352         for (i=0; i<WAYPOINTS_NBX; i++) {
 353
 354                 for (j=0; j<WAYPOINTS_NBY; j++) {
 355                         pt = &djpoints[i][j];
 356
 357                         pt->weight = 0;
 358
 359                         /* corn */
 360                         c = ijcoord_to_corn_idx(i, j);
 361                         if (c >= 0) {
 362                                 pt->type = corn_table[c];
 363                                 continue;
 364                         }
 365                         /* too close of border */
 366                         if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
 367                                 pt->type = TYPE_OBSTACLE;
 368                                 continue;
 369                         }
 370                         /* hill */
 371                         if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
 372                                 pt->type = TYPE_OBSTACLE;
 373                                 continue;
 374                         }
 375                         /* dangerous points */
 376                         if (i == 0 || i == (WAYPOINTS_NBX-1)) {
 377                                 pt->type = TYPE_DANGEROUS;
 378                                 continue;
 379                         }
 380                         if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
 381                                 pt->type = TYPE_DANGEROUS;
 382                                 continue;
 383                         }
 384                         pt->type = TYPE_WAYPOINT;
 385                 }
 386         }
 387 }
 388
 389 int get_path(struct djpoint *start, struct djpoint *end)
 390 {
 391         struct djpoint *cur;
 392         uint8_t prev_direction = 0;
 393         int8_t idx;
 394         int16_t x, y;
 395
 396         for (cur = start;
 397              cur != NULL && cur->parent_pos != END && cur != end;
 398              cur = get_neigh(cur, cur->parent_pos)) {
 399                 if (prev_direction != cur->parent_pos) {
 400                         idx = PT2IDX(cur);
 401                         corn_idx_to_coordxy(idx, &x, &y);
 402                         printf_P(PSTR("%d %d (%d)\r\n"),
 403                                  x, y, cur->parent_pos);
 404                 }
 405                 prev_direction = cur->parent_pos;
 406         }
 407         idx = PT2IDX(end);
 408         corn_idx_to_coordxy(idx, &x, &y);
 409         printf_P(PSTR("%d %d\r\n"), x, y);
 410
 411         return 0; /* XXX */
 412 }