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