projects/microb2010/mainboard/strat_corn.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/error.h>
  31 #include <aversive/pgmspace.h>
  32
  33 #include <ax12.h>
  34 #include <uart.h>
  35 #include <pwm_ng.h>
  36 #include <clock_time.h>
  37 #include <spi.h>
  38
  39 #include <pid.h>
  40 #include <quadramp.h>
  41 #include <control_system_manager.h>
  42 #include <trajectory_manager.h>
  43 #include <trajectory_manager_utils.h>
  44 #include <trajectory_manager_core.h>
  45 #include <vect_base.h>
  46 #include <lines.h>
  47 #include <polygon.h>
  48 #include <obstacle_avoidance.h>
  49 #include <blocking_detection_manager.h>
  50 #include <robot_system.h>
  51 #include <position_manager.h>
  52
  53 #include <diagnostic.h>
  54
  55 #include <rdline.h>
  56 #include <parse.h>
  57
  58 #include "../common/i2c_commands.h"
  59 #include "i2c_protocol.h"
  60 #include "main.h"
  61 #include "strat.h"
  62 #include "strat_db.h"
  63 #include "strat_base.h"
  64 #include "strat_corn.h"
  65 #include "strat_avoid.h"
  66 #include "strat_utils.h"
  67 #include "sensor.h"
  68 #include "actuator.h"
  69
  70 static volatile uint8_t clitoid_slow = 0;
  71
  72 /* return 1 if there is a corn near, and fill the index ptr */
  73 int8_t corn_is_near(uint8_t *corn_idx, uint8_t side,
  74                     int16_t *xspickle, int16_t *yspickle)
  75 {
  76         /* XXX to be checked */
  77 #define SENSOR_CORN_DIST  225
  78 #define SENSOR_CORN_ANGLE 90
  79         double x = position_get_x_double(&mainboard.pos);
  80         double y = position_get_y_double(&mainboard.pos);
  81         double a_rad = position_get_a_rad_double(&mainboard.pos);
  82         double x_corn, y_corn;
  83         int16_t x_corn_int, y_corn_int;
  84         struct waypoint_db *wp;
  85
  86         if (side == I2C_LEFT_SIDE) {
  87                 x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  88                 y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  89         }
  90         else {
  91                 x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  92                 y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  93         }
  94         x_corn_int = x_corn;
  95         y_corn_int = y_corn;
  96         *xspickle = x_corn_int;
  97         *yspickle = y_corn_int;
  98
  99         wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
 100         if (wp == NULL)
 101                 return 0;
 102         *corn_idx = wp->corn.idx;
 103         return 1;
 104 }
 105
 106 /* fill 2 points that are on the line (num, dir) */
 107 static void num2line(struct line_2pts *l, uint8_t num, uint8_t dir)
 108 {
 109         float n = num;
 110
 111         switch (dir) {
 112
 113         case LINE_UP:
 114                 l->p1.x = n * 450 + 375;
 115                 l->p1.y = COLOR_Y(0);
 116                 l->p2.x = n * 450 + 375;
 117                 l->p2.y = COLOR_Y(2100);
 118                 break;
 119         case LINE_DOWN:
 120                 l->p1.x = n * 450 + 375;
 121                 l->p1.y = COLOR_Y(2100);
 122                 l->p2.x = n * 450 + 375;
 123                 l->p2.y = COLOR_Y(0);
 124                 break;
 125         case LINE_R_UP:
 126                 l->p1.x = 150;
 127                 l->p1.y = COLOR_Y(-n * 500 + 1472);
 128                 l->p2.x = 2850;
 129                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
 130                 break;
 131         case LINE_L_DOWN:
 132                 l->p1.x = 2850;
 133                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
 134                 l->p2.x = 150;
 135                 l->p2.y = COLOR_Y(-n * 500 + 1472);
 136                 break;
 137         case LINE_L_UP:
 138                 l->p1.x = 2850;
 139                 l->p1.y = COLOR_Y(-n * 500 + 1472);
 140                 l->p2.x = 150;
 141                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
 142                 break;
 143         case LINE_R_DOWN:
 144                 l->p1.x = 150;
 145                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
 146                 l->p2.x = 2850;
 147                 l->p2.y = COLOR_Y(-n * 500 + 1472);
 148                 break;
 149         default:
 150                 break;
 151         }
 152 }
 153
 154 /* return true if we must go slow */
 155 static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
 156                                     uint8_t num2, uint8_t dir2)
 157 {
 158         int16_t x, y;
 159         uint8_t i, j;
 160         uint8_t i2, i3, j2, j3; /* next wp */
 161
 162         x = position_get_x_s16(&mainboard.pos);
 163         y = position_get_y_s16(&mainboard.pos);
 164
 165         if (get_cob_count() >= 5)
 166                 return 0; /* fast */
 167
 168         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
 169                 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
 170                       __FUNCTION__, x, y);
 171                 return 1;
 172         }
 173
 174         if (corn_count_neigh(i, j) == 2)
 175                 return 1;
 176
 177
 178         /* we are on intersection, keep the same speed... but as we
 179          * enter in the curve-part of the clitoid, we should not go
 180          * there */
 181         if (wp_belongs_to_line(i, j, num2, dir2))
 182                 return clitoid_slow;
 183
 184         /* we can ge fast if it's a 60deg angle and if we checked the
 185          * current point */
 186         if (is_60deg(dir1, dir2))
 187                 return 0;
 188
 189         /* get next point */
 190         if (wp_get_neigh(i, j, &i2, &j2, dir1) < 0) {
 191                 DEBUG(E_USER_STRAT, "%s(): cannot get neigh1",
 192                       __FUNCTION__);
 193                 return 1;
 194         }
 195
 196         /* if (i2, j2) belongs to next line, check corns */
 197         if (wp_belongs_to_line(i2, j2, num2, dir2)) {
 198                 if (corn_count_neigh(i2, j2) > 0)
 199                         return 1;
 200                 else
 201                         return 0;
 202         }
 203
 204         /* get next point */
 205         if (wp_get_neigh(i2, j2, &i3, &j3, dir1) < 0) {
 206                 DEBUG(E_USER_STRAT, "%s(): cannot get neigh2",
 207                       __FUNCTION__);
 208                 return 1;
 209         }
 210
 211         /* if (i3, j3) belongs to next line, check corns */
 212         if (wp_belongs_to_line(i3, j3, num2, dir2)) {
 213                 if (corn_count_neigh(i2, j2) > 0 ||
 214                     corn_count_neigh(i3, j3) > 0)
 215                         return 1;
 216                 else
 217                         return 0;
 218         }
 219
 220         /* go fast */
 221         return 0;
 222 }
 223
 224 /*
 225  * handle speed before clitoid (on the line), depending on strat_db.
 226  * return true if clitoid started
 227  */
 228 #define NORETURN_DIST 300
 229 static uint8_t speedify_clitoid(uint8_t num1, uint8_t dir1,
 230                                 uint8_t num2, uint8_t dir2)
 231 {
 232         uint8_t slow;
 233         double turnx, turny;
 234
 235         slow = clitoid_select_speed(num1, dir1, num2, dir2);
 236         if (slow != clitoid_slow) {
 237                 turnx = mainboard.traj.target.line.turn_pt.x;
 238                 turny = mainboard.traj.target.line.turn_pt.y;
 239                 if (distance_from_robot(turnx, turny) > NORETURN_DIST) {
 240                         clitoid_slow = slow;
 241                         return 1;
 242                 }
 243         }
 244
 245         return trajectory_get_state(&mainboard.traj) == RUNNING_CLITOID_CURVE;
 246 }
 247
 248 /* process the clitoid parameters, return 0 on success or -1 if
 249  * clitoid cannot be executed. pack_spickles is set to I2C_LEFT_SIDE,
 250  * I2C_RIGHT_SIDE or I2C_NO_SIDE to tell if we need to pack a specific
 251  * spickle. */
 252 static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
 253                                  uint8_t num2, uint8_t dir2,
 254                                  uint8_t *pack_spickles)
 255 {
 256         double line1_a_rad, line1_a_deg, line2_a_rad;
 257         double diff_a_deg, diff_a_deg_abs, beta_deg;
 258         double radius;
 259         struct line_2pts l1, l2;
 260         line_t ll1, ll2;
 261         point_t p;
 262         int8_t ret;
 263
 264         /* convert to 2 points */
 265         num2line(&l1, num1, dir1);
 266         num2line(&l2, num2, dir2);
 267
 268         DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
 269               l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
 270         DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
 271               l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
 272
 273         /* convert to line eq and find intersection */
 274         pts2line(&l1.p1, &l1.p2, &ll1);
 275         pts2line(&l2.p1, &l2.p2, &ll2);
 276         intersect_line(&ll1, &ll2, &p);
 277
 278         line1_a_rad = atan2(l1.p2.y - l1.p1.y,
 279                             l1.p2.x - l1.p1.x);
 280         line1_a_deg = DEG(line1_a_rad);
 281         line2_a_rad = atan2(l2.p2.y - l2.p1.y,
 282                             l2.p2.x - l2.p1.x);
 283         diff_a_deg = DEG(line2_a_rad - line1_a_rad);
 284         if (diff_a_deg < -180) {
 285                 diff_a_deg += 360;
 286         }
 287         else if (diff_a_deg > 180) {
 288                 diff_a_deg -= 360;
 289         }
 290         diff_a_deg_abs = fabs(diff_a_deg);
 291
 292 /*      printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
 293 /*      printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
 294
 295         *pack_spickles = I2C_NO_SIDE;
 296
 297         /* small angle, 60 deg */
 298         if (diff_a_deg_abs < 70.) {
 299                 radius = 150;
 300                 if (diff_a_deg > 0) {
 301                         beta_deg = 0;
 302                         *pack_spickles = I2C_RIGHT_SIDE;
 303                 }
 304                 else {
 305                         beta_deg = 0;
 306                         *pack_spickles = I2C_LEFT_SIDE;
 307                 }
 308         }
 309         /* double 90 deg for half turn -- not used */
 310         else if (diff_a_deg_abs < 100.) {
 311                 radius = 100;
 312                 if (diff_a_deg > 0)
 313                         beta_deg = 40;
 314                 else
 315                         beta_deg = -40;
 316         }
 317         /* hard turn, 120 deg */
 318         else {
 319                 radius = 75;
 320                 if (diff_a_deg > 0)
 321                         beta_deg = 0;
 322                 else
 323                         beta_deg = 0;
 324         }
 325
 326         clitoid_slow = clitoid_select_speed(num1, dir1, num2, dir2);
 327         if (clitoid_slow) {
 328                 DEBUG(E_USER_STRAT, "slow clito");
 329                 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 330         }
 331         else {
 332                 DEBUG(E_USER_STRAT, "fast clito");
 333                 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 334         }
 335
 336         ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
 337                                  line1_a_deg, 150., diff_a_deg, beta_deg,
 338                                  radius, xy_norm(l1.p1.x, l1.p1.y,
 339                                                  p.x, p.y));
 340         return ret;
 341 }
 342
 343 /* go from line num1,dir1 to line num2,dir2. Uses trjectory flags
 344  * specified as argument and return END_xxx condition */
 345 uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
 346                   uint8_t dir2, uint8_t flags)
 347 {
 348         int8_t ret;
 349         uint8_t err, pack_spickles;
 350
 351  reprocess:
 352         ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);
 353         if (ret < 0) {
 354                 DEBUG(E_USER_STRAT, "clitoid failed");
 355                 return END_ERROR;
 356         }
 357
 358         /* XXX what to do if cobboard is stucked */
 359
 360         /* wait beginning of clitoid or changing of speed */
 361         err = WAIT_COND_OR_TRAJ_END(speedify_clitoid(num1, dir1,
 362                                                      num2, dir2),
 363                                     flags);
 364
 365         /* error during traj, or traj finished */
 366         if (err != 0)
 367                 return err;
 368
 369         /* the speed has to change */
 370         if (err == 0 &&
 371             trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
 372                 goto reprocess;
 373
 374         DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",
 375               err, pack_spickles);
 376
 377         /* when clitoid starts and angle is 60 deg, pack external
 378          * spickle */
 379         if (err == 0) {
 380                 if (pack_spickles == I2C_LEFT_SIDE)
 381                         strat_lpack60 = 1;
 382                 else if (pack_spickles == I2C_RIGHT_SIDE)
 383                         strat_rpack60 = 1;
 384
 385                 /* wait end of clitoid */
 386                 err = wait_traj_end(flags);
 387         }
 388
 389         DEBUG(E_USER_STRAT, "clitoid finished, err=%d", err);
 390
 391         strat_rpack60 = 0;
 392         strat_lpack60 = 0;
 393         return err;
 394 }
 395