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