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