projects/microb2010/tests/test_board2008/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.5 2009-05-02 10:08:09 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 <uart.h>
  31 #include <pwm_ng.h>
  32 #include <time.h>
  33
  34 #include <pid.h>
  35 #include <quadramp.h>
  36 #include <control_system_manager.h>
  37 #include <trajectory_manager.h>
  38 #include <blocking_detection_manager.h>
  39 #include <robot_system.h>
  40 #include <position_manager.h>
  41
  42 #include <rdline.h>
  43 #include <parse.h>
  44
  45 #include "main.h"
  46 #include "strat_base.h"
  47 #include "strat_utils.h"
  48 #include "sensor.h"
  49
  50 /* return the distance between two points */
  51 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
  52 {
  53         int32_t x,y;
  54         x = (x2-x1);
  55         x = x*x;
  56         y = (y2-y1);
  57         y = y*y;
  58         return sqrt(x+y);
  59 }
  60
  61 /* return the distance to a point in the area */
  62 int16_t distance_from_robot(int16_t x, int16_t y)
  63 {
  64         return distance_between(position_get_x_s16(&mainboard.pos),
  65                                 position_get_y_s16(&mainboard.pos), x, y);
  66 }
  67
  68 /** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
  69 int16_t simple_modulo_360(int16_t a)
  70 {
  71         if (a < -180) {
  72                 a += 360;
  73         }
  74         else if (a > 180) {
  75                 a -= 360;
  76         }
  77         return a;
  78 }
  79
  80 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
  81 double simple_modulo_2pi(double a)
  82 {
  83         if (a < -M_PI) {
  84                 a += M_2PI;
  85         }
  86         else if (a > M_PI) {
  87                 a -= M_2PI;
  88         }
  89         return a;
  90 }
  91
  92 /* return the distance to a point in the area */
  93 int16_t angle_abs_to_rel(int16_t a_abs)
  94 {
  95         return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
  96 }
  97
  98 void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
  99                       double *x_abs, double *y_abs)
 100 {
 101         double x = position_get_x_double(&mainboard.pos);
 102         double y = position_get_y_double(&mainboard.pos);
 103         double a = position_get_a_rad_double(&mainboard.pos);
 104
 105         *x_abs = x + d_rel*cos(a+a_rel_rad);
 106         *y_abs = y + d_rel*sin(a+a_rel_rad);
 107 }
 108
 109 double norm(double x, double y)
 110 {
 111         return sqrt(x*x + y*y);
 112 }
 113
 114 void rel_xy_to_abs_xy(double x_rel, double y_rel,
 115                       double *x_abs, double *y_abs)
 116 {
 117         double d_rel, a_rel;
 118         d_rel = norm(x_rel, y_rel);
 119         a_rel = atan2(y_rel, x_rel);
 120         rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
 121 }
 122
 123 /* return an angle between -pi and pi */
 124 void abs_xy_to_rel_da(double x_abs, double y_abs,
 125                       double *d_rel, double *a_rel_rad)
 126 {
 127         double x = position_get_x_double(&mainboard.pos);
 128         double y = position_get_y_double(&mainboard.pos);
 129         double a = position_get_a_rad_double(&mainboard.pos);
 130
 131         *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
 132         if (*a_rel_rad < -M_PI) {
 133                 *a_rel_rad += M_2PI;
 134         }
 135         else if (*a_rel_rad > M_PI) {
 136                 *a_rel_rad -= M_2PI;
 137         }
 138         *d_rel = norm(x_abs-x, y_abs-y);
 139 }
 140
 141 void rotate(double *x, double *y, double rot)
 142 {
 143         double l, a;
 144
 145         l = norm(*x, *y);
 146         a = atan2(*y, *x);
 147
 148         a += rot;
 149         *x = l * cos(a);
 150         *y = l * sin(a);
 151 }
 152
 153 /* return true if the point is in area */
 154 uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
 155 {
 156         if (x < margin)
 157                 return 0;
 158         if (x > (AREA_X - margin))
 159                 return 0;
 160         if (y < margin)
 161                 return 0;
 162         if (y > (AREA_Y - margin))
 163                 return 0;
 164         return 1;
 165 }
 166
 167
 168 /* return true if the point is in area */
 169 uint8_t robot_is_in_area(int16_t margin)
 170 {
 171         return is_in_area(position_get_x_s16(&mainboard.pos),
 172                           position_get_y_s16(&mainboard.pos),
 173                           margin);
 174 }
 175
 176 /* /\* return true if we are near the disc *\/ */
 177 /* uint8_t robot_is_near_disc(void) */
 178 /* { */
 179 /*      if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) */
 180 /*              return 1; */
 181 /*      return 0; */
 182 /* } */
 183
 184 /* /\* return 1 or 0 depending on which side of a line (y=cste) is the */
 185 /*  * robot. works in red or green color. *\/ */
 186 /* uint8_t y_is_more_than(int16_t y) */
 187 /* { */
 188 /*      int16_t posy; */
 189
 190 /*      posy = position_get_y_s16(&mainboard.pos); */
 191 /*      if (mainboard.our_color == I2C_COLOR_RED) { */
 192 /*              if (posy > y) */
 193 /*                      return 1; */
 194 /*              else */
 195 /*                      return 0; */
 196 /*      } */
 197 /*      else { */
 198 /*              if (posy < (AREA_Y-y)) */
 199 /*                      return 1; */
 200 /*              else */
 201 /*                      return 0; */
 202 /*      } */
 203 /* } */
 204
 205 /* return 1 or 0 depending on which side of a line (x=cste) is the
 206  * robot. works in red or green color. */
 207 uint8_t x_is_more_than(int16_t x)
 208 {
 209         int16_t posx;
 210
 211         posx = position_get_x_s16(&mainboard.pos);
 212         if (posx > x)
 213                 return 1;
 214         else
 215                 return 0;
 216 }
 217
 218 int16_t sin_table[] = {
 219         0,
 220         3211,
 221         6392,
 222         9512,
 223         12539,
 224         15446,
 225         18204,
 226         20787,
 227         23170,
 228         25330,
 229         27245,
 230         28898,
 231         30273,
 232         31357,
 233         32138,
 234         32610,
 235         32767,
 236 };
 237
 238 int16_t fast_sin(int16_t deg)
 239 {
 240         deg %= 360;
 241
 242         if (deg < 0)
 243                 deg += 360;
 244
 245         if (deg < 90)
 246                 return sin_table[(deg*16)/90];
 247         else if (deg < 180)
 248                 return sin_table[((180-deg)*16)/90];
 249         else if (deg < 270)
 250                 return -sin_table[((deg-180)*16)/90];
 251         else
 252                 return -sin_table[((360-deg)*16)/90];
 253 }
 254
 255 int16_t fast_cos(int16_t deg)
 256 {
 257         return fast_sin(90+deg);
 258 }
 259
 260
 261 /* get the color of our robot */
 262 uint8_t get_color(void)
 263 {
 264         return mainboard.our_color;
 265 }
 266
 267 /* /\* get the color of the opponent robot *\/ */
 268 /* uint8_t get_opponent_color(void) */
 269 /* { */
 270 /*      if (mainboard.our_color == I2C_COLOR_RED) */
 271 /*              return I2C_COLOR_GREEN; */
 272 /*      else */
 273 /*              return I2C_COLOR_RED; */
 274 /* } */
 275
 276 /* /\* get the xy pos of the opponent robot *\/ */
 277 /* int8_t get_opponent_xy(int16_t *x, int16_t *y) */
 278 /* { */
 279 /*      uint8_t flags; */
 280 /*      IRQ_LOCK(flags); */
 281 /*      *x = sensorboard.opponent_x; */
 282 /*      *y = sensorboard.opponent_y; */
 283 /*      IRQ_UNLOCK(flags); */
 284 /*      if (*x == I2C_OPPONENT_NOT_THERE) */
 285 /*              return -1; */
 286 /*      return 0; */
 287 /* } */
 288
 289 /* /\* get the da pos of the opponent robot *\/ */
 290 /* int8_t get_opponent_da(int16_t *d, int16_t *a) */
 291 /* { */
 292 /*      uint8_t flags; */
 293 /*      int16_t x_tmp; */
 294 /*      IRQ_LOCK(flags); */
 295 /*      x_tmp = sensorboard.opponent_x; */
 296 /*      *d = sensorboard.opponent_d; */
 297 /*      *a = sensorboard.opponent_a; */
 298 /*      IRQ_UNLOCK(flags); */
 299 /*      if (x_tmp == I2C_OPPONENT_NOT_THERE) */
 300 /*              return -1; */
 301 /*      return 0; */
 302 /* } */
 303
 304 /* /\* get the da pos of the opponent robot *\/ */
 305 /* int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a) */
 306 /* { */
 307 /*      uint8_t flags; */
 308 /*      IRQ_LOCK(flags); */
 309 /*      *x = sensorboard.opponent_x; */
 310 /*      *y = sensorboard.opponent_y; */
 311 /*      *d = sensorboard.opponent_d; */
 312 /*      *a = sensorboard.opponent_a; */
 313 /*      IRQ_UNLOCK(flags); */
 314 /*      if (*x == I2C_OPPONENT_NOT_THERE) */
 315 /*              return -1; */
 316 /*      return 0; */
 317 /* } */
 318
 319 /* uint8_t pump_left1_is_full(void) */
 320 /* { */
 321 /*      return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && */
 322 /*                 (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); */
 323 /* } */
 324
 325 /* uint8_t pump_left2_is_full(void) */
 326 /* { */
 327 /*      return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && */
 328 /*                 (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); */
 329 /* } */
 330
 331 /* uint8_t pump_right1_is_full(void) */
 332 /* { */
 333 /*      return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && */
 334 /*                 (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); */
 335 /* } */
 336
 337 /* uint8_t pump_right2_is_full(void) */
 338 /* { */
 339 /*      return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && */
 340 /*                 (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); */
 341 /* } */
 342
 343 /* /\* number of column owned by the robot *\/ */
 344 /* uint8_t get_column_count_left(void) */
 345 /* { */
 346 /*      uint8_t ret = 0; */
 347 /*      ret += pump_left1_is_full(); */
 348 /*      ret += pump_left2_is_full(); */
 349 /*      return ret; */
 350 /* } */
 351
 352 /* /\* number of column owned by the robot *\/ */
 353 /* uint8_t get_column_count_right(void) */
 354 /* { */
 355 /*      uint8_t ret = 0; */
 356 /*      ret += pump_right1_is_full(); */
 357 /*      ret += pump_right2_is_full(); */
 358 /*      return ret; */
 359 /* } */
 360
 361 /* /\* number of column owned by the robot *\/ */
 362 /* uint8_t get_column_count(void) */
 363 /* { */
 364 /*      uint8_t ret = 0; */
 365 /*      ret += pump_left1_is_full(); */
 366 /*      ret += pump_left2_is_full(); */
 367 /*      ret += pump_right1_is_full(); */
 368 /*      ret += pump_right2_is_full(); */
 369 /*      return ret; */
 370 /* } */
 371
 372 /* uint8_t get_lintel_count(void) */
 373 /* { */
 374 /*      return mechboard.lintel_count; */
 375 /* } */
 376
 377 /* uint8_t get_mechboard_mode(void) */
 378 /* { */
 379 /*      return mechboard.mode; */
 380 /* } */
 381
 382 /* uint8_t get_scanner_status(void) */
 383 /* { */
 384 /*      return sensorboard.scan_status; */
 385 /* } */
 386
 387 /* /\* return 0 if timeout, or 1 if cond is true *\/ */
 388 /* uint8_t wait_scan_done(uint16_t timeout) */
 389 /* { */
 390 /*      uint8_t err; */
 391 /*      err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); */
 392 /*      return err; */
 393 /* } */
 394
 395 /* uint8_t opponent_is_behind(void) */
 396 /* { */
 397 /*      int8_t opp_there; */
 398 /*      int16_t opp_d, opp_a; */
 399
 400 /*      opp_there = get_opponent_da(&opp_d, &opp_a); */
 401 /*      if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) */
 402 /*              return 1; */
 403 /*      return 0; */
 404 /* } */