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_db.h"
  62 #include "strat_base.h"
  63 #include "strat_corn.h"
  64 #include "strat_utils.h"
  65 #include "sensor.h"
  66 #include "actuator.h"
  67
  68 /* XXX TODO
  69 static
  70 const
  71 change x,y -> i,j to avoid confusion with coords
  72 could be optimized in mem space: it is not needed to store the x,y coord,
  73    we can process it from idx. however it will be less optimized for speed
  74
  75 */
  76
  77 /* XXX enum possible ? else just rename */
  78 #define START      0
  79 #define UP         1
  80 #define UP_RIGHT   2
  81 #define DOWN_RIGHT 3
  82 #define DOWN       4
  83 #define DOWN_LEFT  5
  84 #define UP_LEFT    6
  85 #define END        7
  86
  87 struct djpoint {
  88         uint16_t weight;
  89         struct djpoint *parent;
  90
  91         uint8_t parent_pos:3;
  92         uint8_t updated:1;
  93         uint8_t todo:1;
  94         uint8_t reserved:3;
  95 };
  96
  97 /* database for dijkstra */
  98 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
  99
 100 /* return index from neigh pointer */
 101 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
 102
 103 void dump(void)
 104 {
 105         int8_t i, j;
 106         struct djpoint *pt;
 107         struct waypoint_db *wp;
 108
 109         printf_P(PSTR("         "));
 110         for (i=0; i<WAYPOINTS_NBX; i++) {
 111                 printf_P(PSTR(" %2d "), i);
 112         }
 113         printf_P(PSTR("\r\n"));
 114
 115         for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
 116                 printf_P(PSTR("%3d   "), j/2);
 117
 118                 if ((j&1) == 0)
 119                         printf_P(PSTR("    "));
 120
 121                 for (i=0; i<WAYPOINTS_NBX; i++) {
 122                         pt = &djpoints[i][j/2];
 123                         wp = &strat_db.wp_table[i][j/2];
 124
 125                         if (((i+j) & 1) == 0)
 126                                 continue;
 127
 128                         if (wp->type == WP_TYPE_OBSTACLE)
 129                                 printf_P(PSTR("     X  "));
 130                         else if (wp->dangerous)
 131                                 printf_P(PSTR("     D  "));
 132                         else if (wp->type == WP_TYPE_CORN &&
 133                                  wp->corn.color == I2C_COB_WHITE)
 134                                 printf_P(PSTR("     W  "));
 135                         else if (wp->type == WP_TYPE_CORN &&
 136                                  wp->corn.color == I2C_COB_BLACK)
 137                                 printf_P(PSTR("     B  "));
 138                         else if (wp->type == WP_TYPE_CORN &&
 139                                  wp->corn.color == I2C_COB_UNKNOWN)
 140                                 printf_P(PSTR("     U  "));
 141                         else if (wp->type == WP_TYPE_WAYPOINT ||
 142                                  wp->type == WP_TYPE_TOMATO)
 143                                 printf_P(PSTR(" %5d  "), pt->weight);
 144                         else
 145                                 printf_P(PSTR("     ?  "));
 146                 }
 147                 printf_P(PSTR("\r\n"));
 148         }
 149 }
 150
 151 static inline uint8_t opposite_position(uint8_t pos)
 152 {
 153         pos += 3;
 154         if (pos > UP_LEFT)
 155                 pos -= 6;
 156         return pos;
 157 }
 158
 159 /* is point reachable by the robot ? */
 160 static uint8_t is_reachable(uint8_t i, uint8_t j)
 161 {
 162         struct waypoint_db *wp;
 163
 164         wp = &strat_db.wp_table[i][j];
 165         if (wp->type == WP_TYPE_WAYPOINT)
 166                 return 1;
 167         if (wp->type == WP_TYPE_TOMATO)
 168                 return 1;
 169         if (wp->type == WP_TYPE_CORN &&
 170             wp->present == 0)
 171                 return 1;
 172         return 0;
 173 }
 174
 175 /* return coord of the entry in the table from the pointer */
 176 static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j)
 177 {
 178         int8_t idx = PT2IDX(pt);
 179         *i = idx / WAYPOINTS_NBY;
 180         *j = idx % WAYPOINTS_NBY;
 181 }
 182
 183 /* get the neighbour of the point at specified position */
 184 static struct djpoint *get_neigh(struct djpoint *pt,
 185                                  uint8_t position)
 186 {
 187         uint8_t i,j;
 188         struct djpoint *neigh;
 189
 190         djpoint2ij(pt, &i, &j);
 191
 192         switch (position) {
 193         case UP:
 194                 j++;
 195                 break;
 196         case UP_RIGHT:
 197                 if (!(i & 1)) j++;
 198                 i++;
 199                 break;
 200         case DOWN_RIGHT:
 201                 if (i & 1) j--;
 202                 i++;
 203                 break;
 204         case DOWN:
 205                 j--;
 206                 break;
 207         case DOWN_LEFT:
 208                 if (i & 1) j--;
 209                 i--;
 210                 break;
 211         case UP_LEFT:
 212                 if (!(i & 1)) j++;
 213                 i--;
 214                 break;
 215         default:
 216                 return NULL;
 217         }
 218         if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 219                 return NULL;
 220
 221         if (is_reachable(i, j) == 0)
 222                 return NULL;
 223
 224         neigh = &djpoints[i][j];
 225         return neigh;
 226 }
 227
 228 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
 229 {
 230         struct djpoint *neigh = NULL;
 231         uint8_t pos = *position + 1;
 232
 233         for (pos = *position + 1; pos < END; pos++) {
 234                 neigh = get_neigh(pt, pos);
 235                 if (neigh != NULL)
 236                         break;
 237         }
 238
 239         *position = pos;
 240         return neigh;
 241 }
 242
 243 /* browse all points */
 244 #define POINT_FOREACH(cur)                                              \
 245         for (cur = &djpoints[0][0];                                     \
 246              cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];             \
 247              cur ++)
 248
 249 /* XXX comment */
 250 #define NEIGH_FOREACH(neigh, pos, point)                        \
 251         for (pos = START, neigh = get_next_neigh(point, &pos);  \
 252              neigh;                                             \
 253              neigh = get_next_neigh(point, &pos))
 254
 255 int dijkstra_init(void)
 256 {
 257         return 0;
 258 }
 259
 260 /* return distance between p1 and p2 */
 261 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
 262 {
 263         int16_t x1, y1, x2, y2;
 264         double vx, vy;
 265         uint8_t i, j;
 266
 267         djpoint2ij(p1, &i, &j);
 268         ijcoord_to_xycoord(i, j, &x1, &y1);
 269
 270         djpoint2ij(p2, &i, &j);
 271         ijcoord_to_xycoord(i, j, &x2, &y2);
 272
 273         vx = x2 - x1;
 274         vy = y2 - y1;
 275         return sqrt(vx * vx + vy * vy);
 276 }
 277
 278 void dijkstra_process_neighs(struct djpoint *pt)
 279 {
 280         struct djpoint *neigh;
 281         uint8_t pos, parent_pos;
 282         uint16_t weight;
 283         uint8_t i,j;
 284
 285         djpoint2ij(pt, &i, &j);
 286         printf_P(PSTR("at %d %d:\r\n"), i, j);
 287
 288         NEIGH_FOREACH(neigh, pos, pt) {
 289                 weight = pt->weight + dist(pt, neigh);
 290                 parent_pos = opposite_position(pos);
 291
 292                 /* bonus if we keep the same direction */
 293                 if (parent_pos == pt->parent_pos ||
 294                     pt->parent_pos == END)
 295                         weight = weight - 1;
 296
 297                 printf_P(PSTR("  pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
 298                        pt->parent_pos, parent_pos,
 299                        neigh->weight, weight);
 300                 if (neigh->weight == 0 || weight < neigh->weight) {
 301                         djpoint2ij(neigh, &i, &j);
 302                         //printf_P(PSTR("    %d,%d updated\r\n"), i, j);
 303                         neigh->weight = weight;
 304                         neigh->parent_pos = parent_pos;
 305                         neigh->updated = 1;
 306                 }
 307         }
 308 }
 309
 310 int dijkstra(struct djpoint *start)
 311 {
 312         struct djpoint *cur;
 313         uint8_t todolist = 1;
 314
 315         start->todo = 1;
 316
 317         while (todolist) {
 318                 printf_P(PSTR("\r\n"));
 319                 dump();
 320                 /* process all neighbours of todo list */
 321                 POINT_FOREACH(cur) {
 322                         if (!cur->todo)
 323                                 continue;
 324                         dijkstra_process_neighs(cur);
 325                         cur->todo = 0;
 326                 }
 327
 328                 /* convert updated list in todo list */
 329                 todolist = 0;
 330                 POINT_FOREACH(cur) {
 331                         if (!cur->updated)
 332                                 continue;
 333                         todolist = 1;
 334                         cur->todo = 1;
 335                         cur->updated = 0;
 336                 }
 337         }
 338         return 0; /* XXX */
 339 }
 340
 341 /* init waypoints position */
 342 void init_djpoints(void)
 343 {
 344         int8_t i, j;
 345         struct djpoint *pt;
 346
 347         for (i=0; i<WAYPOINTS_NBX; i++) {
 348
 349                 for (j=0; j<WAYPOINTS_NBY; j++) {
 350                         pt = &djpoints[i][j];
 351                         pt->parent_pos = END;
 352                         pt->updated = 0;
 353                         pt->todo = 0;
 354                         pt->weight = 0;
 355                 }
 356         }
 357 }
 358
 359 int get_path(struct djpoint *start, struct djpoint *end)
 360 {
 361         struct djpoint *cur;
 362         uint8_t prev_direction = 0;
 363         int8_t idx;
 364         int16_t x, y;
 365
 366         for (cur = start;
 367              cur != NULL && cur->parent_pos != END && cur != end;
 368              cur = get_neigh(cur, cur->parent_pos)) {
 369                 if (prev_direction != cur->parent_pos) {
 370                         idx = PT2IDX(cur);
 371                         corn_idx_to_xycoord(idx, &x, &y);
 372                         printf_P(PSTR("%d %d (%d)\r\n"),
 373                                  x, y, cur->parent_pos);
 374                 }
 375                 prev_direction = cur->parent_pos;
 376         }
 377         idx = PT2IDX(end);
 378         corn_idx_to_xycoord(idx, &x, &y);
 379         printf_P(PSTR("%d %d\r\n"), x, y);
 380
 381         return 0; /* XXX */
 382 }