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 y)
 188 {
 189         int16_t posy;
 190
 191         posy = position_get_y_s16(&mainboard.pos);
 192         if (mainboard.our_color == I2C_COLOR_YELLOW) {
 193                 if (posy > y)
 194                         return 1;
 195                 else
 196                         return 0;
 197         }
 198         else {
 199                 if (posy < (AREA_Y-y))
 200                         return 1;
 201                 else
 202                         return 0;
 203         }
 204 }
 205
 206 /* return 1 or 0 depending on which side of a line (x=cste) is the
 207  * robot. works in yellow or blue color. */
 208 uint8_t x_is_more_than(int16_t x)
 209 {
 210         int16_t posx;
 211
 212         posx = position_get_x_s16(&mainboard.pos);
 213         if (posx > x)
 214                 return 1;
 215         else
 216                 return 0;
 217 }
 218
 219 int16_t sin_table[] = {
 220         0,
 221         3211,
 222         6392,
 223         9512,
 224         12539,
 225         15446,
 226         18204,
 227         20787,
 228         23170,
 229         25330,
 230         27245,
 231         28898,
 232         30273,
 233         31357,
 234         32138,
 235         32610,
 236         32767,
 237 };
 238
 239 int16_t fast_sin(int16_t deg)
 240 {
 241         deg %= 360;
 242
 243         if (deg < 0)
 244                 deg += 360;
 245
 246         if (deg < 90)
 247                 return sin_table[(deg*16)/90];
 248         else if (deg < 180)
 249                 return sin_table[((180-deg)*16)/90];
 250         else if (deg < 270)
 251                 return -sin_table[((deg-180)*16)/90];
 252         else
 253                 return -sin_table[((360-deg)*16)/90];
 254 }
 255
 256 int16_t fast_cos(int16_t deg)
 257 {
 258         return fast_sin(90+deg);
 259 }
 260
 261
 262 /* get the color of our robot */
 263 uint8_t get_color(void)
 264 {
 265         return mainboard.our_color;
 266 }
 267
 268 /* get the color of the opponent robot */
 269 uint8_t get_opponent_color(void)
 270 {
 271         if (mainboard.our_color == I2C_COLOR_YELLOW)
 272                 return I2C_COLOR_BLUE;
 273         else
 274                 return I2C_COLOR_YELLOW;
 275 }
 276
 277 /* get the da pos of the opponent robot */
 278 int8_t get_opponent_da(int16_t *d, int16_t *a)
 279 {
 280         uint8_t flags;
 281         int16_t x;
 282
 283         IRQ_LOCK(flags);
 284         *d = beaconboard.oppd;
 285         *a = beaconboard.oppa;
 286         x = beaconboard.oppx;
 287         IRQ_UNLOCK(flags);
 288         if (x == I2C_OPPONENT_NOT_THERE)
 289                 return -1;
 290         return 0;
 291 }
 292
 293 /* get the xy pos of the opponent robot */
 294 int8_t get_opponent_xy(int16_t *x, int16_t *y)
 295 {
 296         uint8_t flags;
 297
 298         IRQ_LOCK(flags);
 299         *x = beaconboard.oppx;
 300         *y = beaconboard.oppy;
 301         IRQ_UNLOCK(flags);
 302         if (*x == I2C_OPPONENT_NOT_THERE)
 303                 return -1;
 304
 305         return 0;
 306 }
 307
 308 /* get the da pos of the opponent robot */
 309 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
 310 {
 311         uint8_t flags;
 312
 313         IRQ_LOCK(flags);
 314         *x = beaconboard.oppx;
 315         *y = beaconboard.oppy;
 316         *d = beaconboard.oppd;
 317         *a = beaconboard.oppa;
 318         IRQ_UNLOCK(flags);
 319         if (*x == I2C_OPPONENT_NOT_THERE)
 320                 return -1;
 321
 322         return 0;
 323 }
 324
 325 int16_t distance_from_opponent(int16_t x, int16_t y)
 326 {
 327         int16_t oppx, oppy;
 328         if (get_opponent_xy(&oppx, &oppy) < 0)
 329                 return -1;
 330         return distance_between(x, y, oppx, oppy);
 331 }
 332
 333 uint8_t get_ball_count(void)
 334 {
 335         return ballboard.ball_count;
 336 }
 337
 338 uint8_t get_cob_count(void)
 339 {
 340         return cobboard.cob_count;
 341 }