extern parse_pgm_inst_t cmd_clitoid;
extern parse_pgm_inst_t cmd_time_monitor;
extern parse_pgm_inst_t cmd_strat_event;
-extern parse_pgm_inst_t cmd_climb;
extern parse_pgm_inst_t cmd_sleep;
extern parse_pgm_inst_t cmd_test;
(parse_pgm_inst_t *)&cmd_clitoid,
(parse_pgm_inst_t *)&cmd_time_monitor,
(parse_pgm_inst_t *)&cmd_strat_event,
- (parse_pgm_inst_t *)&cmd_climb,
(parse_pgm_inst_t *)&cmd_sleep,
(parse_pgm_inst_t *)&cmd_test,
NULL,
},
};
-
-
-
-
-/**********************************************************/
-/* Climb */
-
-/* this structure is filled when cmd_climb is parsed successfully */
-struct cmd_climb_result {
- fixed_string_t arg0;
- int32_t radius;
- int32_t dist;
-};
-
-
-//#define DIM_BALLS_DOWN 620
-//#define DIM_BALLS_UP 700
-
-#define DIM_BALLS_DOWN 530
-#define DIM_BALLS_UP 700
-
-/* function called when cmd_climb is parsed successfully */
-static void cmd_climb_parsed(void *parsed_result, void *data)
-{
- uint8_t err;
-
- printf_P(PSTR("starting Clifenger\r\n"));
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW );
-
- trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- trajectory_a_abs(&mainboard.traj, 0);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
-
-
- trajectory_goto_xy_abs(&mainboard.traj, 1250, 250);
- err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF);
- printf_P(PSTR("ball_pack\r\n"));
- support_balls_pack();
- err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF);
- printf_P(PSTR("ball_deploy\r\n"));
- support_balls_deploy();
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- /* reach top, go down */
- trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
- err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_UP, 0xFF);
- printf_P(PSTR("ball_pack\r\n"));
- support_balls_pack();
- err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_DOWN, 0xFF);
- printf_P(PSTR("ball_deploy\r\n"));
- support_balls_deploy();
-
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-}
-
-prog_char str_climb_arg0[] = "climb";
-parse_pgm_token_string_t cmd_climb_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_climb_result, arg0, str_climb_arg0);
-parse_pgm_token_num_t cmd_climb_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, radius, INT32);
-parse_pgm_token_num_t cmd_climb_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, dist, INT32);
-
-prog_char help_climb[] = "Climb function";
-parse_pgm_inst_t cmd_climb = {
- .f = cmd_climb_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_climb,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_climb_arg0,
- NULL,
- },
-};
else
on = 0;
-#if 0
- if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
- bit = STRAT_CONF_ONLY_ONE_ON_DISC;
- else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
- bit = STRAT_CONF_BYPASS_STATIC2;
- else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
- bit = STRAT_CONF_TAKE_ONE_LINTEL;
- else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
- bit = STRAT_CONF_TAKE_ONE_LINTEL;
- else if (!strcmp_P(res->arg1, PSTR("store_static2")))
- bit = STRAT_CONF_STORE_STATIC2;
- else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
- bit = STRAT_CONF_BIG_3_TEMPLE;
- else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
- bit = STRAT_CONF_EARLY_SCAN;
- else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
- bit = STRAT_CONF_PUSH_OPP_COLS;
-#endif
+ if (!strcmp_P(res->arg1, PSTR("our_orange")))
+ bit = STRAT_CONF_OUR_ORANGE;
if (on)
strat_conf.flags |= bit;
prog_char str_strat_conf2_arg0[] = "strat_conf";
parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
-prog_char str_strat_conf2_arg1[] = "faux";
+prog_char str_strat_conf2_arg1[] = "our_orange";
parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
prog_char str_strat_conf2_arg2[] = "on#off";
parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
/* function called when cmd_strat_conf3 is parsed successfully */
static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
{
-#if 0
struct cmd_strat_conf3_result *res = parsed_result;
- if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
- if (res->arg2 > 90)
- res->arg2 = 90;
- strat_conf.scan_opp_min_time = res->arg2;
- }
- else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
+ if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) {
if (res->arg2 > 90)
res->arg2 = 90;
- strat_conf.delay_between_opp_scan = res->arg2;
+ strat_conf.orphan_tomato = res->arg2;
}
- else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
+ else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) {
if (res->arg2 > 90)
res->arg2 = 90;
- strat_conf.scan_our_min_time = res->arg2;
+ strat_conf.opp_orange = res->arg2;
}
- else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
- if (res->arg2 > 90)
- res->arg2 = 90;
- strat_conf.delay_between_our_scan = res->arg2;
- }
- else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
- strat_conf.wait_opponent = res->arg2;
- }
- else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
- strat_conf.lintel_min_time = res->arg2;
- }
-#endif
strat_conf.dump_enabled = 1;
strat_conf_dump(__FUNCTION__);
}
prog_char str_strat_conf3_arg0[] = "strat_conf";
parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
-prog_char str_strat_conf3_arg1[] = "faux2";
+prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange";
parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
/**********************************************************/
/* Subtraj */
-//////////////////////
-
-// 500 -- 5
-// 400 -- 3
-#define TEST_SPEED 400
-#define TEST_ACC 3
-
-static void line2line(double line1x1, double line1y1,
- double line1x2, double line1y2,
- double line2x1, double line2y1,
- double line2x2, double line2y2,
- double radius, double dist)
-{
- uint8_t err;
- double speed_d, speed_a;
- double distance, angle;
- double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
- double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
-
- printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
- strat_set_speed(TEST_SPEED, TEST_SPEED);
- quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
- circle_get_da_speed_from_radius(&mainboard.traj, radius,
- &speed_d, &speed_a);
- trajectory_line_abs(&mainboard.traj,
- line1x1, line1y1,
- line1x2, line1y2, 150.);
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
- dist, TRAJ_FLAGS_NO_NEAR);
- /* circle */
- strat_set_speed(speed_d, speed_a);
- angle = line2_angle - line1_angle;
- distance = angle * radius;
- if (distance < 0)
- distance = -distance;
- angle = simple_modulo_2pi(angle);
- angle = DEG(angle);
- printf_P(PSTR("(%d,%d,%d) "),
- position_get_x_s16(&mainboard.pos),
- position_get_y_s16(&mainboard.pos),
- position_get_a_deg_s16(&mainboard.pos));
- printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
- distance, angle);
-
- /* take some margin on dist to avoid deceleration */
- trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
-
- /* circle exit condition */
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_NO_NEAR);
-
- strat_set_speed(500, 500);
- printf_P(PSTR("(%d,%d,%d) "),
- position_get_x_s16(&mainboard.pos),
- position_get_y_s16(&mainboard.pos),
- position_get_a_deg_s16(&mainboard.pos));
- printf_P(PSTR("line\r\n"));
- trajectory_line_abs(&mainboard.traj,
- line2x1, line2y1,
- line2x2, line2y2, 150.);
-}
-
-static void halfturn(double line1x1, double line1y1,
- double line1x2, double line1y2,
- double line2x1, double line2y1,
- double line2x2, double line2y2,
- double radius, double dist, double dir)
-{
- uint8_t err;
- double speed_d, speed_a;
- double distance, angle;
-
- printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
- strat_set_speed(TEST_SPEED, TEST_SPEED);
- quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
- circle_get_da_speed_from_radius(&mainboard.traj, radius,
- &speed_d, &speed_a);
- trajectory_line_abs(&mainboard.traj,
- line1x1, line1y1,
- line1x2, line1y2, 150.);
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
- dist, TRAJ_FLAGS_NO_NEAR);
- /* circle */
- strat_set_speed(speed_d, speed_a);
- angle = dir * M_PI/2.;
- distance = angle * radius;
- if (distance < 0)
- distance = -distance;
- angle = simple_modulo_2pi(angle);
- angle = DEG(angle);
-
- /* take some margin on dist to avoid deceleration */
- DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
- distance, angle);
- trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
- /* circle exit condition */
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_NO_NEAR);
-
- DEBUG(E_USER_STRAT, "miniline");
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
- dist, TRAJ_FLAGS_NO_NEAR);
- DEBUG(E_USER_STRAT, "circle2");
- /* take some margin on dist to avoid deceleration */
- trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_NO_NEAR);
-
- strat_set_speed(500, 500);
- DEBUG(E_USER_STRAT, "line");
- trajectory_line_abs(&mainboard.traj,
- line2x1, line2y1,
- line2x2, line2y2, 150.);
-}
-
/* function called when cmd_test is parsed successfully */
static void subtraj_test(void)
{
-#ifdef HOST_VERSION
- strat_reset_pos(400, 400, 90);
- mainboard.angle.on = 1;
- mainboard.distance.on = 1;
-#endif
- printf_P(PSTR("%s()\r\n"), __FUNCTION__);
- while (!cmdline_keypressed()) {
- /****** PASS1 */
-
-#define DIST_HARD_TURN 260
-#define RADIUS_HARD_TURN 100
-#define DIST_EASY_TURN 190
-#define RADIUS_EASY_TURN 190
-#define DIST_HALF_TURN 225
-#define RADIUS_HALF_TURN 130
-
- /* hard turn */
- line2line(375, 597, 375, 1847,
- 375, 1847, 1050, 1472,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* easy left and easy right !*/
- line2line(825, 1596, 1050, 1472,
- 1050, 1472, 1500, 1722,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(1050, 1472, 1500, 1722,
- 1500, 1722, 2175, 1347,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(1500, 1722, 2175, 1347,
- 2175, 1347, 2175, 847,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* half turns */
- halfturn(2175, 1347, 2175, 722,
- 2625, 722, 2625, 1597,
- RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
- halfturn(2625, 847, 2625, 1722,
- 2175, 1722, 2175, 1097,
- RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
-
- /* easy turns */
- line2line(2175, 1597, 2175, 1097,
- 2175, 1097, 1500, 722,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(2175, 1097, 1500, 722,
- 1500, 722, 1050, 972,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(1500, 722, 1050, 972,
- 1050, 972, 375, 597,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* hard turn */
- line2line(1050, 972, 375, 597,
- 375, 597, 375, 1097,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /****** PASS2 */
-
- /* easy turn */
- line2line(375, 597, 375, 1097,
- 375, 1097, 1050, 1472,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* hard turn */
- line2line(375, 1097, 1050, 1472,
- 1050, 1472, 375, 1847,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* hard turn */
- line2line(1050, 1472, 375, 1847,
- 375, 1847, 375, 1347,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* easy turn */
- line2line(375, 1847, 375, 1347,
- 375, 1347, 1050, 972,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* hard turn */
- line2line(375, 1347, 1050, 972,
- 1050, 972, 375, 597,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* hard turn */
- line2line(1050, 972, 375, 597,
- 375, 597, 375, 1847,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- }
- trajectory_hardstop(&mainboard.traj);
}
if (!strcmp_P(res->arg1, PSTR("test")))
subtraj_test();
+ else if (!strcmp_P(res->arg1, PSTR("orange")))
+ subtraj_test();
+ else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
+ position_set(&mainboard.pos, 2625,
+ COLOR_Y(1847), COLOR_A(0.00));
+ get_orphan_tomatoes();
+ }
trajectory_hardstop(&mainboard.traj);
}
prog_char str_subtraj_arg0[] = "subtraj";
parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
-prog_char str_subtraj_arg1[] = "faux";
+prog_char str_subtraj_arg1[] = "test#orange#tomato";
parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
lspickle.axis = axis
lspickle.size = (20, ROBOT_WIDTH, 5)
if robot_lspickle_autoharvest:
- lspickle.color = (1, 0, 0)
+ lspickle.color = (0.2, 0.2, 1)
else:
lspickle.color = (0.4, 0.4, 0.4)
rspickle.axis = axis
rspickle.size = (20, ROBOT_WIDTH, 5)
if robot_rspickle_autoharvest:
- rspickle.color = (1, 0, 0)
+ rspickle.color = (0.2, 0.2, 1)
else:
rspickle.color = (0.4, 0.4, 0.4)
{
#ifdef HOST_VERSION
cobboard.mode = mode;
- return 0;
-#else
+#endif
struct i2c_cmd_cobboard_set_mode buf;
buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
buf.mode = mode;
return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-#endif
}
static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags)
sei();
strat_db_init();
- test_strat_avoid();
printf_P(PSTR("\r\n"));
printf_P(PSTR("Respect et robustesse.\r\n"));
ballboard.mode = cmd->mode;
len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode);
+ if (cmd->mode == I2C_BALLBOARD_MODE_EJECT)
+ ballboard.ball_count = 0;
hostsim_lock();
write(fdw, buf, len);
hostsim_unlock();
return 0;
}
+static int8_t
+robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+{
+ if (cmd->mode == I2C_COBBOARD_MODE_EJECT)
+ cobboard.cob_count = 0;
+ return 0;
+}
+
int8_t
robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags)
{
static int8_t
robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
{
- // void *void_cmd = buf;
+ void *void_cmd = buf;
switch (buf[0]) {
-#if 0 /* deleted */
+
case I2C_CMD_COBBOARD_SET_MODE:
{
struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
robotsim_i2c_cobboard_set_mode(cmd);
break;
}
-#endif
+
default:
break;
}
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
+#include <trajectory_manager_core.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
volatile uint8_t strat_want_pack = 0;
volatile uint8_t strat_lpack60 = 0;
volatile uint8_t strat_rpack60 = 0;
-struct strat_conf strat_conf;
+struct strat_conf strat_conf = {
+ .dump_enabled = 0,
+ .opp_orange = 90,
+ .orphan_tomato = 50,
+ .flags = 0,
+};
/*************************************************************/
return;
printf_P(PSTR("-- conf --\r\n"));
-
+ printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange);
+ printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato);
+ printf_P(PSTR("our_orange = %s\n"),
+ (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n");
}
void strat_event_enable(void)
{
int16_t x, y;
uint8_t i, j;
+ static uint8_t prev_check_time;
+ uint8_t cur_time;
+
+ /* check present tomatoes once per second */
+ cur_time = time_get_s();
+ if (cur_time != prev_check_time) {
+ uint8_t k;
+
+ for (k = 0; k < TOMATO_NB; k++) {
+ if (strat_db.tomato_table[k]->time_removed != -1 &&
+ strat_db.tomato_table[k]->time_removed + 2 == time_get_s()) {
+#ifdef HOST_VERSION
+ printf("remove tomato %d\n", k);
+#endif
+ strat_db.tomato_table[k]->present = 0;
+ }
+ }
+ prev_check_time = cur_time;
+ }
x = position_get_x_s16(&mainboard.pos);
y = position_get_y_s16(&mainboard.pos);
if (strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
return;
+ if (strat_db.wp_table[i][j].present == 0)
+ return;
+
+ strat_db.wp_table[i][j].time_removed = time_get_s();
strat_db.wp_table[i][j].present = 0;
+#ifdef HOST_VERSION
+ ballboard.ball_count ++;
+ printf("add ball %d,%d\n", i, j);
+#endif
}
/* mark corn as not present and give correct commands to the cobboard
int8_t lcob_near, rcob_near;
uint8_t lcob, rcob;
uint8_t lidx, ridx;
+ static uint8_t prev_check_time;
+ uint8_t cur_time;
/* read sensors from ballboard */
IRQ_LOCK(flags);
ballboard.rcob = I2C_COB_NONE;
IRQ_UNLOCK(flags);
-/* if (lcob != I2C_COB_NONE) */
-/* DEBUG(E_USER_STRAT, "XXX lcob %s", */
-/* lcob == I2C_COB_WHITE ? "white" : "black"); */
-/* if (rcob != I2C_COB_NONE) */
-/* DEBUG(E_USER_STRAT, "XXX rcob %s", */
-/* rcob == I2C_COB_WHITE ? "white" : "black"); */
/* XXX take opponent position into account */
+ /* check present cobs once per second */
+ cur_time = time_get_s();
+ if (cur_time != prev_check_time) {
+ uint8_t i;
+
+ for (i = 0; i < CORN_NB; i++) {
+ if (strat_db.corn_table[i]->time_removed != -1 &&
+ strat_db.corn_table[i]->time_removed + 2 == time_get_s()) {
#ifdef HOST_VERSION
- if (time_get_s() == 15)
- cobboard.cob_count = 5;
- if (time_get_s() == 16)
- cobboard.cob_count = 0;
+ printf("remove cob %d\n", i);
#endif
+ strat_db.corn_table[i]->present = 0;
+ }
+ }
+ prev_check_time = cur_time;
+ }
/* detect cob on left side */
lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
/* deploy spickle and harvest white ones */
if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
- if (cobboard.status == I2C_COBBOARD_STATUS_LBUSY)
- strat_db.corn_table[lidx]->present = 0;
+ if (strat_db.corn_table[lidx]->time_removed == -1
+#ifndef HOST_VERSION
+ && cobboard.status == I2C_COBBOARD_STATUS_LBUSY
+#endif
+ ) {
+ strat_db.corn_table[lidx]->time_removed = time_get_s();
+#ifdef HOST_VERSION
+ cobboard.cob_count ++;
+ printf("add cob %d\n", lidx);
+#endif
+ }
}
else
i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
/* deploy spickle and harvest white ones */
if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
- if (cobboard.status == I2C_COBBOARD_STATUS_RBUSY)
- strat_db.corn_table[ridx]->present = 0;
+ if (strat_db.corn_table[ridx]->time_removed == -1
+#ifndef HOST_VERSION
+ && cobboard.status == I2C_COBBOARD_STATUS_RBUSY
+#endif
+ ) {
+ strat_db.corn_table[ridx]->time_removed = time_get_s();
+#ifdef HOST_VERSION
+ cobboard.cob_count ++;
+ printf("add cob %d\n", ridx);
+#endif
+ }
}
else
i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
{
uint8_t err;
- DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+ DEBUG(E_USER_STRAT, "%s() cob_count=%d ball_count=%d",
+ __FUNCTION__, get_cob_count(), get_ball_count());
/* check that we are called from an eject line */
if (!robot_is_on_eject_line()) {
if (get_cob_count() > 0) {
i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
- strat_db_dump(__FUNCTION__);
time_wait_ms(2000);
}
+ /* cob ejection */
+ trajectory_d_rel(&mainboard.traj, 70);
+ err = wait_traj_end(END_INTR|END_TRAJ);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ strat_db_dump(__FUNCTION__);
err = END_TRAJ;
fail:
return err;
}
-static uint8_t strat_beginning(void)
+static uint8_t strat_beginning(uint8_t do_initturn)
{
uint8_t err;
strat_set_acc(ACC_DIST, ACC_ANGLE);
- strat_set_speed(600, 60); /* OK */
- trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_STD);
+ if (do_initturn) {
+ strat_set_speed(600, 60); /* OK */
+ trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_STD);
+ }
strat_set_acc(ACC_DIST, ACC_ANGLE);
strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
}
#endif
-uint8_t strat_main(void)
+/* get tomatoes near our goals (12,5 and 12,3) */
+uint8_t get_orphan_tomatoes(void)
+{
+#define CLITOID_TOMATO_RADIUS 100.
+#define TOMATO_BACK_X 2760
+#define TOMATO_BACK_LEN 200
+
+ int16_t x, y, a;
+ uint8_t i, j;
+ uint8_t err;
+ int8_t ret;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ /* only go if both tomatoes are present */
+ if (!strat_db.wp_table[12][5].present ||
+ !strat_db.wp_table[12][3].present) {
+ return END_TRAJ;
+ }
+
+ x = position_get_x_s16(&mainboard.pos);
+ y = position_get_y_s16(&mainboard.pos);
+
+ if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
+ return END_ERROR;
+
+ /* not on eject point */
+ if (i != 11 || j != 6)
+ return END_ERROR;
+
+ strat_want_pack = 1;
+ strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
+
+ /* turn in the correct direction */
+ trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ /* clitoid to turn and take the first ball */
+ ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
+ COLOR_A(-90), 150., COLOR_A(90), 0,
+ CLITOID_TOMATO_RADIUS, 3*125);
+ if (ret < 0) {
+ err = END_ERROR;
+ goto fail;
+ }
+
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
+ err = strat_calib(300, END_TRAJ|END_BLOCKING);
+ a = position_get_a_deg_s16(&mainboard.pos);
+ if (ABS(a) < 10)
+ strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
+ DO_NOT_SET_POS,
+ COLOR_A(0));
+
+ strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -250);
+ err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
+ TRAJ_FLAGS_NO_NEAR);
+#ifdef HOST_VERSION /* simulator sees 2 blockings */
+ if (err == END_BLOCKING) {
+ trajectory_d_rel(&mainboard.traj, -250);
+ err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
+ TRAJ_FLAGS_NO_NEAR);
+ }
+#endif
+
+ if (err != 0 && !TRAJ_SUCCESS(err))
+ goto fail;
+
+ trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ /* next ball */
+
+ /* clitoid to turn and take the first ball */
+ strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
+ ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
+ COLOR_A(-90), 150., COLOR_A(90), 0,
+ CLITOID_TOMATO_RADIUS, 7*125);
+ if (ret < 0) {
+ err = END_ERROR;
+ goto fail;
+ }
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ strat_hardstop();
+ strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
+ trajectory_d_rel(&mainboard.traj, -250);
+ err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
+ TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+fail:
+ strat_want_pack = 0;
+ return err;
+}
+
+/* get oranges, must be called near game area */
+uint8_t run_to_the_hills(uint8_t orange_color)
{
+#define HILL_LEN 500
+#define HILL_POSY_YELLOW 300
+#define HILL_POSY_BLUE 200
+#define HILL_POSX_BALLS_DOWN 900
+#define HILL_START_POSX 580
+
+ double aa, ad;
+ uint16_t sa, sd;
uint8_t err;
+ uint8_t our_color = get_color();
+ int16_t startx, starty;
+
+ strat_want_pack = 1;
+ strat_get_acc(&ad, &aa);
+ strat_get_speed(&sd, &sa);
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ if (orange_color == I2C_COLOR_YELLOW)
+ starty = HILL_POSY_YELLOW;
+ else
+ starty = HILL_POSY_BLUE;
+ if (orange_color == our_color)
+ startx = HILL_START_POSX;
+ else
+ startx = AREA_X - HILL_START_POSX;
+ trajectory_goto_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ /* turn to the hills */
+ if (orange_color == our_color)
+ trajectory_a_abs(&mainboard.traj, COLOR_A(0));
+ else
+ trajectory_a_abs(&mainboard.traj, COLOR_A(180));
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ strat_set_acc(3, 3);
+ strat_set_speed(300, 500);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 40);
+ support_balls_pack();
+
+ /* here it is difficult to handle return values, because we
+ * are on the hill */
+ trajectory_d_rel(&mainboard.traj, HILL_LEN);
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > HILL_POSX_BALLS_DOWN,
+ TRAJ_FLAGS_NO_NEAR);
+ printf_P(PSTR("deploy\r\n"));
+ support_balls_deploy();
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ time_wait_ms(2000);
+
+ /* reach top, go down */
+ trajectory_d_rel(&mainboard.traj, -HILL_LEN);
+
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < HILL_POSX_BALLS_DOWN,
+ TRAJ_FLAGS_NO_NEAR);
+ printf_P(PSTR("pack\r\n"));
+ support_balls_pack();
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ printf_P(PSTR("deploy\r\n"));
+ support_balls_deploy();
+
+ /* calibrate position on the wall */
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ strat_set_acc(ad, aa);
+ trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
+ TRAJ_FLAGS_NO_NEAR);
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ err = strat_calib(-300, TRAJ_FLAGS_NO_NEAR);
+ if (orange_color == our_color)
+ position_set(&mainboard.pos, ROBOT_HALF_LENGTH_REAR,
+ DO_NOT_SET_POS, COLOR_A(0));
+ else
+ position_set(&mainboard.pos, AREA_X - ROBOT_HALF_LENGTH_REAR,
+ DO_NOT_SET_POS, COLOR_A(180));
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
+ err = wait_traj_end(TRAJ_FLAGS_STD);
+
+ /* revert acceleration and speed */
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ strat_want_pack = 0;
+ strat_set_speed(sd, sa);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+ support_balls_deploy();
+ return err;
+}
+
+uint8_t strat_main(void)
+{
+ uint8_t err, do_initturn = 1;
+
+ /* get oranges */
+ if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
+ err = run_to_the_hills(get_color());
+ do_initturn = 0;
+ }
/* harvest the first cobs + balls */
- err = strat_beginning();
+ err = strat_beginning(do_initturn);
+
if (!TRAJ_SUCCESS(err))
strat_unblock();
else
err = strat_eject();
+ /* choose circuit, and harvest on it */
while (1) {
+ DEBUG(E_USER_STRAT, "start main loop");
+
+ /* if it's time to get tomatoes, do it */
+ if (time_get_s() > strat_conf.orphan_tomato) {
+ err = get_orphan_tomatoes();
+ if (err == END_ERROR) {
+ DEBUG(E_USER_STRAT,
+ "get_orphan_tomatoes returned END_ERROR");
+ }
+ else if (err == END_TIMER) {
+ DEBUG(E_USER_STRAT, "End of time");
+ strat_exit();
+ break;
+ }
+ else if (!TRAJ_SUCCESS(err)) {
+ /* don't retry these tomatoes if it failed */
+ strat_conf.orphan_tomato = 90;
+ strat_unblock();
+ }
+ }
+
/**********************/
/* harvest on circuit */
/**********************/
- DEBUG(E_USER_STRAT, "start main loop");
-
err = strat_harvest_circuit();
if (err == END_TIMER) {
DEBUG(E_USER_STRAT, "End of time");
struct strat_conf {
uint8_t dump_enabled;
-#define STRAT_CONF_XXX 0x01
+ uint8_t opp_orange;
+ uint8_t orphan_tomato;
+
+#define STRAT_CONF_OUR_ORANGE 0x01
uint8_t flags;
};
void strat_event(void *dummy);
void strat_event_enable(void);
void strat_event_disable(void);
+uint8_t run_to_the_hills(uint8_t orange_color);
+uint8_t get_orphan_tomatoes(void);
#endif
.path = letter_v_tab,
};
+const struct wp_coord sperma_tab[] = {
+ { .i = 11, .j = 6, },
+ { .i = 10, .j = 6, },
+ { .i = 9, .j = 5, },
+ { .i = 8, .j = 5, },
+ { .i = 7, .j = 5, },
+ { .i = 6, .j = 6, },
+ { .i = 5, .j = 5, },
+ { .i = 4, .j = 5, },
+ { .i = 3, .j = 4, },
+ { .i = 2, .j = 4, },
+ { .i = 1, .j = 3, },
+ { .i = 1, .j = 4, },
+ { .i = 1, .j = 5, },
+ { .i = 1, .j = 6, },
+ { .i = 2, .j = 6, },
+ { .i = 3, .j = 5, },
+ { .i = 4, .j = 5, },
+ { .i = 5, .j = 5, },
+ { .i = 6, .j = 6, },
+ { .i = 7, .j = 5, },
+ { .i = 8, .j = 5, },
+ { .i = 9, .j = 5, },
+ { .i = 10, .j = 6, },
+ { .i = 11, .j = 6, },
+};
+
+const struct circuit sperma_circuit = {
+ .name = "sperma",
+ .len = sizeof(sperma_tab)/sizeof(struct wp_coord),
+ .path = sperma_tab,
+};
+
/* list of all possible circuits */
const struct circuit *circuits[] = {
&butterfly_circuit,
&asym_butterfly_circuit,
&big_h_lambda_circuit,
&letter_v_circuit,
+ &sperma_circuit,
NULL,
};
return 0;
}
-static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
+static int8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
{
+ uint8_t mod;
+
switch (dir) {
case LINE_UP:
case LINE_DOWN:
+ if ((i & 1) == 0)
+ return -1;
return i/2;
case LINE_R_UP:
case LINE_L_DOWN:
+ mod = i & 3;
+ if ((mod == 0 || mod == 1) && ((j & 1) == 0))
+ return -1;
+ if ((mod == 2 || mod == 3) && ((j & 1) == 1))
+ return -1;
i &= 0xfe;
j -= i/2;
return (5-j)/2;
case LINE_R_DOWN:
case LINE_L_UP:
+ mod = i & 3;
+ if ((mod == 0 || mod == 3) && ((j & 1) == 0))
+ return -1;
+ if ((mod == 1 || mod == 2) && ((j & 1) == 1))
+ return -1;
i &= 0xfe;
j += i/2;
return (11-j)/2;
return 0xFF;
}
+/* return approximative angle of line */
+int16_t linedir2angle(uint8_t dir)
+{
+ switch (dir) {
+ case LINE_UP:
+ return COLOR_A(90);
+ case LINE_DOWN:
+ return COLOR_A(-90);
+ case LINE_R_UP:
+ return COLOR_A(30);
+ case LINE_R_DOWN:
+ return COLOR_A(-90);
+ case LINE_L_UP:
+ return COLOR_A(150);
+ case LINE_L_DOWN:
+ return COLOR_A(-150);
+ default:
+ return 0;
+ }
+}
+
/* return true if a waypoint belongs to a line */
uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
{
- uint8_t ln;
+ int8_t ln;
ln = get_line_num(i, j, dir);
+ if (ln == -1)
+ return 0;
if (ln == linenum)
return 1;
return 0;
if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
continue;
- /* is there a corn cob ? */
+ /* is there a corn cob removed for more than 2 secs ? */
if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
- strat_db.wp_table[ni][nj].present &&
- strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
+ strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK &&
+ (strat_db.wp_table[ni][nj].present ||
+ strat_db.wp_table[ni][nj].time_removed + 2 > time_get_s()))
n ++;
}
/* malus if there is opponent on the path */
if (opp_on_path) {
- DPR("malus for opponent: 1000\r\n");
- score -= 2000;
+ DPR("malus for opponent: %d\r\n", (500 * opp_on_path));
+ score -= (500 * opp_on_path);
}
return score;
uint8_t ni = 0, nj = 0;
uint8_t dir, color, idx;
int8_t step = faceA ? 1 : -1;
- int16_t x, y, d, prev_d = 0;
+ int16_t x, y;
+ int32_t d, prev_d = 0;
int16_t oppx, oppy;
*score = 0x8000; /* -int_max */
/* is opponent near the point ? */
ijcoord_to_xycoord(i, j, &x, &y);
if (oppx != I2C_OPPONENT_NOT_THERE) {
- d = distance_between(oppx, oppy, x, y);
+ d = quad_distance_between(oppx, oppy, x, y);
DPR("%s(): opp at %d mm (ij=%d,%d opp=%d,%d pos=%d,%d)\r\n",
__FUNCTION__, d, i, j, oppx, oppy, x, y);
- if (d < 600 && d < prev_d)
- opponent_on_path = 1;
+ if (d < (250L*250L) && d < prev_d)
+ opponent_on_path += 3;
+ else if (d < (500L*500L) && d < prev_d)
+ opponent_on_path ++;
prev_d = d;
}
return found;
}
-static void test_all_circuits(void)
+static void init_all_circuits(void)
{
const struct circuit **circuit;
const struct wp_coord *cur;
i = cur->i;
j = cur->j;
+ strat_db.wp_table[i][j].on_circuit = 1;
+
dir = get_dir(prev_i, prev_j, i, j);
if (dir == 0xFF)
printf_P("Bad circuit %s %d %d\r\n", (*circuit)->name, i, j);
uint8_t err;
strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
+ strat_want_pack = 1;
+
+ printf("PACK PACK\n");
x = position_get_x_s16(&mainboard.pos);
y = position_get_y_s16(&mainboard.pos);
if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
__FUNCTION__, x, y);
- return END_ERROR;
+ err = END_ERROR;
+ goto fail;
}
if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
__FUNCTION__);
- return END_ERROR;
+ err = END_ERROR;
+ goto fail;
}
len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
if (len < 0) {
DEBUG(E_USER_STRAT, "%s(): cannot find a path",
__FUNCTION__);
- return END_ERROR;
+ err = END_ERROR;
+ goto fail;
}
dump_circuit_wp(circuit_wpline, len);
prev_linenum = circuit_wpline[0].line_num;
prev_dir = circuit_wpline[0].dir;
+ /* fix orientation first */
+ trajectory_a_abs(&mainboard.traj, linedir2angle(prev_dir));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ strat_want_pack = 0;
+ printf("UNPACK UNPACK\n");
+
/* do all lines of circuit */
for (idx = 1; idx < len; idx ++) {
linenum = circuit_wpline[idx].line_num;
err = line2line(prev_linenum, prev_dir, linenum, dir,
TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
- return err;
+ goto fail;
prev_linenum = linenum;
prev_dir = dir;
}
+ err = END_TRAJ;
- return END_TRAJ;
+ fail:
+ strat_want_pack = 0;
+ return err;
}
+/* list of waypoints when we are not on a circuit */
+const struct xy_point unblock_pts[] = {
+ { .x = 375, .y = 597 }, /* 1,1 */
+ { .x = 2625, .y = 597 }, /* 11,1 */
+ { .x = 1500, .y = 722 }, /* 6,2 */
+ { .x = 375, .y = 1097 }, /* 1,3 */
+ { .x = 375, .y = 1597 }, /* 1,5 */
+ { .x = 2625, .y = 1097 }, /* 11,3 */
+ { .x = 2625, .y = 1597 }, /* 11,5 */
+ { .x = 1500, .y = 1722 }, /* 6,6 */
+};
+
+
/* try to unblock in any situation */
uint8_t strat_unblock(void)
{
int16_t x, y;
- uint8_t i, j;
+ uint8_t i, j, k;
uint16_t old_dspeed, old_aspeed;
uint8_t err;
+ uint16_t d_min = 0xFFFF, d;
+ const struct xy_point *pt;
DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
x = position_get_x_s16(&mainboard.pos);
y = position_get_y_s16(&mainboard.pos);
- if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
- /* aie... go to center... but it's really a bad
- * idea */
- x = CENTER_X;
- y = CENTER_Y;
+
+ if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
+ x = -1;
+ else if (strat_db.wp_table[i][j].on_circuit == 0)
+ x = -1;
+
+ /* find the nearest unblock point */
+ if (x == -1) {
+ /* position may have been modified */
+ x = position_get_x_s16(&mainboard.pos);
+ y = position_get_y_s16(&mainboard.pos);
+
+ /* browse all points and find the nearest */
+ for (k = 0; k < sizeof(unblock_pts)/sizeof(*unblock_pts); k++) {
+ pt = &unblock_pts[k];
+ d = distance_between(x, y, pt->x, COLOR_Y(pt->y));
+ if (d < d_min) {
+ d_min = d;
+ x = pt->x;
+ y = COLOR_Y(pt->y);
+ }
+ }
}
/* XXX if opponent is too close, go back, or wait ? */
return END_TRAJ;
}
-void test_strat_avoid(void)
+void strat_avoid_init(void)
{
- test_all_circuits();
+ init_all_circuits();
#ifdef TEST_STRAT_AVOID
uint8_t i, j;
uint8_t is_60deg(uint8_t dir1, uint8_t dir2);
uint8_t is_120deg(uint8_t dir1, uint8_t dir2);
-void test_strat_avoid(void);
+void strat_avoid_init(void);
#endif
IRQ_UNLOCK(flags);
}
+void strat_get_acc(double *d, double *a)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ *d = mainboard.traj.d_acc;
+ *a = mainboard.traj.a_acc;
+ IRQ_UNLOCK(flags);
+}
+
void strat_limit_speed_enable(void)
{
strat_limit_speed_enabled = 1;
y_rel = sin(RAD(opp_a)) * (double)opp_d;
/* opponent too far */
- if (opp_d > 600)
+ if (opp_d > 500)
return 0;
/* opponent is in front of us */
void strat_set_speed(uint16_t d, uint16_t a);
void strat_get_speed(uint16_t *d, uint16_t *a);
void strat_set_acc(double d, double a);
+void strat_get_acc(double *d, double *a);
/* when user type ctrl-c we can interrupt traj */
void interrupt_traj(void);
if (corn_count_neigh(i, j) == 2)
return 1;
- /* we are on intersection, let's go slow... but as we enter in
- * the curve-part of the clitoid, we should not go there */
+
+ /* we are on intersection, keep the same speed... but as we
+ * enter in the curve-part of the clitoid, we should not go
+ * there */
if (wp_belongs_to_line(i, j, num2, dir2))
- return 0;
+ return clitoid_slow;
/* we can ge fast if it's a 60deg angle and if we checked the
* current point */
#include "main.h"
#include "strat.h"
#include "strat_base.h"
+#include "strat_avoid.h"
#include "strat_corn.h"
#include "strat_db.h"
#include "strat_utils.h"
8, 9, 6, 7, 3, 4, 5, 0, 1, 2
};
-#if 0 /* XXX maybe useless */
+#ifdef HOST_VERSION
+#define SIDE_CONF 0
+#define CENTER_CONF 0
/* the 10 possible configurations for corn on the side */
static const uint8_t corn_side_confs[9][2] = {
{ 1, 4 },
/* default type */
wp->type = WP_TYPE_WAYPOINT;
+ /* */
+ wp->time_removed = -1;
+
/* mark dangerous points */
if (i == 0 || i == (WAYPOINTS_NBX-1))
wp->dangerous = 1;
wp->type = WP_TYPE_CORN;
wp->present = 1;
wp->corn.idx = idx;
+#ifdef HOST_VERSION
+ if (idx == corn_side_confs[SIDE_CONF][0] ||
+ idx == corn_side_confs[SIDE_CONF][1] ||
+ corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][0] ||
+ corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][1] ||
+ idx == corn_center_confs[CENTER_CONF][0] ||
+ idx == corn_center_confs[CENTER_CONF][1] ||
+ corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][0] ||
+ corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][1])
+ wp->corn.color = I2C_COB_BLACK;
+ else
+ wp->corn.color = I2C_COB_WHITE;
+#else
wp->corn.color = I2C_COB_UNKNOWN;
+#endif
continue;
}
printf_P(PSTR("tomato%d: present=%d opp=%d\r\n"),
i, wp->present, wp->opp_visited);
}
+
+ /* fill circuit infos */
+ strat_avoid_init();
}
/* visited by opponent */
uint8_t opp_visited:1;
- uint8_t reserved:3;
+ /* true if the wp is on a circuit */
+ uint8_t on_circuit:1;
- /* absolute position of the waypoint */
-/* int16_t x; */
-/* int16_t y; */
+ uint8_t reserved:2;
union {
struct corn_db corn;
struct tomato_db tomato;
};
+
+ /* to monitor time when corn/ball was removed */
+ /* not optimal... but... we have enough ram */
+ int8_t time_removed;
};
/* database reflecting the status of objects on area */
return sqrt(x+y);
}
+/* return the distance between two points */
+int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
+{
+ int32_t x,y;
+ x = (x2-x1);
+ x = x*x;
+ y = (y2-y1);
+ y = y*y;
+ return x+y;
+}
+
/* return the distance to a point in the area */
int16_t distance_from_robot(int16_t x, int16_t y)
{
while ( (! (cond)) && (__err == 0)) { \
__err = test_traj_end(mask); \
} \
+ if (!__err) \
+ DEBUG(E_USER_STRAT, "cond is true at line %d", \
+ __LINE__); \
+ else \
+ DEBUG(E_USER_STRAT, "got %s at line %d", \
+ get_err(__err), __LINE__); \
__err; \
}) \
int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
+int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
int16_t distance_from_robot(int16_t x, int16_t y);
int16_t simple_modulo_360(int16_t a);
int16_t angle_abs_to_rel(int16_t a_abs);
git.droids-corp.org / aversive.git / commitdiff