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 <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 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
  90 double simple_modulo_2pi(double a)
  91 {
  92         if (a < -M_PI) {
  93                 a += M_2PI;
  94         }
  95         else if (a > M_PI) {
  96                 a -= M_2PI;
  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 true if we are near the disc */
 186 uint8_t robot_is_near_disc(void)
 187 {
 188         if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG)
 189                 return 1;
 190         return 0;
 191 }
 192
 193 /* return 1 or 0 depending on which side of a line (y=cste) is the
 194  * robot. works in red or green color. */
 195 uint8_t y_is_more_than(int16_t y)
 196 {
 197         int16_t posy;
 198
 199         posy = position_get_y_s16(&mainboard.pos);
 200         if (mainboard.our_color == I2C_COLOR_RED) {
 201                 if (posy > y)
 202                         return 1;
 203                 else
 204                         return 0;
 205         }
 206         else {
 207                 if (posy < (AREA_Y-y))
 208                         return 1;
 209                 else
 210                         return 0;
 211         }
 212 }
 213
 214 /* return 1 or 0 depending on which side of a line (x=cste) is the
 215  * robot. works in red or green color. */
 216 uint8_t x_is_more_than(int16_t x)
 217 {
 218         int16_t posx;
 219
 220         posx = position_get_x_s16(&mainboard.pos);
 221         if (posx > x)
 222                 return 1;
 223         else
 224                 return 0;
 225 }
 226
 227 int16_t sin_table[] = {
 228         0,
 229         3211,
 230         6392,
 231         9512,
 232         12539,
 233         15446,
 234         18204,
 235         20787,
 236         23170,
 237         25330,
 238         27245,
 239         28898,
 240         30273,
 241         31357,
 242         32138,
 243         32610,
 244         32767,
 245 };
 246
 247 int16_t fast_sin(int16_t deg)
 248 {
 249         deg %= 360;
 250
 251         if (deg < 0)
 252                 deg += 360;
 253
 254         if (deg < 90)
 255                 return sin_table[(deg*16)/90];
 256         else if (deg < 180)
 257                 return sin_table[((180-deg)*16)/90];
 258         else if (deg < 270)
 259                 return -sin_table[((deg-180)*16)/90];
 260         else
 261                 return -sin_table[((360-deg)*16)/90];
 262 }
 263
 264 int16_t fast_cos(int16_t deg)
 265 {
 266         return fast_sin(90+deg);
 267 }
 268
 269
 270 /* get the color of our robot */
 271 uint8_t get_color(void)
 272 {
 273         return mainboard.our_color;
 274 }
 275
 276 /* get the color of the opponent robot */
 277 uint8_t get_opponent_color(void)
 278 {
 279         if (mainboard.our_color == I2C_COLOR_RED)
 280                 return I2C_COLOR_GREEN;
 281         else
 282                 return I2C_COLOR_RED;
 283 }
 284
 285 /* get the xy pos of the opponent robot */
 286 int8_t get_opponent_xy(int16_t *x, int16_t *y)
 287 {
 288         uint8_t flags;
 289         IRQ_LOCK(flags);
 290         *x = sensorboard.opponent_x;
 291         *y = sensorboard.opponent_y;
 292         IRQ_UNLOCK(flags);
 293         if (*x == I2C_OPPONENT_NOT_THERE)
 294                 return -1;
 295         return 0;
 296 }
 297
 298 /* get the da pos of the opponent robot */
 299 int8_t get_opponent_da(int16_t *d, int16_t *a)
 300 {
 301         uint8_t flags;
 302         int16_t x_tmp;
 303         IRQ_LOCK(flags);
 304         x_tmp = sensorboard.opponent_x;
 305         *d = sensorboard.opponent_d;
 306         *a = sensorboard.opponent_a;
 307         IRQ_UNLOCK(flags);
 308         if (x_tmp == I2C_OPPONENT_NOT_THERE)
 309                 return -1;
 310         return 0;
 311 }
 312
 313 /* get the da pos of the opponent robot */
 314 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
 315 {
 316         uint8_t flags;
 317         IRQ_LOCK(flags);
 318         *x = sensorboard.opponent_x;
 319         *y = sensorboard.opponent_y;
 320         *d = sensorboard.opponent_d;
 321         *a = sensorboard.opponent_a;
 322         IRQ_UNLOCK(flags);
 323         if (*x == I2C_OPPONENT_NOT_THERE)
 324                 return -1;
 325         return 0;
 326 }
 327
 328 uint8_t pump_left1_is_full(void)
 329 {
 330         return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) &&
 331                    (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN));
 332 }
 333
 334 uint8_t pump_left2_is_full(void)
 335 {
 336         return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) &&
 337                    (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN));
 338 }
 339
 340 uint8_t pump_right1_is_full(void)
 341 {
 342         return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) &&
 343                    (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN));
 344 }
 345
 346 uint8_t pump_right2_is_full(void)
 347 {
 348         return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) &&
 349                    (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN));
 350 }
 351
 352 /* number of column owned by the robot */
 353 uint8_t get_column_count_left(void)
 354 {
 355         uint8_t ret = 0;
 356         ret += pump_left1_is_full();
 357         ret += pump_left2_is_full();
 358         return ret;
 359 }
 360
 361 /* number of column owned by the robot */
 362 uint8_t get_column_count_right(void)
 363 {
 364         uint8_t ret = 0;
 365         ret += pump_right1_is_full();
 366         ret += pump_right2_is_full();
 367         return ret;
 368 }
 369
 370 /* number of column owned by the robot */
 371 uint8_t get_column_count(void)
 372 {
 373         uint8_t ret = 0;
 374         ret += pump_left1_is_full();
 375         ret += pump_left2_is_full();
 376         ret += pump_right1_is_full();
 377         ret += pump_right2_is_full();
 378         return ret;
 379 }
 380
 381 uint8_t get_lintel_count(void)
 382 {
 383         return mechboard.lintel_count;
 384 }
 385
 386 uint8_t get_mechboard_mode(void)
 387 {
 388         return mechboard.mode;
 389 }
 390
 391 uint8_t get_scanner_status(void)
 392 {
 393         return sensorboard.scan_status;
 394 }
 395
 396 /* return 0 if timeout, or 1 if cond is true */
 397 uint8_t wait_scan_done(uint16_t timeout)
 398 {
 399         uint8_t err;
 400         err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout);
 401         return err;
 402 }
 403
 404 uint8_t opponent_is_behind(void)
 405 {
 406         int8_t opp_there;
 407         int16_t opp_d, opp_a;
 408
 409         opp_there = get_opponent_da(&opp_d, &opp_a);
 410         if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600)
 411                 return 1;
 412         return 0;
 413 }