-/*
+/*
* Copyright Droids Corporation, Microb Technology (2009)
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <pid.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
position_get_y_s16(&mainboard.pos), x, y);
}
-/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
+/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
int16_t simple_modulo_360(int16_t a)
{
if (a < -180) {
return a;
}
-/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
-double simple_modulo_2pi(double a)
-{
- if (a < -M_PI) {
- a += M_2PI;
- }
- else if (a > M_PI) {
- a -= M_2PI;
- }
- return a;
-}
-
/* return the distance to a point in the area */
int16_t angle_abs_to_rel(int16_t a_abs)
{
return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
}
-void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
+void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
double *x_abs, double *y_abs)
{
- double x = position_get_x_double(&mainboard.pos);
+ double x = position_get_x_double(&mainboard.pos);
double y = position_get_y_double(&mainboard.pos);
double a = position_get_a_rad_double(&mainboard.pos);
return sqrt(x*x + y*y);
}
-void rel_xy_to_abs_xy(double x_rel, double y_rel,
+void rel_xy_to_abs_xy(double x_rel, double y_rel,
double *x_abs, double *y_abs)
{
double d_rel, a_rel;
}
/* return an angle between -pi and pi */
-void abs_xy_to_rel_da(double x_abs, double y_abs,
+void abs_xy_to_rel_da(double x_abs, double y_abs,
double *d_rel, double *a_rel_rad)
{
- double x = position_get_x_double(&mainboard.pos);
+ double x = position_get_x_double(&mainboard.pos);
double y = position_get_y_double(&mainboard.pos);
double a = position_get_a_rad_double(&mainboard.pos);
-
+
*a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
if (*a_rel_rad < -M_PI) {
*a_rel_rad += M_2PI;
void rotate(double *x, double *y, double rot)
{
double l, a;
-
+
l = norm(*x, *y);
a = atan2(*y, *x);
margin);
}
-/* return true if we are near the disc */
-uint8_t robot_is_near_disc(void)
-{
- if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG)
- return 1;
- return 0;
-}
-
/* return 1 or 0 depending on which side of a line (y=cste) is the
- * robot. works in red or green color. */
+ * robot. works in yellow or blue color. */
uint8_t y_is_more_than(int16_t y)
{
int16_t posy;
-
+
posy = position_get_y_s16(&mainboard.pos);
- if (mainboard.our_color == I2C_COLOR_RED) {
+ if (mainboard.our_color == I2C_COLOR_YELLOW) {
if (posy > y)
return 1;
else
}
/* return 1 or 0 depending on which side of a line (x=cste) is the
- * robot. works in red or green color. */
+ * robot. works in yellow or blue color. */
uint8_t x_is_more_than(int16_t x)
{
int16_t posx;
-
+
posx = position_get_x_s16(&mainboard.pos);
if (posx > x)
return 1;
int16_t fast_sin(int16_t deg)
{
deg %= 360;
-
+
if (deg < 0)
deg += 360;
- if (deg < 90)
+ if (deg < 90)
return sin_table[(deg*16)/90];
- else if (deg < 180)
+ else if (deg < 180)
return sin_table[((180-deg)*16)/90];
- else if (deg < 270)
+ else if (deg < 270)
return -sin_table[((deg-180)*16)/90];
else
return -sin_table[((360-deg)*16)/90];
/* get the color of the opponent robot */
uint8_t get_opponent_color(void)
{
- if (mainboard.our_color == I2C_COLOR_RED)
- return I2C_COLOR_GREEN;
+ if (mainboard.our_color == I2C_COLOR_YELLOW)
+ return I2C_COLOR_BLUE;
else
- return I2C_COLOR_RED;
+ return I2C_COLOR_YELLOW;
}
-/* get the xy pos of the opponent robot */
-int8_t get_opponent_xy(int16_t *x, int16_t *y)
+/* get the da pos of the opponent robot */
+int8_t get_opponent_da(int16_t *d, int16_t *a)
{
uint8_t flags;
+ int16_t x;
+
IRQ_LOCK(flags);
- *x = sensorboard.opponent_x;
- *y = sensorboard.opponent_y;
+ *d = beaconboard.oppd;
+ *a = beaconboard.oppa;
+ x = beaconboard.oppx;
IRQ_UNLOCK(flags);
- if (*x == I2C_OPPONENT_NOT_THERE)
+ if (x == I2C_OPPONENT_NOT_THERE)
return -1;
return 0;
}
-/* get the da pos of the opponent robot */
-int8_t get_opponent_da(int16_t *d, int16_t *a)
+/* get the xy pos of the opponent robot */
+int8_t get_opponent_xy(int16_t *x, int16_t *y)
{
uint8_t flags;
- int16_t x_tmp;
+
IRQ_LOCK(flags);
- x_tmp = sensorboard.opponent_x;
- *d = sensorboard.opponent_d;
- *a = sensorboard.opponent_a;
+ *x = beaconboard.oppx;
+ *y = beaconboard.oppy;
IRQ_UNLOCK(flags);
- if (x_tmp == I2C_OPPONENT_NOT_THERE)
+ if (*x == I2C_OPPONENT_NOT_THERE)
return -1;
+
return 0;
}
int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
{
uint8_t flags;
+
IRQ_LOCK(flags);
- *x = sensorboard.opponent_x;
- *y = sensorboard.opponent_y;
- *d = sensorboard.opponent_d;
- *a = sensorboard.opponent_a;
+ *x = beaconboard.oppx;
+ *y = beaconboard.oppy;
+ *d = beaconboard.oppd;
+ *a = beaconboard.oppa;
IRQ_UNLOCK(flags);
if (*x == I2C_OPPONENT_NOT_THERE)
return -1;
- return 0;
-}
-
-uint8_t pump_left1_is_full(void)
-{
- return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) &&
- (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN));
-}
-
-uint8_t pump_left2_is_full(void)
-{
- return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) &&
- (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN));
-}
-
-uint8_t pump_right1_is_full(void)
-{
- return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) &&
- (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN));
-}
-
-uint8_t pump_right2_is_full(void)
-{
- return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) &&
- (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN));
-}
-
-/* number of column owned by the robot */
-uint8_t get_column_count_left(void)
-{
- uint8_t ret = 0;
- ret += pump_left1_is_full();
- ret += pump_left2_is_full();
- return ret;
-}
-/* number of column owned by the robot */
-uint8_t get_column_count_right(void)
-{
- uint8_t ret = 0;
- ret += pump_right1_is_full();
- ret += pump_right2_is_full();
- return ret;
-}
-
-/* number of column owned by the robot */
-uint8_t get_column_count(void)
-{
- uint8_t ret = 0;
- ret += pump_left1_is_full();
- ret += pump_left2_is_full();
- ret += pump_right1_is_full();
- ret += pump_right2_is_full();
- return ret;
-}
-
-uint8_t get_lintel_count(void)
-{
- return mechboard.lintel_count;
-}
-
-uint8_t get_mechboard_mode(void)
-{
- return mechboard.mode;
+ return 0;
}
-uint8_t get_scanner_status(void)
+int16_t distance_from_opponent(int16_t x, int16_t y)
{
- return sensorboard.scan_status;
+ int16_t oppx, oppy;
+ if (get_opponent_xy(&oppx, &oppy) < 0)
+ return -1;
+ return distance_between(x, y, oppx, oppy);
}
-/* return 0 if timeout, or 1 if cond is true */
-uint8_t wait_scan_done(uint16_t timeout)
+uint8_t get_ball_count(void)
{
- uint8_t err;
- err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout);
- return err;
+ return ballboard.ball_count;
}
-uint8_t opponent_is_behind(void)
+uint8_t get_cob_count(void)
{
- int8_t opp_there;
- int16_t opp_d, opp_a;
-
- opp_there = get_opponent_da(&opp_d, &opp_a);
- if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600)
- return 1;
- return 0;
+ return cobboard.cob_count;
}
git.droids-corp.org / aversive.git / blobdiff