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 to a point in the area */
  72 int16_t distance_from_robot(int16_t x, int16_t y)
  73 {
  74         return distance_between(position_get_x_s16(&mainboard.pos),
  75                                 position_get_y_s16(&mainboard.pos), x, y);
  76 }
  77
  78 /** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
  79 int16_t simple_modulo_360(int16_t a)
  80 {
  81         if (a < -180) {
  82                 a += 360;
  83         }
  84         else if (a > 180) {
  85                 a -= 360;
  86         }
  87         return a;
  88 }
  89
  90 /* return the distance to a point in the area */
  91 int16_t angle_abs_to_rel(int16_t a_abs)
  92 {
  93         return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
  94 }
  95
  96 void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
  97                       double *x_abs, double *y_abs)
  98 {
  99         double x = position_get_x_double(&mainboard.pos);
 100         double y = position_get_y_double(&mainboard.pos);
 101         double a = position_get_a_rad_double(&mainboard.pos);
 102
 103         *x_abs = x + d_rel*cos(a+a_rel_rad);
 104         *y_abs = y + d_rel*sin(a+a_rel_rad);
 105 }
 106
 107 double norm(double x, double y)
 108 {
 109         return sqrt(x*x + y*y);
 110 }
 111
 112 void rel_xy_to_abs_xy(double x_rel, double y_rel,
 113                       double *x_abs, double *y_abs)
 114 {
 115         double d_rel, a_rel;
 116         d_rel = norm(x_rel, y_rel);
 117         a_rel = atan2(y_rel, x_rel);
 118         rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
 119 }
 120
 121 /* return an angle between -pi and pi */
 122 void abs_xy_to_rel_da(double x_abs, double y_abs,
 123                       double *d_rel, double *a_rel_rad)
 124 {
 125         double x = position_get_x_double(&mainboard.pos);
 126         double y = position_get_y_double(&mainboard.pos);
 127         double a = position_get_a_rad_double(&mainboard.pos);
 128
 129         *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
 130         if (*a_rel_rad < -M_PI) {
 131                 *a_rel_rad += M_2PI;
 132         }
 133         else if (*a_rel_rad > M_PI) {
 134                 *a_rel_rad -= M_2PI;
 135         }
 136         *d_rel = norm(x_abs-x, y_abs-y);
 137 }
 138
 139 void rotate(double *x, double *y, double rot)
 140 {
 141         double l, a;
 142
 143         l = norm(*x, *y);
 144         a = atan2(*y, *x);
 145
 146         a += rot;
 147         *x = l * cos(a);
 148         *y = l * sin(a);
 149 }
 150
 151 /* return true if the point is in area */
 152 uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
 153 {
 154         if (x < margin)
 155                 return 0;
 156         if (x > (AREA_X - margin))
 157                 return 0;
 158         if (y < margin)
 159                 return 0;
 160         if (y > (AREA_Y - margin))
 161                 return 0;
 162         return 1;
 163 }
 164
 165
 166 /* return true if the point is in area */
 167 uint8_t robot_is_in_area(int16_t margin)
 168 {
 169         return is_in_area(position_get_x_s16(&mainboard.pos),
 170                           position_get_y_s16(&mainboard.pos),
 171                           margin);
 172 }
 173
 174 /* return 1 or 0 depending on which side of a line (y=cste) is the
 175  * robot. works in yellow or blue color. */
 176 uint8_t y_is_more_than(int16_t y)
 177 {
 178         int16_t posy;
 179
 180         posy = position_get_y_s16(&mainboard.pos);
 181         if (mainboard.our_color == I2C_COLOR_YELLOW) {
 182                 if (posy > y)
 183                         return 1;
 184                 else
 185                         return 0;
 186         }
 187         else {
 188                 if (posy < (AREA_Y-y))
 189                         return 1;
 190                 else
 191                         return 0;
 192         }
 193 }
 194
 195 /* return 1 or 0 depending on which side of a line (x=cste) is the
 196  * robot. works in yellow or blue color. */
 197 uint8_t x_is_more_than(int16_t x)
 198 {
 199         int16_t posx;
 200
 201         posx = position_get_x_s16(&mainboard.pos);
 202         if (posx > x)
 203                 return 1;
 204         else
 205                 return 0;
 206 }
 207
 208 int16_t sin_table[] = {
 209         0,
 210         3211,
 211         6392,
 212         9512,
 213         12539,
 214         15446,
 215         18204,
 216         20787,
 217         23170,
 218         25330,
 219         27245,
 220         28898,
 221         30273,
 222         31357,
 223         32138,
 224         32610,
 225         32767,
 226 };
 227
 228 int16_t fast_sin(int16_t deg)
 229 {
 230         deg %= 360;
 231
 232         if (deg < 0)
 233                 deg += 360;
 234
 235         if (deg < 90)
 236                 return sin_table[(deg*16)/90];
 237         else if (deg < 180)
 238                 return sin_table[((180-deg)*16)/90];
 239         else if (deg < 270)
 240                 return -sin_table[((deg-180)*16)/90];
 241         else
 242                 return -sin_table[((360-deg)*16)/90];
 243 }
 244
 245 int16_t fast_cos(int16_t deg)
 246 {
 247         return fast_sin(90+deg);
 248 }
 249
 250
 251 /* get the color of our robot */
 252 uint8_t get_color(void)
 253 {
 254         return mainboard.our_color;
 255 }
 256
 257 /* get the color of the opponent robot */
 258 uint8_t get_opponent_color(void)
 259 {
 260         if (mainboard.our_color == I2C_COLOR_YELLOW)
 261                 return I2C_COLOR_BLUE;
 262         else
 263                 return I2C_COLOR_YELLOW;
 264 }
 265
 266 /* get the da pos of the opponent robot */
 267 int8_t get_opponent_da(int16_t *d, int16_t *a)
 268 {
 269         uint8_t flags;
 270         int16_t x;
 271
 272         IRQ_LOCK(flags);
 273         *d = beaconboard.oppd;
 274         *a = beaconboard.oppa;
 275         x = beaconboard.oppx;
 276         IRQ_UNLOCK(flags);
 277         if (x == I2C_OPPONENT_NOT_THERE)
 278                 return -1;
 279         return 0;
 280 }
 281
 282 /* get the xy pos of the opponent robot */
 283 int8_t get_opponent_xy(int16_t *x, int16_t *y)
 284 {
 285         uint8_t flags;
 286
 287         IRQ_LOCK(flags);
 288         *x = beaconboard.oppx;
 289         *y = beaconboard.oppy;
 290         IRQ_UNLOCK(flags);
 291         if (*x == I2C_OPPONENT_NOT_THERE)
 292                 return -1;
 293
 294         return 0;
 295 }
 296
 297 /* get the da pos of the opponent robot */
 298 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
 299 {
 300         uint8_t flags;
 301
 302         IRQ_LOCK(flags);
 303         *x = beaconboard.oppx;
 304         *y = beaconboard.oppy;
 305         *d = beaconboard.oppd;
 306         *a = beaconboard.oppa;
 307         IRQ_UNLOCK(flags);
 308         if (*x == I2C_OPPONENT_NOT_THERE)
 309                 return -1;
 310
 311         return 0;
 312 }
 313
 314 int16_t distance_from_opponent(int16_t x, int16_t y)
 315 {
 316         int16_t oppx, oppy;
 317         if (get_opponent_xy(&oppx, &oppy) < 0)
 318                 return -1;
 319         return distance_between(x, y, oppx, oppy);
 320 }
 321
 322 uint8_t get_ball_count(void)
 323 {
 324         return ballboard.ball_count;
 325 }
 326
 327 uint8_t get_cob_count(void)
 328 {
 329         return cobboard.cob_count;
 330 }