From 4e7801883ed4076cb14b63a0571467747894c0f8 Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 22 Apr 2010 22:35:44 +0200 Subject: [PATCH] strat db --- projects/microb2010/ballboard/main.c | 2 +- projects/microb2010/ballboard/main.h | 2 + projects/microb2010/cobboard/main.c | 2 +- projects/microb2010/cobboard/main.h | 2 + projects/microb2010/cobboard/sensor.c | 6 +- projects/microb2010/cobboard/sensor.h | 4 +- projects/microb2010/cobboard/spickle.c | 4 +- projects/microb2010/common/i2c_commands.h | 2 + projects/microb2010/mainboard/Makefile | 1 + projects/microb2010/mainboard/commands.c | 4 +- .../microb2010/mainboard/commands_mainboard.c | 54 +- projects/microb2010/mainboard/commands_traj.c | 66 +-- projects/microb2010/mainboard/main.c | 3 +- projects/microb2010/mainboard/main.h | 4 +- projects/microb2010/mainboard/sensor.c | 4 +- projects/microb2010/mainboard/sensor.h | 4 +- projects/microb2010/mainboard/strat.c | 90 ++- projects/microb2010/mainboard/strat.h | 74 +-- projects/microb2010/mainboard/strat_avoid.c | 369 +++++++++++- projects/microb2010/mainboard/strat_base.c | 26 +- projects/microb2010/mainboard/strat_corn.c | 552 +----------------- projects/microb2010/mainboard/strat_corn.h | 16 +- 22 files changed, 549 insertions(+), 742 deletions(-) diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index de8bc0c..725a89d 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -170,7 +170,7 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD); + eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_BALLBOARD) { diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h index 9f10c35..0758279 100755 --- a/projects/microb2010/ballboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -19,6 +19,8 @@ * */ +/* was sensorboard in 2009 */ + #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ port &= ~_BV(bit); \ diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index d21e7b7..7a3d240 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -173,7 +173,7 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); + eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_COBBOARD) { diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 0c12fdc..105ca7d 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -19,6 +19,8 @@ * */ +/* was mainboard in 2009 */ + #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ port &= ~_BV(bit); \ diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index c471033..eb1f42e 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -140,7 +140,7 @@ struct sensor_filter { }; /* pullup mapping: - * CAP 1,5,6,7,8 + * CAP 1,5,6,7,8 (notation elec) */ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 1 }, /* 0 */ @@ -148,8 +148,8 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ [S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */ - [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ /////// not used - [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ /////// not used + [S_CAP6] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ + [S_CAP7] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ [S_RCOB] = { 1, 0, 0, 1, 0, 1 }, /* 7 */ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index c24af73..a603e97 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -33,8 +33,8 @@ #define S_COB_INSIDE_R 2 #define S_CAP4 3 #define S_LCOB 4 -#define S_LEFT 5 -#define S_RIGHT 6 +#define S_CAP6 5 +#define S_CAP7 6 #define S_RCOB 7 #define S_RESERVED1 8 #define S_RESERVED2 9 diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index cbcb65c..f8e1613 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -67,8 +67,8 @@ static struct spickle_params spickle = { &cobboard.right_spickle, }, .pos_deployed = { - 200, /* left */ - 200, /* right */ + 7000, // 200, /* left */ + 7000, // 200, /* right */ }, .pos_mid = { 25000, /* left */ diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 4b8aea5..c0ff8e1 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -38,6 +38,8 @@ #define I2C_COB_BLACK 0 #define I2C_COB_WHITE 1 +#define I2C_COB_UNKNOWN 2 +#define I2C_COB_REMOVED 3 struct i2c_cmd_hdr { uint8_t cmd; diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 33b5c52..2ebfff2 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -7,6 +7,7 @@ SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c SRC += strat_utils.c strat_base.c strat.c strat_corn.c +SRC += strat_db.c ifeq ($(H),1) SRC += robotsim.c endif diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index e5bb456..879297a 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -105,7 +105,7 @@ extern parse_pgm_inst_t cmd_goto2; extern parse_pgm_inst_t cmd_goto3; extern parse_pgm_inst_t cmd_position; extern parse_pgm_inst_t cmd_position_set; -extern parse_pgm_inst_t cmd_strat_infos; +extern parse_pgm_inst_t cmd_strat_db; extern parse_pgm_inst_t cmd_strat_conf; extern parse_pgm_inst_t cmd_strat_conf2; extern parse_pgm_inst_t cmd_strat_conf3; @@ -194,7 +194,7 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_goto2, (parse_pgm_inst_t *)&cmd_position, (parse_pgm_inst_t *)&cmd_position_set, - (parse_pgm_inst_t *)&cmd_strat_infos, + (parse_pgm_inst_t *)&cmd_strat_db, (parse_pgm_inst_t *)&cmd_strat_conf, (parse_pgm_inst_t *)&cmd_strat_conf2, (parse_pgm_inst_t *)&cmd_strat_conf3, diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index de73518..e4eee57 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -66,6 +66,7 @@ #include "strat.h" #include "strat_utils.h" #include "strat_base.h" +#include "strat_db.h" #include "strat_corn.h" #include "i2c_protocol.h" #include "actuator.h" @@ -301,11 +302,11 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = E_USER_STRAT; if (!strcmp_P(res->debug, PSTR("debug"))) { - strat_infos.dump_enabled = 1; + strat_db.dump_enabled = 1; gen.log_level = 5; } else { - strat_infos.dump_enabled = 0; + strat_db.dump_enabled = 0; gen.log_level = 0; } @@ -1126,58 +1127,9 @@ struct cmd_test_result { int32_t dist; }; -#if 0 -static void reverse_line(struct line_2pts *l) -{ - point_t tmp; - - tmp.x = l->p1.x; - tmp.y = l->p1.y; - l->p1.x = l->p2.x; - l->p1.y = l->p2.y; - l->p2.x = tmp.x; - l->p2.y = tmp.y; -} -#endif - /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - uint8_t err; - -#ifdef HOST_VERSION - strat_reset_pos(298.48, COLOR_Y(309.21), - COLOR_A(70.02)); - mainboard.angle.on = 1; - mainboard.distance.on = 1; - strat_set_speed(250, SPEED_ANGLE_FAST); -#endif - init_corn_table(0, 0); - time_wait_ms(100); - - err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); - err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); - err = line2line(LINE_R_UP, 2, LINE_UP, 5); - trajectory_hardstop(&mainboard.traj); - - /* ball ejection */ - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); - time_wait_ms(2000); - - /* half turn */ - trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); - trajectory_a_rel(&mainboard.traj, COLOR_A(180)); - err = wait_traj_end(END_INTR|END_TRAJ); - - /* cob ejection */ - trajectory_d_rel(&mainboard.traj, -100); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_eject(); - time_wait_ms(2000); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index be9150b..d63f57a 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -59,6 +59,7 @@ #include "strat_utils.h" #include "strat_base.h" #include "strat.h" +#include "strat_db.h" #include "../common/i2c_commands.h" #include "i2c_protocol.h" @@ -891,37 +892,37 @@ parse_pgm_inst_t cmd_position_set = { /**********************************************************/ /* strat configuration */ -/* this structure is filled when cmd_strat_infos is parsed successfully */ -struct cmd_strat_infos_result { +/* this structure is filled when cmd_strat_db is parsed successfully */ +struct cmd_strat_db_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_strat_infos is parsed successfully */ -static void cmd_strat_infos_parsed(void *parsed_result, void *data) +/* function called when cmd_strat_db is parsed successfully */ +static void cmd_strat_db_parsed(void *parsed_result, void *data) { - struct cmd_strat_infos_result *res = parsed_result; + struct cmd_strat_db_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("reset"))) { - strat_reset_infos(); + strat_db_init(); } - strat_infos.dump_enabled = 1; - strat_dump_infos(__FUNCTION__); + strat_db.dump_enabled = 1; + strat_db_dump(__FUNCTION__); } -prog_char str_strat_infos_arg0[] = "strat_infos"; -parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0); -prog_char str_strat_infos_arg1[] = "show#reset"; -parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1); +prog_char str_strat_db_arg0[] = "strat_db"; +parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0); +prog_char str_strat_db_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1); -prog_char help_strat_infos[] = "reset/show strat_infos"; -parse_pgm_inst_t cmd_strat_infos = { - .f = cmd_strat_infos_parsed, /* function to call */ +prog_char help_strat_db[] = "reset/show strat_db"; +parse_pgm_inst_t cmd_strat_db = { + .f = cmd_strat_db_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_strat_infos, + .help_str = help_strat_db, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_infos_arg0, - (prog_void *)&cmd_strat_infos_arg1, + (prog_void *)&cmd_strat_db_arg0, + (prog_void *)&cmd_strat_db_arg1, NULL, }, }; @@ -939,9 +940,8 @@ struct cmd_strat_conf_result { static void cmd_strat_conf_parsed(void *parsed_result, void *data) { // struct cmd_strat_conf_result *res = parsed_result; - - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf_arg0[] = "strat_conf"; @@ -1002,12 +1002,12 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) #endif if (on) - strat_infos.conf.flags |= bit; + strat_conf.flags |= bit; else - strat_infos.conf.flags &= (~bit); + strat_conf.flags &= (~bit); - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf2_arg0[] = "strat_conf"; @@ -1050,32 +1050,32 @@ static void cmd_strat_conf3_parsed(void *parsed_result, void *data) if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.scan_opp_min_time = res->arg2; + strat_conf.scan_opp_min_time = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.delay_between_opp_scan = res->arg2; + strat_conf.delay_between_opp_scan = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.scan_our_min_time = res->arg2; + strat_conf.scan_our_min_time = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.delay_between_our_scan = res->arg2; + strat_conf.delay_between_our_scan = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { - strat_infos.conf.wait_opponent = res->arg2; + strat_conf.wait_opponent = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { - strat_infos.conf.lintel_min_time = res->arg2; + strat_conf.lintel_min_time = res->arg2; } #endif - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf3_arg0[] = "strat_conf"; diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 633a35d..14a2aad 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -67,6 +67,7 @@ #include "actuator.h" #include "cs.h" #include "strat_base.h" +#include "strat_db.h" #include "i2c_protocol.h" /* 0 means "programmed" @@ -289,7 +290,7 @@ int main(void) /* strat */ gen.logs[0] = E_USER_STRAT; gen.log_level = 5; - strat_reset_infos(); + strat_db_init(); /* strat-related event */ scheduler_add_periodical_event_priority(strat_event, NULL, diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 367718f..ff86dd2 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -19,6 +19,8 @@ * */ +/* was mechboard in 2009 */ + #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ port &= ~_BV(bit); \ @@ -119,7 +121,7 @@ #define I2C_POLL_PRIO 20 #define EEPROM_TIME_PRIO 10 -#define CS_PERIOD 5000L /* in microsecond */ +#define CS_PERIOD 5000L /* in microsecond */ #define CS_HZ (1000000. / CS_PERIOD) #define NB_LOGS 4 diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c index 473ed76..fb0bfd9 100644 --- a/projects/microb2010/mainboard/sensor.c +++ b/projects/microb2010/mainboard/sensor.c @@ -160,8 +160,8 @@ struct sensor_filter { static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */ [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ - [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ - [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ + [S_LFRONT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ + [S_RFRONT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 4 */ [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ [S_RCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 6 */ diff --git a/projects/microb2010/mainboard/sensor.h b/projects/microb2010/mainboard/sensor.h index 0ab7a3b..9a4cd96 100644 --- a/projects/microb2010/mainboard/sensor.h +++ b/projects/microb2010/mainboard/sensor.h @@ -32,8 +32,8 @@ /* synchronize with sensor.c */ #define S_CAP1 0 #define S_CAP2 1 -#define S_COLUMN_RIGHT 2 -#define S_COLUMN_LEFT 3 +#define S_LFRONT 2 +#define S_RFRONT 3 #define S_START_SWITCH 4 #define S_DISP_LEFT 5 #define S_RCOB_WHITE 6 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index c253e5f..a5a952c 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -57,6 +57,7 @@ #include "i2c_protocol.h" #include "main.h" #include "strat.h" +#include "strat_db.h" #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" @@ -66,16 +67,7 @@ #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ #define COL_SCAN_PRE_MARGIN 250 -struct strat_infos strat_infos = { - /* conf */ - .conf = { - .flags = 0, - }, - /* status */ - .status = { - .flags = 0, - }, -}; +struct strat_conf strat_conf; /*************************************************************/ @@ -83,27 +75,6 @@ struct strat_infos strat_infos = { /*************************************************************/ -void strat_set_bounding_box(void) -{ - if (get_color() == I2C_COLOR_YELLOW) { - strat_infos.area_bbox.x1 = 300; - strat_infos.area_bbox.y1 = 200; - strat_infos.area_bbox.x2 = 2720; /* needed for c1 */ - strat_infos.area_bbox.y2 = 1800; - } - else { - strat_infos.area_bbox.x1 = 200; - strat_infos.area_bbox.y1 = 300; - strat_infos.area_bbox.x2 = 2720; /* needed for c1 */ - strat_infos.area_bbox.y2 = 1900; - } - - polygon_set_boundingbox(strat_infos.area_bbox.x1, - strat_infos.area_bbox.y1, - strat_infos.area_bbox.x2, - strat_infos.area_bbox.y2); -} - /* called before each strat, and before the start switch */ void strat_preinit(void) { @@ -113,41 +84,25 @@ void strat_preinit(void) DO_POS | DO_BD | DO_POWER; //i2c_cobboard_mode_init(); - strat_dump_conf(); - strat_dump_infos(__FUNCTION__); + strat_conf_dump(__FUNCTION__); + strat_db_dump(__FUNCTION__); } -void strat_dump_conf(void) +void strat_conf_dump(const char *caller) { - if (!strat_infos.dump_enabled) + if (!strat_conf.dump_enabled) return; printf_P(PSTR("-- conf --\r\n")); } -/* display current information about the state of the game */ -void strat_dump_infos(const char *caller) -{ - if (!strat_infos.dump_enabled) - return; - - printf_P(PSTR("%s() dump strat infos:\r\n"), caller); -} - -/* init current area state before a match. Dump update user conf - * here */ -void strat_reset_infos(void) -{ - init_corn_table(-1, -1); -} - /* call it just before launching the strat */ void strat_init(void) { /* XXX init rollers, .. */ - strat_reset_infos(); + strat_db_init(); /* we consider that the color is correctly set */ @@ -168,17 +123,21 @@ void strat_init(void) /* call it after each strat */ void strat_exit(void) { +#ifndef HOST_VERSION uint8_t flags; +#endif mainboard.flags &= ~(DO_TIMER); strat_hardstop(); time_reset(); - wait_ms(1000); + wait_ms(100); +#ifndef HOST_VERSION IRQ_LOCK(flags); mainboard.flags &= ~(DO_CS); + IRQ_UNLOCK(flags); pwm_ng_set(LEFT_PWM, 0); pwm_ng_set(RIGHT_PWM, 0); - IRQ_UNLOCK(flags); +#endif } /* called periodically */ @@ -204,11 +163,31 @@ static uint8_t strat_beginning(void) uint8_t err; strat_set_speed(250, SPEED_ANGLE_FAST); - //init_corn_table(0, 0); + l1: err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); + if (!TRAJ_SUCCESS(err)) { + trajectory_hardstop(&mainboard.traj); + time_wait_ms(2000); + goto l1; + } + + l2: err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + if (!TRAJ_SUCCESS(err)) { + trajectory_hardstop(&mainboard.traj); + time_wait_ms(2000); + goto l2; + } + + l3: err = line2line(LINE_R_UP, 2, LINE_UP, 5); + if (!TRAJ_SUCCESS(err)) { + trajectory_hardstop(&mainboard.traj); + time_wait_ms(2000); + goto l3; + } + trajectory_hardstop(&mainboard.traj); /* ball ejection */ @@ -230,6 +209,7 @@ static uint8_t strat_beginning(void) i2c_cobboard_mode_eject(); time_wait_ms(2000); + trajectory_hardstop(&mainboard.traj); return END_TRAJ; } diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 0a31073..42b47dd 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -42,24 +42,51 @@ #define CORNER_X 3000 #define CORNER_Y COLOR_Y(2100) +/* XXX these offset are not related to corn, but to waypoints */ +#define OFFSET_CORN_X 150 +#define OFFSET_CORN_Y 222 +#define STEP_CORN_X 225 +#define STEP_CORN_Y 250 + +#define CORN_NB 18 +#define TOMATO_NB 14 + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + /* - * + * Corn position and lines * * vertical lines * O 1 2 3 4 5 * 2100 +-----|-----|-----|-----|-----|-----|-----+ - * | o o o | - * | o o o o | diag - * | o o o | lines - * 0/ o o o o \0 - * y | o o | - * 1/ o o \1 - * | | - * 2/------ ------\2 + * | c5 c9 c14 | + * | c2 c7 c11 c17 | diag + * | c4 c8 c13 | lines + * 0/ c1 c6 c10 c16 \0 + * y | c3 c12 | + * 1/ c0 c15 \1 + * |-----+ +-----| + * 2/ | | \2 * | | | | + * 0 +-----+-----------------------------+-----+ + * 0 x 3000 + * + * Ball (tomato) and i,j coords (for waypoints) + * + * 2100 +--0--1--2--3--4--5--6--7--8--9-10-11-12--+ + * 7 t5 t9 | + * 6 t3 t7 t11 | + * 5 t1 t4 t8 t13 | + * 4 t2 t6 t10 | + * y 3 t0 t12 | + * 2 | + * 1-----+ +-----| + * 0 | | | * | | | | * 0 +-----+-----------------------------+-----+ * 0 x 3000 + * */ /* useful traj flags */ @@ -83,38 +110,17 @@ #define SPEED_ANGLE_VERY_SLOW 400. /* strat infos structures */ - -struct strat_bbox { - int32_t x1; - int32_t y1; - int32_t x2; - int32_t y2; -}; - struct strat_conf { -#define STRAT_CONF_XXX 0x01 - uint8_t flags; -}; + uint8_t dump_enabled; -struct strat_status { -#define STRAT_STATUS_LHARVEST 0x01 -#define STRAT_STATUS_RHARVEST 0x02 +#define STRAT_CONF_XXX 0x01 uint8_t flags; }; -/* all infos related to strat */ -struct strat_infos { - uint8_t dump_enabled; - struct strat_conf conf; - struct strat_bbox area_bbox; - struct strat_status status; -}; -extern struct strat_infos strat_infos; +extern struct strat_conf strat_conf; /* in strat.c */ -void strat_dump_infos(const char *caller); /* show current known state - of area */ -void strat_dump_conf(void); +void strat_conf_dump(const char *caller); void strat_reset_infos(void); /* reset current known state */ void strat_preinit(void); void strat_init(void); diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 60dead3..3c128bf 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -1,5 +1,5 @@ /* - * Copyright Droids Corporation, Microb Technology (2009) + * Copyright Droids, Microb Technology (2010) * * 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 @@ -15,18 +15,19 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: strat_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ * + * Olivier MATZ */ + #include #include -#include +#include #include +#include #include -#include -#include #include #include @@ -38,6 +39,8 @@ #include #include #include +#include +#include #include #include #include @@ -46,10 +49,364 @@ #include #include +#include + #include #include #include "../common/i2c_commands.h" - +#include "i2c_protocol.h" #include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_corn.h" +#include "strat_utils.h" +#include "sensor.h" +#include "actuator.h" + +/* XXX TODO +static +const +change x,y -> i,j to avoid confusion with coords +could be optimized in mem space: it is not needed to store the x,y coord, + we can process it from idx. however it will be less optimized for speed + +*/ + +#define OFFSET_CORN_X 150 +#define OFFSET_CORN_Y 222 +#define STEP_CORN_X 225 +#define STEP_CORN_Y 250 + +#define CORN_NB 18 + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + +/* enum is better */ +#define TYPE_WAYPOINT 0 +#define TYPE_DANGEROUS 1 +#define TYPE_WHITE_CORN 2 +#define TYPE_BLACK_CORN 3 +#define TYPE_OBSTACLE 4 +#define TYPE_UNKNOWN 5 + +/* XXX enum possible ? else just rename */ +#define START 0 +#define UP 1 +#define UP_RIGHT 2 +#define DOWN_RIGHT 3 +#define DOWN 4 +#define DOWN_LEFT 5 +#define UP_LEFT 6 +#define END 7 + +struct djpoint { + uint16_t weight; + struct djpoint *parent; + + uint8_t type:3; + uint8_t parent_pos:3; + uint8_t updated:1; + uint8_t todo:1; +}; + +uint8_t corn_table[CORN_NB]; + +static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; + +/* return index from neigh pointer */ +#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) + +void dump(void) +{ + int8_t i, j; + struct djpoint *pt; + + printf_P(PSTR(" ")); + for (i=0; i=0; j--) { + printf_P(PSTR("%3d "), j/2); + + if ((j&1) == 0) + printf_P(PSTR(" ")); + + for (i=0; itype == TYPE_OBSTACLE) + printf_P(PSTR(" X ")); + else if (pt->type == TYPE_DANGEROUS) + printf_P(PSTR(" D ")); + else if (pt->type == TYPE_WHITE_CORN) + printf_P(PSTR(" W ")); + else if (pt->type == TYPE_BLACK_CORN) + printf_P(PSTR(" B ")); + else if (pt->type == TYPE_WAYPOINT) + printf_P(PSTR(" %5d "), pt->weight); + else + printf_P(PSTR(" ? ")); + } + printf_P(PSTR("\r\n")); + } +} + +static inline uint8_t opposite_position(uint8_t pos) +{ + pos += 3; + if (pos > UP_LEFT) + pos -= 6; + return pos; +} + +/* return coord of the entry in the table from the pointer */ +static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) +{ + int8_t idx = PT2IDX(pt); + *x = idx / WAYPOINTS_NBY; + *y = idx % WAYPOINTS_NBY; +} + +/* get the neighbour of the point at specified position */ +static struct djpoint *get_neigh(struct djpoint *pt, + uint8_t position) +{ + int8_t i,j; + struct djpoint *neigh; + + djpoint2ij(pt, &i, &j); + + switch (position) { + case UP: + j++; + break; + case UP_RIGHT: + if (!(i & 1)) j++; + i++; + break; + case DOWN_RIGHT: + if (i & 1) j--; + i++; + break; + case DOWN: + j--; + break; + case DOWN_LEFT: + if (i & 1) j--; + i--; + break; + case UP_LEFT: + if (!(i & 1)) j++; + i--; + break; + default: + return NULL; + } + if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) + return NULL; + + neigh = &djpoints[i][j]; + + if (neigh->type != TYPE_WAYPOINT) + return NULL; + + return neigh; +} + +static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) +{ + struct djpoint *neigh = NULL; + uint8_t pos = *position + 1; + + for (pos = *position + 1; pos < END; pos++) { + neigh = get_neigh(pt, pos); + if (neigh != NULL) + break; + } + + *position = pos; + return neigh; +} + +/* browse all points */ +#define POINT_FOREACH(cur) \ + for (cur = &djpoints[0][0]; \ + cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ + cur ++) + +/* XXX comment */ +#define NEIGH_FOREACH(neigh, pos, point) \ + for (pos = START, neigh = get_next_neigh(point, &pos); \ + neigh; \ + neigh = get_next_neigh(point, &pos)) + +int dijkstra_init(void) +{ + return 0; +} + +static uint16_t dist(struct djpoint *p1, struct djpoint *p2) +{ + double vx, vy; + vx = p2->pos.x - p1->pos.x; + vy = p2->pos.y - p1->pos.y; + return sqrt(vx * vx + vy * vy); +} + +void dijkstra_process_neighs(struct djpoint *pt) +{ + struct djpoint *neigh; + uint8_t pos, parent_pos; + uint16_t weight; + int8_t i,j; + + djpoint2ij(pt, &i, &j); + printf_P(PSTR("at %d %d:\r\n"), i, j); + + NEIGH_FOREACH(neigh, pos, pt) { + weight = pt->weight + dist(pt, neigh); + parent_pos = opposite_position(pos); + + /* bonus if we keep the same direction */ + if (parent_pos == pt->parent_pos || + pt->parent_pos == END) + weight = weight - 1; + + printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, + pt->parent_pos, parent_pos, + neigh->weight, weight); + if (neigh->weight == 0 || weight < neigh->weight) { + djpoint2ij(neigh, &i, &j); + //printf_P(PSTR(" %d,%d updated\r\n"), i, j); + neigh->weight = weight; + neigh->parent_pos = parent_pos; + neigh->updated = 1; + } + } +} + +int dijkstra(struct djpoint *start) +{ + struct djpoint *cur; + uint8_t todolist = 1; + + start->todo = 1; + + while (todolist) { + printf_P(PSTR("\r\n")); + dump(); + /* process all neighbours of todo list */ + POINT_FOREACH(cur) { + if (!cur->todo) + continue; + dijkstra_process_neighs(cur); + cur->todo = 0; + } + + /* convert updated list in todo list */ + todolist = 0; + POINT_FOREACH(cur) { + if (!cur->updated) + continue; + todolist = 1; + cur->todo = 1; + cur->updated = 0; + } + } + return 0; /* XXX */ +} + +/* init waypoints position */ +void init_djpoints(void) +{ + int8_t i, j; + struct djpoint *pt; + + for (i=0; itype = TYPE_WAYPOINT; + pt->parent_pos = END; + pt->updated = 0; + pt->todo = 0; + } + } +} + +/* update the type and weight of waypoints, before starting + * dijkstra */ +void update_djpoints(void) +{ + int8_t i, j, c; + struct djpoint *pt; + + for (i=0; iweight = 0; + + /* corn */ + c = ijcoord_to_corn_idx(i, j); + if (c >= 0) { + pt->type = corn_table[c]; + continue; + } + /* too close of border */ + if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* hill */ + if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* dangerous points */ + if (i == 0 || i == (WAYPOINTS_NBX-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + pt->type = TYPE_WAYPOINT; + } + } +} + +int get_path(struct djpoint *start, struct djpoint *end) +{ + struct djpoint *cur; + uint8_t prev_direction = 0; + int8_t idx; + int16_t x, y; + + for (cur = start; + cur != NULL && cur->parent_pos != END && cur != end; + cur = get_neigh(cur, cur->parent_pos)) { + if (prev_direction != cur->parent_pos) { + idx = PT2IDX(cur); + corn_idx_to_coordxy(idx, &x, &y); + printf_P(PSTR("%d %d (%d)\r\n"), + x, y, cur->parent_pos); + } + prev_direction = cur->parent_pos; + } + idx = PT2IDX(end); + corn_idx_to_coordxy(idx, &x, &y); + printf_P(PSTR("%d %d\r\n"), x, y); + return 0; /* XXX */ +} diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index a6724ad..1f3b6e5 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -278,13 +278,15 @@ void strat_limit_speed(void) /* start the strat */ void strat_start(void) { - int8_t i; uint8_t err; strat_preinit(); +#ifndef HOST_VERSION /* if start sw not plugged */ if (sensor_get(S_START_SWITCH)) { + int8_t i; + printf_P(PSTR("No start switch, press a key or plug it\r\n")); /* while start sw not plugged */ @@ -306,9 +308,11 @@ void strat_start(void) /* while start sw plugged */ while (!sensor_get(S_START_SWITCH)); } +#endif strat_init(); err = strat_main(); + printf("coucou\n"); NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err)); strat_exit(); } @@ -316,6 +320,7 @@ void strat_start(void) /* return true if we have to brake due to an obstacle */ uint8_t strat_obstacle(void) { +#if 0 int16_t x_rel, y_rel; int16_t opp_x, opp_y, opp_d, opp_a; @@ -333,6 +338,25 @@ uint8_t strat_obstacle(void) opponent_obstacle.y = opp_y; opponent_obstacle.d = opp_d; opponent_obstacle.a = opp_a; +#else /* belgium cup only */ + int16_t x_rel, y_rel; + int16_t opp_d, opp_a; + double opp_x, opp_y; + +#ifdef HOST_VERSION + if (time_get_s() >= 12 && time_get_s() <= 30) + return 1; +#endif + if (!sensor_get(S_RCOB_WHITE)) + return 0; + + opp_a = 0; + opp_d = 300; + + rel_da_to_abs_xy(opp_d, RAD(opp_a), &opp_x, &opp_y); + if (!is_in_area(opp_x, opp_y, 250)) + return 0; +#endif /* sensor are temporarily disabled */ if (sensor_obstacle_is_disabled()) diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 4262e34..3694bcd 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -58,508 +58,16 @@ #include "i2c_protocol.h" #include "main.h" #include "strat.h" +#include "strat_db.h" #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" #include "sensor.h" #include "actuator.h" -/* XXX TODO -static -const -change x,y -> i,j to avoid confusion with coords -could be optimized in mem space: it is not needed to store the x,y coord, - we can process it from idx. however it will be less optimized for speed - -*/ - -#define OFFSET_CORN_X 150 -#define OFFSET_CORN_Y 222 -#define STEP_CORN_X 225 -#define STEP_CORN_Y 250 - -#define CORN_NB 18 - -#define WAYPOINTS_NBX 13 -#define WAYPOINTS_NBY 8 - -/* enum is better */ -#define TYPE_WAYPOINT 0 -#define TYPE_DANGEROUS 1 -#define TYPE_WHITE_CORN 2 -#define TYPE_BLACK_CORN 3 -#define TYPE_OBSTACLE 4 -#define TYPE_UNKNOWN 5 - -/* XXX enum possible ? else just rename */ -#define START 0 -#define UP 1 -#define UP_RIGHT 2 -#define DOWN_RIGHT 3 -#define DOWN 4 -#define DOWN_LEFT 5 -#define UP_LEFT 6 -#define END 7 - -struct point { - int32_t x; - int32_t y; -}; - -struct djpoint { - struct point pos; - uint16_t weight; - struct djpoint *parent; - - uint8_t type:3; - uint8_t parent_pos:3; - uint8_t updated:1; - uint8_t todo:1; -}; - -uint8_t corn_table[CORN_NB]; - -const uint8_t corn_coord_i[CORN_NB] = { - 0, 0, 0, 2, 2, 2, 4, 4, 6, - 6, 8, 8, 10, 10, 10, 12, 12, 12, -}; - -const uint8_t corn_coord_j[CORN_NB] = { - 2, 4, 6, 3, 5, 7, 4, 6, 5, - 7, 4, 6, 3, 5, 7, 2, 4, 6, -}; - -static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; - -/* table to find the symetric idx */ -uint8_t corn_sym[] = { - 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 -}; - -uint8_t corn_side_confs[9][2] = { - { 1, 4 }, - { 0, 4 }, - { 2, 4 }, - { 2, 3 }, - { 0, 3 }, - { 1, 3 }, - { 1, 6 }, - { 0, 6 }, - { 2, 6 }, -}; -uint8_t corn_center_confs[4][2] = { - { 5, 8 }, - { 7, 8 }, - { 5, 9 }, - { 7, 8 }, -}; - - -/* return index from neigh pointer */ -#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) - -void dump(void) -{ - int8_t i, j; - struct djpoint *pt; - - printf_P(PSTR(" ")); - for (i=0; i=0; j--) { - printf_P(PSTR("%3d "), j/2); - - if ((j&1) == 0) - printf_P(PSTR(" ")); - - for (i=0; itype == TYPE_OBSTACLE) - printf_P(PSTR(" X ")); - else if (pt->type == TYPE_DANGEROUS) - printf_P(PSTR(" D ")); - else if (pt->type == TYPE_WHITE_CORN) - printf_P(PSTR(" W ")); - else if (pt->type == TYPE_BLACK_CORN) - printf_P(PSTR(" B ")); - else if (pt->type == TYPE_WAYPOINT) - printf_P(PSTR(" %5d "), pt->weight); - else - printf_P(PSTR(" ? ")); - } - printf_P(PSTR("\r\n")); - } -} - -static inline uint8_t opposite_position(uint8_t pos) -{ - pos += 3; - if (pos > UP_LEFT) - pos -= 6; - return pos; -} - -/* return coord of the entry in the table from the pointer */ -static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) -{ - int8_t idx = PT2IDX(pt); - *x = idx / WAYPOINTS_NBY; - *y = idx % WAYPOINTS_NBY; -} - -/* get the neighbour of the point at specified position */ -static struct djpoint *get_neigh(struct djpoint *pt, - uint8_t position) -{ - int8_t i,j; - struct djpoint *neigh; - - djpoint2ij(pt, &i, &j); - - switch (position) { - case UP: - j++; - break; - case UP_RIGHT: - if (!(i & 1)) j++; - i++; - break; - case DOWN_RIGHT: - if (i & 1) j--; - i++; - break; - case DOWN: - j--; - break; - case DOWN_LEFT: - if (i & 1) j--; - i--; - break; - case UP_LEFT: - if (!(i & 1)) j++; - i--; - break; - default: - return NULL; - } - if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) - return NULL; - - neigh = &djpoints[i][j]; - - if (neigh->type != TYPE_WAYPOINT) - return NULL; - - return neigh; -} - -static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) -{ - struct djpoint *neigh = NULL; - uint8_t pos = *position + 1; - - for (pos = *position + 1; pos < END; pos++) { - neigh = get_neigh(pt, pos); - if (neigh != NULL) - break; - } - - *position = pos; - return neigh; -} - -/* browse all points */ -#define POINT_FOREACH(cur) \ - for (cur = &djpoints[0][0]; \ - cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ - cur ++) - -/* XXX comment */ -#define NEIGH_FOREACH(neigh, pos, point) \ - for (pos = START, neigh = get_next_neigh(point, &pos); \ - neigh; \ - neigh = get_next_neigh(point, &pos)) - -int dijkstra_init(void) -{ - return 0; -} - -static uint16_t dist(struct djpoint *p1, struct djpoint *p2) -{ - double vx, vy; - vx = p2->pos.x - p1->pos.x; - vy = p2->pos.y - p1->pos.y; - return sqrt(vx * vx + vy * vy); -} - -void dijkstra_process_neighs(struct djpoint *pt) -{ - struct djpoint *neigh; - uint8_t pos, parent_pos; - uint16_t weight; - int8_t i,j; - - djpoint2ij(pt, &i, &j); - printf_P(PSTR("at %d %d:\r\n"), i, j); - - NEIGH_FOREACH(neigh, pos, pt) { - weight = pt->weight + dist(pt, neigh); - parent_pos = opposite_position(pos); - - /* bonus if we keep the same direction */ - if (parent_pos == pt->parent_pos || - pt->parent_pos == END) - weight = weight - 1; - - printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, - pt->parent_pos, parent_pos, - neigh->weight, weight); - if (neigh->weight == 0 || weight < neigh->weight) { - djpoint2ij(neigh, &i, &j); - //printf_P(PSTR(" %d,%d updated\r\n"), i, j); - neigh->weight = weight; - neigh->parent_pos = parent_pos; - neigh->updated = 1; - } - } -} - -int dijkstra(struct djpoint *start) -{ - struct djpoint *cur; - uint8_t todolist = 1; - - start->todo = 1; - - while (todolist) { - printf_P(PSTR("\r\n")); - dump(); - /* process all neighbours of todo list */ - POINT_FOREACH(cur) { - if (!cur->todo) - continue; - dijkstra_process_neighs(cur); - cur->todo = 0; - } - - /* convert updated list in todo list */ - todolist = 0; - POINT_FOREACH(cur) { - if (!cur->updated) - continue; - todolist = 1; - cur->todo = 1; - cur->updated = 0; - } - } - return 0; /* XXX */ -} - -int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j) -{ - uint8_t n; - for (n = 0; n < CORN_NB; n ++) { - if (i == corn_coord_i[n] && - j == corn_coord_j[n]) - return n; - } - return -1; -} - -int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j) -{ - if (idx >= CORN_NB) - return -1; - *i = corn_coord_i[idx]; - *j = corn_coord_j[idx]; - return 0; -} - -/* return the index of the closest corn at these coordinates. If the - * corn is really too far (~20cm), return -1. The x and y pointer are - * updated with the real position of the corn */ -int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y) -{ - uint8_t idx = -1, n, i, j; - int16_t d, x_corn, y_corn; - int16_t x_corn_min = 0, y_corn_min = 0; - int16_t d_min = 0; - - for (n = 0; n < CORN_NB; n ++) { - corn_idx_to_coordij(n, &i, &j); - x_corn = (OFFSET_CORN_X + i*STEP_CORN_X); - y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y); - d = xy_norm(x_corn, y_corn, *x, *y); - if (d < 200 && (d_min == 0 || d < d_min)) { - d_min = d; - idx = n; - x_corn_min = x_corn; - y_corn_min = y_corn; - } - } - if (d_min == 0) - return -1; - - *x = x_corn_min; - *y = y_corn_min; - - return idx; -} - -int8_t corn_get_sym(int8_t i) -{ - if (i >= CORN_NB) - return -1; - return corn_sym[i]; -} - -void init_corn_table(int8_t conf_side, int8_t conf_center) -{ - int8_t sym, i; - - /* before match */ - if (conf_side == -1) { - for (i=0; ipos.x = x; - pt->pos.y = y; - - pt->type = TYPE_WAYPOINT; - pt->parent_pos = END; - pt->updated = 0; - pt->todo = 0; - - y += STEP_CORN_Y; - } - - x += STEP_CORN_X; - } -} - -/* update the type and weight of waypoints, before starting - * dijkstra */ -void update_waypoints(void) -{ - int8_t i, j, c; - struct djpoint *pt; - - for (i=0; iweight = 0; - - /* corn */ - c = ijcoord_to_corn_idx(i, j); - if (c >= 0) { - pt->type = corn_table[c]; - continue; - } - /* too close of border */ - if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { - pt->type = TYPE_OBSTACLE; - continue; - } - /* hill */ - if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { - pt->type = TYPE_OBSTACLE; - continue; - } - /* dangerous points */ - if (i == 0 || i == (WAYPOINTS_NBX-1)) { - pt->type = TYPE_DANGEROUS; - continue; - } - if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { - pt->type = TYPE_DANGEROUS; - continue; - } - pt->type = TYPE_WAYPOINT; - } - } -} - -int get_path(struct djpoint *start, struct djpoint *end) -{ - struct djpoint *cur; - uint8_t prev_direction = 0; - - for (cur=start; - cur != NULL && cur->parent_pos != END && cur != end; - cur = get_neigh(cur, cur->parent_pos)) { - if (prev_direction != cur->parent_pos) { - printf_P(PSTR("%d %d (%d)\r\n"), - cur->pos.x, cur->pos.y, - cur->parent_pos); - } - prev_direction = cur->parent_pos; - } - printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y); - - return 0; /* XXX */ -} - +#if 0 /* return 1 if there is a corn near, and fill the index ptr */ -static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) +uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) { #define SENSOR_CORN_DIST 225 #define SENSOR_CORN_ANGLE 90 @@ -568,6 +76,7 @@ static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) double a_rad = position_get_a_rad_double(&mainboard.pos); double x_corn, y_corn; int16_t x_corn_int, y_corn_int; + struct waypoint_db *wp; if (side == I2C_LEFT_SIDE) { x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; @@ -580,11 +89,12 @@ static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) x_corn_int = x_corn; y_corn_int = y_corn; - *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); - if (*corn_idx < 0) + wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); + if (wp->corn.idx < 0) return 0; return 1; } +#endif /* * - send the correct commands to the spickles @@ -592,9 +102,9 @@ static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) */ static uint8_t handle_spickles(void) { - int8_t corn_idx; - return 0; +#if 0 + int8_t corn_idx; if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); @@ -617,6 +127,7 @@ static uint8_t handle_spickles(void) } return 0; +#endif } uint8_t line2line(uint8_t dir1, uint8_t num1, @@ -678,16 +189,20 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, line1_a_deg, 150., diff_a_deg, beta_deg, radius, xy_norm(l1.p1.x, l1.p1.y, p.x, p.y)); - err = 0; - while (err == 0) { - err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); - if (err == 0) { - /* cobboard is stucked */ - trajectory_hardstop(&mainboard.traj); - return err; /* XXX do something */ + if (0) { + err = 0; + while (err == 0) { + err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); + if (err == 0) { + /* cobboard is stucked */ + trajectory_hardstop(&mainboard.traj); + return err; /* XXX do something */ + } + err = test_traj_end(0xFF); } - err = test_traj_end(0xFF); } + + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); return err; } @@ -737,26 +252,3 @@ void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) break; } } - - -#if 0 -int main(void) -{ - struct djpoint *start; - struct djpoint *end; - - start = &djpoints[1][1]; - end = &djpoints[12][1]; - - init_corn_table(0, 0); - init_waypoints(); - update_waypoints(); - - dijkstra(end); - dump(); - - get_path(start, end); - - return 0; -} -#endif diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h index def2237..ca4a7d6 100644 --- a/projects/microb2010/mainboard/strat_corn.h +++ b/projects/microb2010/mainboard/strat_corn.h @@ -20,15 +20,6 @@ * Olivier MATZ */ -#define CORN_NB 18 - -/* enum is better */ -#define TYPE_WAYPOINT 0 -#define TYPE_DANGEROUS 1 -#define TYPE_WHITE_CORN 2 -#define TYPE_BLACK_CORN 3 -#define TYPE_OBSTACLE 4 - #define LINE_UP 0 #define LINE_DOWN 1 #define LINE_R_UP 2 @@ -41,12 +32,7 @@ struct line_2pts { point_t p2; }; -extern uint8_t corn_table[CORN_NB]; - -int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j); -int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y); -void init_corn_table(int8_t conf_side, int8_t conf_center); +void num2line(struct line_2pts *l, uint8_t dir, uint8_t num); uint8_t line2line(uint8_t dir1, uint8_t num1, uint8_t dir2, uint8_t num2); -void num2line(struct line_2pts *l, uint8_t dir, uint8_t num); -- 2.39.5