} while (0)
+/* return true if we need to grab some more elements */
+static uint8_t need_more_elements(void)
+{
+ if (time_get_s() <= 75) {
+ /* we have enough time left */
+ if (get_ball_count() >= 4)
+ return 0;
+ if (get_cob_count() >= 4)
+ return 0;
+ if ((get_ball_count() >= 2) &&
+ (get_cob_count() >= 2))
+ return 0;
+ return 1;
+ }
+ else {
+ /* not much time remaining */
+ if ((get_ball_count() >= 1) &&
+ (get_cob_count() >= 1))
+ return 0;
+ return 1;
+ }
+}
+
+static uint8_t strat_harvest(void)
+{
+ return 0;
+}
+
+static uint8_t strat_eject(void)
+{
+ return 0;
+}
+
uint8_t strat_main(void)
{
uint8_t err;
- /* */
+ /* harvest the first cobs + balls */
err = strat_beginning();
+ while (1) {
+ /* end of time exit ! */
+ if (err == END_TIMER) {
+ DEBUG(E_USER_STRAT, "End of time");
+ strat_exit();
+ break;
+ }
+
+ if (need_more_elements() == 0) {
+ /* we have enough elements, go to eject */
+ err = strat_eject();
+ if (!TRAJ_SUCCESS(err))
+ continue;
+ }
+ else {
+ /* harvest */
+ err = strat_harvest();
+ if (!TRAJ_SUCCESS(err))
+ continue;
+ }
+ }
+
return err;
}
-/*
+/*
* 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
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 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);
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_YELLOW) {
if (posy > y)
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];
/* return 1; */
return 0;
}
+
+uint8_t get_ball_count(void)
+{
+ return ballboard.ball_count;
+}
+
+uint8_t get_cob_count(void)
+{
+ return cobboard.cob_count;
+}