projects/microb2010/mainboard/strat_utils.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology (2009)
   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_utils.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  */
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/wait.h>
  28 #include <aversive/error.h>
  29
  30 #include <ax12.h>
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <clock_time.h>
  34 #include <spi.h>
  35
  36 #include <pid.h>
  37 #include <quadramp.h>
  38 #include <control_system_manager.h>
  39 #include <trajectory_manager.h>
  40 #include <trajectory_manager_utils.h>
  41 #include <vect_base.h>
  42 #include <lines.h>
  43 #include <polygon.h>
  44 #include <obstacle_avoidance.h>
  45 #include <blocking_detection_manager.h>
  46 #include <robot_system.h>
  47 #include <position_manager.h>
  48
  49 #include <rdline.h>
  50 #include <parse.h>
  51
  52 #include "../common/i2c_commands.h"
  53
  54 #include "main.h"
  55 #include "strat_utils.h"
  56 #include "strat.h"
  57 #include "sensor.h"
  58 #include "i2c_protocol.h"
  59
  60 /* return the distance between two points */
  61 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
  62 {
  63         int32_t x,y;
  64         x = (x2-x1);
  65         x = x*x;
  66         y = (y2-y1);
  67         y = y*y;
  68         return sqrt(x+y);
  69 }
  70
  71 /* return the distance between two points */
  72 int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
  73 {
  74         int32_t x,y;
  75         x = (x2-x1);
  76         x = x*x;
  77         y = (y2-y1);
  78         y = y*y;
  79         return x+y;
  80 }
  81
  82 /* return the distance to a point in the area */
  83 int16_t distance_from_robot(int16_t x, int16_t y)
  84 {
  85         return distance_between(position_get_x_s16(&mainboard.pos),
  86                                 position_get_y_s16(&mainboard.pos), x, y);
  87 }
  88
  89 /** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
  90 int16_t simple_modulo_360(int16_t a)
  91 {
  92         if (a < -180) {
  93                 a += 360;
  94         }
  95         else if (a > 180) {
  96                 a -= 360;
  97         }
  98         return a;
  99 }
 100
 101 /* return the distance to a point in the area */
 102 int16_t angle_abs_to_rel(int16_t a_abs)
 103 {
 104         return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
 105 }
 106
 107 void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
 108                       double *x_abs, double *y_abs)
 109 {
 110         double x = position_get_x_double(&mainboard.pos);
 111         double y = position_get_y_double(&mainboard.pos);
 112         double a = position_get_a_rad_double(&mainboard.pos);
 113
 114         *x_abs = x + d_rel*cos(a+a_rel_rad);
 115         *y_abs = y + d_rel*sin(a+a_rel_rad);
 116 }
 117
 118 double norm(double x, double y)
 119 {
 120         return sqrt(x*x + y*y);
 121 }
 122
 123 void rel_xy_to_abs_xy(double x_rel, double y_rel,
 124                       double *x_abs, double *y_abs)
 125 {
 126         double d_rel, a_rel;
 127         d_rel = norm(x_rel, y_rel);
 128         a_rel = atan2(y_rel, x_rel);
 129         rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
 130 }
 131
 132 /* return an angle between -pi and pi */
 133 void abs_xy_to_rel_da(double x_abs, double y_abs,
 134                       double *d_rel, double *a_rel_rad)
 135 {
 136         double x = position_get_x_double(&mainboard.pos);
 137         double y = position_get_y_double(&mainboard.pos);
 138         double a = position_get_a_rad_double(&mainboard.pos);
 139
 140         *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
 141         if (*a_rel_rad < -M_PI) {
 142                 *a_rel_rad += M_2PI;
 143         }
 144         else if (*a_rel_rad > M_PI) {
 145                 *a_rel_rad -= M_2PI;
 146         }
 147         *d_rel = norm(x_abs-x, y_abs-y);
 148 }
 149
 150 void rotate(double *x, double *y, double rot)
 151 {
 152         double l, a;
 153
 154         l = norm(*x, *y);
 155         a = atan2(*y, *x);
 156
 157         a += rot;
 158         *x = l * cos(a);
 159         *y = l * sin(a);
 160 }
 161
 162 /* return true if the point is in area */
 163 uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
 164 {
 165         if (x < margin)
 166                 return 0;
 167         if (x > (AREA_X - margin))
 168                 return 0;
 169         if (y < margin)
 170                 return 0;
 171         if (y > (AREA_Y - margin))
 172                 return 0;
 173         return 1;
 174 }
 175
 176
 177 /* return true if the point is in area */
 178 uint8_t robot_is_in_area(int16_t margin)
 179 {
 180         return is_in_area(position_get_x_s16(&mainboard.pos),
 181                           position_get_y_s16(&mainboard.pos),
 182                           margin);
 183 }
 184
 185 /* return 1 or 0 depending on which side of a line (y=cste) is the
 186  * robot. works in yellow or blue color. */
 187 uint8_t __y_is_more_than(int16_t posy, int16_t y)
 188 {
 189         if (mainboard.our_color == I2C_COLOR_YELLOW) {
 190                 if (posy > y)
 191                         return 1;
 192                 else
 193                         return 0;
 194         }
 195         else {
 196                 if (posy < (AREA_Y-y))
 197                         return 1;
 198                 else
 199                         return 0;
 200         }
 201 }
 202
 203 /* return 1 or 0 depending on which side of a line (x=cste) is the
 204  * robot. works in yellow or blue color. */
 205 uint8_t __x_is_more_than(int16_t posx, int16_t x)
 206 {
 207         if (posx > x)
 208                 return 1;
 209         else
 210                 return 0;
 211 }
 212
 213 /* return 1 or 0 depending on which side of a line (y=cste) is the
 214  * robot. works in yellow or blue color. */
 215 uint8_t y_is_more_than(int16_t y)
 216 {
 217         int16_t posy;
 218         posy = position_get_y_s16(&mainboard.pos);
 219         return __y_is_more_than(posy, y);
 220 }
 221
 222 /* return 1 or 0 depending on which side of a line (x=cste) is the
 223  * robot. works in yellow or blue color. */
 224 uint8_t x_is_more_than(int16_t x)
 225 {
 226         int16_t posx;
 227         posx = position_get_x_s16(&mainboard.pos);
 228         return __x_is_more_than(posx, x);
 229 }
 230
 231 int16_t sin_table[] = {
 232         0,
 233         3211,
 234         6392,
 235         9512,
 236         12539,
 237         15446,
 238         18204,
 239         20787,
 240         23170,
 241         25330,
 242         27245,
 243         28898,
 244         30273,
 245         31357,
 246         32138,
 247         32610,
 248         32767,
 249 };
 250
 251 int16_t fast_sin(int16_t deg)
 252 {
 253         deg %= 360;
 254
 255         if (deg < 0)
 256                 deg += 360;
 257
 258         if (deg < 90)
 259                 return sin_table[(deg*16)/90];
 260         else if (deg < 180)
 261                 return sin_table[((180-deg)*16)/90];
 262         else if (deg < 270)
 263                 return -sin_table[((deg-180)*16)/90];
 264         else
 265                 return -sin_table[((360-deg)*16)/90];
 266 }
 267
 268 int16_t fast_cos(int16_t deg)
 269 {
 270         return fast_sin(90+deg);
 271 }
 272
 273
 274 /* get the color of our robot */
 275 uint8_t get_color(void)
 276 {
 277         return mainboard.our_color;
 278 }
 279
 280 /* get the color of the opponent robot */
 281 uint8_t get_opponent_color(void)
 282 {
 283         if (mainboard.our_color == I2C_COLOR_YELLOW)
 284                 return I2C_COLOR_BLUE;
 285         else
 286                 return I2C_COLOR_YELLOW;
 287 }
 288
 289 /* get the xy pos of the opponent robot */
 290 int8_t get_opponent_xy(int16_t *x, int16_t *y)
 291 {
 292         uint8_t flags;
 293         IRQ_LOCK(flags);
 294         *x = ballboard.opponent_x;
 295         *y = ballboard.opponent_y;
 296         IRQ_UNLOCK(flags);
 297         if (*x == I2C_OPPONENT_NOT_THERE)
 298                 return -1;
 299         return 0;
 300 }
 301
 302 /* get the da pos of the opponent robot */
 303 int8_t get_opponent_da(int16_t *d, int16_t *a)
 304 {
 305         uint8_t flags;
 306         int16_t x_tmp;
 307         IRQ_LOCK(flags);
 308         x_tmp = ballboard.opponent_x;
 309         *d = ballboard.opponent_d;
 310         *a = ballboard.opponent_a;
 311         IRQ_UNLOCK(flags);
 312         if (x_tmp == I2C_OPPONENT_NOT_THERE)
 313                 return -1;
 314         return 0;
 315 }
 316
 317 /* get the da pos of the opponent robot */
 318 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
 319 {
 320         uint8_t flags;
 321         IRQ_LOCK(flags);
 322         *x = ballboard.opponent_x;
 323         *y = ballboard.opponent_y;
 324         *d = ballboard.opponent_d;
 325         *a = ballboard.opponent_a;
 326         IRQ_UNLOCK(flags);
 327         if (*x == I2C_OPPONENT_NOT_THERE)
 328                 return -1;
 329         return 0;
 330 }
 331
 332 int16_t distance_from_opponent(int16_t x, int16_t y)
 333 {
 334         int16_t oppx, oppy;
 335         if (get_opponent_xy(&oppx, &oppy) < 0)
 336                 return -1;
 337         return distance_between(x, y, oppx, oppy);
 338 }
 339
 340 uint8_t get_ball_count(void)
 341 {
 342         return ballboard.ball_count;
 343 }
 344
 345 uint8_t get_cob_count(void)
 346 {
 347         return cobboard.cob_count;
 348 }