From 28da5858ac871c626153f47566e968ecb05ff52b Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 10 May 2010 18:09:37 +0200 Subject: [PATCH] oranges and enhance strats --- projects/microb2010/ballboard/actuator.c | 26 +- projects/microb2010/ballboard/actuator.h | 6 +- .../microb2010/ballboard/commands_ballboard.c | 32 +-- projects/microb2010/ballboard/cs.c | 4 +- projects/microb2010/ballboard/main.c | 6 +- projects/microb2010/ballboard/state.c | 22 +- .../microb2010/cobboard/commands_cobboard.c | 8 +- projects/microb2010/cobboard/shovel.c | 21 +- projects/microb2010/cobboard/shovel.h | 3 +- projects/microb2010/cobboard/state.c | 17 +- projects/microb2010/common/i2c_commands.h | 22 +- projects/microb2010/mainboard/beacon.c | 5 +- .../microb2010/mainboard/commands_mainboard.c | 34 +-- projects/microb2010/mainboard/commands_traj.c | 27 +- projects/microb2010/mainboard/display.py | 2 +- projects/microb2010/mainboard/strat.c | 261 ++++++++++++++---- projects/microb2010/mainboard/strat.h | 5 +- projects/microb2010/mainboard/strat_avoid.c | 21 +- projects/microb2010/mainboard/strat_base.c | 52 +++- projects/microb2010/mainboard/strat_utils.h | 21 ++ 20 files changed, 416 insertions(+), 179 deletions(-) diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c index d745fa0..0dc89ff 100644 --- a/projects/microb2010/ballboard/actuator.c +++ b/projects/microb2010/ballboard/actuator.c @@ -47,13 +47,10 @@ #define ROLLER_OFF 0 #define ROLLER_REVERSE ROLLER_SPEED -#define FORKROT_DEPLOYED -50000 +#define FORKROT_DEPLOYED -55000 +#define FORKROT_MID -33000 #define FORKROT_PACKED 0 -#define FORKTRANS_LEFT 0 -#define FORKTRANS_MIDDLE 500 -#define FORKTRANS_RIGHT 1000 - void roller_on(void) { cs_set_consign(&ballboard.roller.cs, ROLLER_ON); @@ -79,17 +76,18 @@ void fork_pack(void) cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED); } -void fork_left(void) -{ - cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_LEFT); -} - -void fork_right(void) +void fork_mid(void) { - cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_RIGHT); + cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID); } -void fork_middle(void) +void actuator_init(void) { - cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_MIDDLE); + printf_P(PSTR("fork autopos...")); + pwm_ng_set(FORKROT_PWM, 400); + wait_ms(1000); + pwm_ng_set(FORKROT_PWM, 0); + encoders_spi_set_value(FORKROT_ENCODER, 0); + printf_P(PSTR("ok\r\n")); + ballboard.forkrot.on = 1; } diff --git a/projects/microb2010/ballboard/actuator.h b/projects/microb2010/ballboard/actuator.h index 039cb1d..bd93ddc 100644 --- a/projects/microb2010/ballboard/actuator.h +++ b/projects/microb2010/ballboard/actuator.h @@ -27,6 +27,6 @@ void roller_reverse(void); void fork_deploy(void); void fork_pack(void); -void fork_left(void); -void fork_right(void); -void fork_middle(void); +void fork_mid(void); + +void actuator_init(void); diff --git a/projects/microb2010/ballboard/commands_ballboard.c b/projects/microb2010/ballboard/commands_ballboard.c index ba570a4..70ae084 100644 --- a/projects/microb2010/ballboard/commands_ballboard.c +++ b/projects/microb2010/ballboard/commands_ballboard.c @@ -195,13 +195,17 @@ static void cmd_state1_parsed(void *parsed_result, state_set_mode(I2C_BALLBOARD_MODE_EJECT); else if (!strcmp_P(res->arg1, PSTR("harvest"))) state_set_mode(I2C_BALLBOARD_MODE_HARVEST); + else if (!strcmp_P(res->arg1, PSTR("prepare"))) + state_set_mode(I2C_BALLBOARD_MODE_PREP_FORK); + else if (!strcmp_P(res->arg1, PSTR("take"))) + state_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); /* other commands */ } prog_char str_state1_arg0[] = "ballboard"; parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0); -prog_char str_state1_arg1[] = "init#eject#harvest#off"; +prog_char str_state1_arg1[] = "init#eject#harvest#off#prepare#take"; parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); prog_char help_state1[] = "set ballboard mode"; @@ -230,27 +234,11 @@ struct cmd_state2_result { static void cmd_state2_parsed(void *parsed_result, __attribute__((unused)) void *data) { - struct cmd_state2_result *res = parsed_result; - uint8_t mode; - - if (!strcmp_P(res->arg2, PSTR("left"))) { - if (!strcmp_P(res->arg1, PSTR("prepare"))) - mode = I2C_BALLBOARD_MODE_PREP_L_FORK; - else if (!strcmp_P(res->arg1, PSTR("take"))) - mode = I2C_BALLBOARD_MODE_TAKE_L_FORK; - } - else { - if (!strcmp_P(res->arg1, PSTR("prepare"))) - mode = I2C_BALLBOARD_MODE_PREP_R_FORK; - else if (!strcmp_P(res->arg1, PSTR("take"))) - mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; - } - //state_set_mode(mode); } prog_char str_state2_arg0[] = "ballboard"; parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0); -prog_char str_state2_arg1[] = "prepare#take"; +prog_char str_state2_arg1[] = "xxx"; parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1); prog_char str_state2_arg2[] = "left#right"; parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2); @@ -391,17 +379,13 @@ static void cmd_fork_parsed(void *parsed_result, fork_deploy(); else if (!strcmp_P(res->arg1, PSTR("pack"))) fork_pack(); - else if (!strcmp_P(res->arg1, PSTR("left"))) - fork_left(); - else if (!strcmp_P(res->arg1, PSTR("right"))) - fork_right(); else if (!strcmp_P(res->arg1, PSTR("middle"))) - fork_middle(); + fork_mid(); } prog_char str_fork_arg0[] = "fork"; parse_pgm_token_string_t cmd_fork_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg0, str_fork_arg0); -prog_char str_fork_arg1[] = "deploy#pack#left#right#middle"; +prog_char str_fork_arg1[] = "deploy#pack#middle"; parse_pgm_token_string_t cmd_fork_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg1, str_fork_arg1); prog_char help_fork[] = "move fork"; diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index ed5c0ff..6bfd518 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -178,7 +178,7 @@ void microb_cs_init(void) /* QUADRAMP */ quadramp_init(&ballboard.forkrot.qr); - quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 800, 800); /* set speed */ + quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 800); /* set speed */ quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */ /* CS */ @@ -197,7 +197,7 @@ void microb_cs_init(void) /* set them on !! */ ballboard.roller.on = 1; ballboard.forktrans.on = 1; - ballboard.forkrot.on = 1; + ballboard.forkrot.on = 0; scheduler_add_periodical_event_priority(do_cs, NULL, diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index 925109f..e737ba3 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -212,8 +212,8 @@ int main(void) &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED, &PORTD, 5); - PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, - &PORTD, 6); + PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED | + PWM_NG_MODE_SIGN_INVERTED, &PORTD, 6); PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED, &PORTD, 7); @@ -255,6 +255,8 @@ int main(void) sei(); + actuator_init(); + printf_P(PSTR("\r\n")); printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n")); diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index a322d17..e777e49 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -64,10 +64,8 @@ static volatile uint8_t ball_count; #define OFF I2C_BALLBOARD_MODE_OFF #define HARVEST I2C_BALLBOARD_MODE_HARVEST #define EJECT I2C_BALLBOARD_MODE_EJECT -#define PREP_L_FORK I2C_BALLBOARD_MODE_PREP_L_FORK -#define TAKE_L_FORK I2C_BALLBOARD_MODE_TAKE_L_FORK -#define PREP_R_FORK I2C_BALLBOARD_MODE_PREP_R_FORK -#define TAKE_R_FORK I2C_BALLBOARD_MODE_TAKE_R_FORK +#define PREP_FORK I2C_BALLBOARD_MODE_PREP_FORK +#define TAKE_FORK I2C_BALLBOARD_MODE_TAKE_FORK uint8_t state_debug = 0; @@ -216,6 +214,7 @@ void state_machine(void) case INIT: state_init(); + fork_pack(); state_mode = OFF; state_status = I2C_BALLBOARD_STATUS_F_READY; break; @@ -223,20 +222,35 @@ void state_machine(void) case OFF: state_status = I2C_BALLBOARD_STATUS_F_READY; roller_off(); + fork_pack(); break; case HARVEST: state_status = I2C_BALLBOARD_STATUS_F_READY; + fork_pack(); state_do_harvest(); break; case EJECT: state_status = I2C_BALLBOARD_STATUS_F_BUSY; + fork_pack(); state_do_eject(); state_status = I2C_BALLBOARD_STATUS_F_READY; state_mode = HARVEST; break; + case PREP_FORK: + roller_off(); + fork_deploy(); + break; + + case TAKE_FORK: + roller_off(); + fork_mid(); + time_wait_ms(1300); + state_mode = OFF; + break; + default: break; } diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 3f37356..9f8b562 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -193,8 +193,10 @@ static void cmd_state1_parsed(void *parsed_result, state_init(); else if (!strcmp_P(res->arg1, PSTR("eject"))) state_set_mode(I2C_COBBOARD_MODE_EJECT); - else if (!strcmp_P(res->arg1, PSTR("kickstand"))) - state_set_mode(I2C_COBBOARD_MODE_KICKSTAND); + else if (!strcmp_P(res->arg1, PSTR("kickstand_up"))) + state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP); + else if (!strcmp_P(res->arg1, PSTR("kickstand_down"))) + state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN); else if (!strcmp_P(res->arg1, PSTR("ignore_i2c"))) state_set_i2c_ignore(1); else if (!strcmp_P(res->arg1, PSTR("care_i2c"))) @@ -205,7 +207,7 @@ static void cmd_state1_parsed(void *parsed_result, prog_char str_state1_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0); -prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c#kickstand"; +prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c#kickstand_up#kickstand_down"; parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); prog_char help_state1[] = "set cobboard mode"; diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c index 42f734c..1f38e1a 100644 --- a/projects/microb2010/cobboard/shovel.c +++ b/projects/microb2010/cobboard/shovel.c @@ -46,7 +46,8 @@ #define SHOVEL_DOWN 100 #define SHOVEL_MID 4500 #define SHOVEL_UP 11000 -#define SHOVEL_KICKSTAND 12800 +#define SHOVEL_KICKSTAND_UP 12800 +#define SHOVEL_KICKSTAND_DOWN 10000 static int32_t shovel_k1 = 1000; static int32_t shovel_k2 = 20; @@ -121,6 +122,7 @@ void shovel_set(void *mot, int32_t cmd) void shovel_down(void) { + shovel_current_limit_enable(0); quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500); quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 80); cs_set_consign(&cobboard.shovel.cs, SHOVEL_DOWN); @@ -128,6 +130,7 @@ void shovel_down(void) void shovel_mid(void) { + shovel_current_limit_enable(0); quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500); quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 80, 80); cs_set_consign(&cobboard.shovel.cs, SHOVEL_MID); @@ -135,6 +138,7 @@ void shovel_mid(void) void shovel_up(void) { + shovel_current_limit_enable(0); if (state_get_cob_count() <= 1) quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 2500); else @@ -143,11 +147,22 @@ void shovel_up(void) cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); } -void shovel_kickstand(void) +void shovel_kickstand_up(void) { + shovel_set_current_limit_coefs(1000, 20); + shovel_current_limit_enable(1); quadramp_set_1st_order_vars(&cobboard.shovel.qr, 200, 200); quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 10, 10); - cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND); + cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND_UP); +} + +void shovel_kickstand_down(void) +{ + shovel_set_current_limit_coefs(500, 0); + shovel_current_limit_enable(1); + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 200, 200); + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 10, 10); + cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND_DOWN); } uint8_t shovel_is_up(void) diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h index 3b01e77..0019d2d 100644 --- a/projects/microb2010/cobboard/shovel.h +++ b/projects/microb2010/cobboard/shovel.h @@ -32,7 +32,8 @@ void shovel_set(void *mot, int32_t cmd); void shovel_down(void); void shovel_mid(void); void shovel_up(void); -void shovel_kickstand(void); +void shovel_kickstand_up(void); +void shovel_kickstand_down(void); uint8_t shovel_is_up(void); uint8_t shovel_is_down(void); diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index d9e6533..2d65208 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -66,7 +66,8 @@ static uint8_t cob_count; #define INIT(mode) ((mode) == I2C_COBBOARD_MODE_INIT) #define HARVEST(mode) ((mode) == I2C_COBBOARD_MODE_HARVEST) #define EJECT(mode) ((mode) == I2C_COBBOARD_MODE_EJECT) -#define KICKSTAND(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND) +#define KICKSTAND_UP(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND_UP) +#define KICKSTAND_DOWN(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND_DOWN) uint8_t state_debug = 0; @@ -347,7 +348,6 @@ void state_machine(void) shovel_down(); servo_carry_close(); servo_door_close(); - shovel_current_limit_enable(0); /* pack/deply spickles, enable/disable roller */ cobroller_off(I2C_LEFT_SIDE); @@ -367,11 +367,18 @@ void state_machine(void) } /* help to climb the hill */ - if (KICKSTAND(state_mode)) { + if (KICKSTAND_UP(state_mode)) { + state_status = I2C_COBBOARD_STATUS_KICKSTAND_UP; servo_carry_open(); servo_door_open(); - shovel_current_limit_enable(1); - shovel_kickstand(); + shovel_kickstand_up(); + } + + if (KICKSTAND_DOWN(state_mode)) { + state_status = I2C_COBBOARD_STATUS_KICKSTAND_DOWN; + servo_carry_open(); + servo_door_open(); + shovel_kickstand_down(); } /* eject */ diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 5e4a449..312a18e 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -78,7 +78,8 @@ struct i2c_cmd_cobboard_set_mode { #define I2C_COBBOARD_MODE_HARVEST 0x01 /* harvest mode */ #define I2C_COBBOARD_MODE_EJECT 0x02 /* eject cobs */ #define I2C_COBBOARD_MODE_INIT 0x03 /* init state machine */ -#define I2C_COBBOARD_MODE_KICKSTAND 0x04 /* help to climb the hill */ +#define I2C_COBBOARD_MODE_KICKSTAND_UP 0x04 /* help to climb the hill */ +#define I2C_COBBOARD_MODE_KICKSTAND_DOWN 0x05 /* help to climb the hill */ uint8_t mode; }; @@ -91,10 +92,8 @@ struct i2c_cmd_ballboard_set_mode { #define I2C_BALLBOARD_MODE_OFF 0x01 #define I2C_BALLBOARD_MODE_HARVEST 0x02 #define I2C_BALLBOARD_MODE_EJECT 0x03 -#define I2C_BALLBOARD_MODE_PREP_L_FORK 0x04 -#define I2C_BALLBOARD_MODE_TAKE_L_FORK 0x05 -#define I2C_BALLBOARD_MODE_PREP_R_FORK 0x06 -#define I2C_BALLBOARD_MODE_TAKE_R_FORK 0x07 +#define I2C_BALLBOARD_MODE_PREP_FORK 0x04 +#define I2C_BALLBOARD_MODE_TAKE_FORK 0x05 uint8_t mode; }; @@ -124,12 +123,13 @@ struct i2c_ans_cobboard_status { /* mode type are defined above: I2C_COBBOARD_MODE_xxx */ uint8_t mode; -#define I2C_COBBOARD_STATUS_READY 0x00 -#define I2C_COBBOARD_STATUS_OFF 0x01 -#define I2C_COBBOARD_STATUS_LBUSY 0x02 -#define I2C_COBBOARD_STATUS_RBUSY 0x03 -#define I2C_COBBOARD_STATUS_EJECT 0x04 -#define I2C_COBBOARD_STATUS_KICKSTAND 0x05 +#define I2C_COBBOARD_STATUS_READY 0x00 +#define I2C_COBBOARD_STATUS_OFF 0x01 +#define I2C_COBBOARD_STATUS_LBUSY 0x02 +#define I2C_COBBOARD_STATUS_RBUSY 0x03 +#define I2C_COBBOARD_STATUS_EJECT 0x04 +#define I2C_COBBOARD_STATUS_KICKSTAND_UP 0x05 +#define I2C_COBBOARD_STATUS_KICKSTAND_DOWN 0x06 uint8_t status; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c index c661613..ea26951 100644 --- a/projects/microb2010/mainboard/beacon.c +++ b/projects/microb2010/mainboard/beacon.c @@ -162,8 +162,11 @@ static void beacon_event(void *dummy) IRQ_LOCK(flags); if (opp_age < 3) opp_age ++; - else + else { beaconboard.oppx = I2C_OPPONENT_NOT_THERE; + IRQ_UNLOCK(flags); + return; + } ia = opp_a; id = opp_d; diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index b43f05b..8a97000 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -512,7 +512,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) } else { #ifdef HOST_VERSION -#define PWM_INTERACT 300 +#define PWM_INTERACT 1200 #else #define PWM_INTERACT 1200 #endif @@ -738,13 +738,15 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); - else if (!strcmp_P(res->arg1, PSTR("kickstand"))) - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND); + else if (!strcmp_P(res->arg1, PSTR("kickstand_up"))) + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP); + else if (!strcmp_P(res->arg1, PSTR("kickstand_down"))) + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0); -prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand"; +prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand_up#kickstand_down"; parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1); prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)"; @@ -925,13 +927,17 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); else if (!strcmp_P(res->arg1, PSTR("harvest"))) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); + else if (!strcmp_P(res->arg1, PSTR("prepare"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK); + else if (!strcmp_P(res->arg1, PSTR("take"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); /* other commands */ } prog_char str_ballboard_setmode1_arg0[] = "ballboard"; parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0); -prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off"; +prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off#take#prepare"; parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1); prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)"; @@ -959,27 +965,11 @@ struct cmd_ballboard_setmode2_result { /* function called when cmd_ballboard_setmode2 is parsed successfully */ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) { - struct cmd_ballboard_setmode2_result *res = parsed_result; - uint8_t mode = I2C_BALLBOARD_MODE_INIT; - - if (!strcmp_P(res->arg2, PSTR("left"))) { - if (!strcmp_P(res->arg1, PSTR("prepare"))) - mode = I2C_BALLBOARD_MODE_PREP_L_FORK; - else if (!strcmp_P(res->arg1, PSTR("take"))) - mode = I2C_BALLBOARD_MODE_TAKE_L_FORK; - } - else { - if (!strcmp_P(res->arg1, PSTR("prepare"))) - mode = I2C_BALLBOARD_MODE_PREP_R_FORK; - else if (!strcmp_P(res->arg1, PSTR("take"))) - mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; - } - i2c_ballboard_set_mode(mode); } prog_char str_ballboard_setmode2_arg0[] = "ballboard"; parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0); -prog_char str_ballboard_setmode2_arg1[] = "prepare#take"; +prog_char str_ballboard_setmode2_arg1[] = "xxx"; parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1); prog_char str_ballboard_setmode2_arg2[] = "left#right"; parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2); diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 0dff9ed..09a59b7 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -885,7 +885,8 @@ static void cmd_position_parsed(void * parsed_result, void * data) else if (!strcmp_P(res->arg1, PSTR("set"))) { position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4); } - else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) { + else if (!strcmp_P(res->arg1, PSTR("autoset_blue")) || + !strcmp_P(res->arg1, PSTR("autoset_or_blue"))) { mainboard.our_color = I2C_COLOR_BLUE; #ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); @@ -893,7 +894,8 @@ static void cmd_position_parsed(void * parsed_result, void * data) #endif auto_position(); } - else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) { + else if (!strcmp_P(res->arg1, PSTR("autoset_yellow")) || + !strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) { mainboard.our_color = I2C_COLOR_YELLOW; #ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); @@ -902,6 +904,15 @@ static void cmd_position_parsed(void * parsed_result, void * data) auto_position(); } + if (!strcmp_P(res->arg1, PSTR("autoset_or_blue"))) { + strat_conf.flags |= STRAT_CONF_OUR_ORANGE; + prepare_hill(I2C_COLOR_BLUE, 340); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) { + strat_conf.flags |= STRAT_CONF_OUR_ORANGE; + prepare_hill(I2C_COLOR_YELLOW, 340); + } + /* else it's just a "show" */ printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), position_get_x_double(&mainboard.pos), @@ -911,7 +922,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) prog_char str_position_arg0[] = "position"; parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); -prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow"; +prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow#autoset_or_blue#autoset_or_yellow"; parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); prog_char help_position[] = "Show/reset (x,y,a) position"; @@ -1108,7 +1119,7 @@ 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[] = "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); +parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT8); prog_char help_strat_conf3[] = "configure strat options"; parse_pgm_inst_t cmd_strat_conf3 = { @@ -1151,8 +1162,10 @@ static void cmd_subtraj_parsed(void *parsed_result, void *data) 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("orange_yellow"))) + run_to_the_hills(I2C_COLOR_YELLOW); + else if (!strcmp_P(res->arg1, PSTR("orange_blue"))) + run_to_the_hills(I2C_COLOR_BLUE); else if (!strcmp_P(res->arg1, PSTR("tomato"))) { position_set(&mainboard.pos, 2625, COLOR_Y(1847), COLOR_A(0.00)); @@ -1164,7 +1177,7 @@ static void cmd_subtraj_parsed(void *parsed_result, void *data) 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[] = "test#orange#tomato"; +prog_char str_subtraj_arg1[] = "test#orange_yellow#orange_blue#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); diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index 476a5ba..787c1c2 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -380,7 +380,7 @@ while True: if not m: m = re.match("cobboard=%s,%s"%(INT,INT), l) if m: - #print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1])) + print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1])) side = int(m.groups()[0]) flags = int(m.groups()[1]) if side == 0: diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 19c6883..3e29f3a 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -58,6 +58,7 @@ #include "../common/i2c_commands.h" #include "i2c_protocol.h" #include "main.h" +#include "cs.h" #include "strat.h" #include "strat_db.h" #include "strat_base.h" @@ -179,8 +180,9 @@ static void check_tomato(void) 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()) { + if (strat_db.tomato_table[k]->present == 1 && + 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 @@ -237,8 +239,9 @@ static void check_corn(void) 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()) { + if (strat_db.corn_table[i]->present == 1 && + strat_db.corn_table[i]->time_removed != -1 && + strat_db.corn_table[i]->time_removed + 2 <= time_get_s()) { #ifdef HOST_VERSION printf("remove cob %d\n", i); #endif @@ -419,7 +422,7 @@ static uint8_t strat_eject(void) } /* go to eject point */ - trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); err = WAIT_COND_OR_TRAJ_END(speedify_eject(), TRAJ_FLAGS_NO_NEAR); /* err is never == 0 because speedify_eject() always return 0 */ @@ -546,6 +549,57 @@ static uint8_t need_more_elements(void) } #endif +/* get tomatoes near our goals (12,5 and 12,3) */ +uint8_t get_opp_oranges(void) +{ + int16_t x, y; + uint8_t i, j; + uint8_t err; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + /* only if oranges are present */ + if (strat_db.opp_oranges_count == 0) + return END_TRAJ; + + strat_db.opp_oranges_count = 0; + 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_DIST_FAST, SPEED_ANGLE_FAST); + + /* turn in the correct direction */ + trajectory_a_abs(&mainboard.traj, COLOR_A(-90)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + goto fail; + + trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597)); + err = wait_traj_end(TRAJ_FLAGS_STD); + if (!TRAJ_SUCCESS(err)) + goto fail; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + goto fail; + + err = run_to_the_hills(get_opponent_color()); + + fail: + strat_want_pack = 0; + return err; +} + /* get tomatoes near our goals (12,5 and 12,3) */ uint8_t get_orphan_tomatoes(void) { @@ -608,13 +662,6 @@ uint8_t get_orphan_tomatoes(void) 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; @@ -650,97 +697,169 @@ fail: 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; +#define HILL_LEN 1000 - strat_want_pack = 1; - strat_get_acc(&ad, &aa); - strat_get_speed(&sd, &sa); +#define HILL_ANGLE 0 +#define HILL_POSY_YELLOW 310 +#define HILL_POSY_BLUE 190 - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); +#define HILL_POSX_BALLS_DOWN1 830 +#define HILL_POSX_BALLS_DOWN2 920 +#define HILL_POSX_BALLS_DOWN3 730 +#define HILL_START_POSX 580 + +uint8_t prepare_hill(uint8_t orange_color, int16_t posx) +{ + int16_t startx, starty; + uint8_t our_color = get_color(); + uint8_t err; - 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; + startx = posx; else - startx = AREA_X - HILL_START_POSX; - trajectory_goto_xy_abs(&mainboard.traj, startx, COLOR_Y(starty)); + startx = AREA_X - posx; + trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty)); err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return err; /* turn to the hills */ if (orange_color == our_color) - trajectory_a_abs(&mainboard.traj, COLOR_A(0)); + trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE)); else - trajectory_a_abs(&mainboard.traj, COLOR_A(180)); + trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE)); err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return err; - strat_set_acc(3, 3); - strat_set_speed(300, 500); - bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 40); + return END_TRAJ; +} + +/* get oranges, must be called near game area */ +uint8_t run_to_the_hills(uint8_t orange_color) +{ + double aa, ad; + uint16_t sa, sd; + uint8_t err; + uint8_t our_color = get_color(); + int32_t p = pid_get_gain_P(&mainboard.angle.pid); + int32_t i = pid_get_gain_I(&mainboard.angle.pid); + int32_t d = pid_get_gain_D(&mainboard.angle.pid); + int32_t max_in = pid_get_max_in(&mainboard.angle.pid); + int32_t max_i = pid_get_max_I(&mainboard.angle.pid); + int32_t max_out = pid_get_max_out(&mainboard.angle.pid); + + 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); + err = prepare_hill(orange_color, HILL_START_POSX); + if (!TRAJ_SUCCESS(err)) + return err; + + strat_set_acc(5, ACC_ANGLE); + strat_set_speed(300, SPEED_ANGLE_SLOW); + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80); + bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80); + bd_set_speed_threshold(&mainboard.distance.bd, 10); support_balls_pack(); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK); + + /* decrease angle gains */ + pid_set_gains(&mainboard.angle.pid, 200, 0, 2000); /* here it is difficult to handle return values, because we * are on the hill */ + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_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("deploy\r\n")); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > + HILL_POSX_BALLS_DOWN1, + TRAJ_FLAGS_SMALL_DIST); + DEBUG(E_USER_STRAT, "deploy support balls"); support_balls_deploy(); - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP); + trajectory_only_a_rel(&mainboard.traj, 2); + err = WAIT_COND_OR_TE_TO(0, 0, 2200); - time_wait_ms(2000); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN); + strat_set_acc(3, 3); + strat_hardstop(); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); + + time_wait_ms(1800); /* 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")); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < + HILL_POSX_BALLS_DOWN2, + TRAJ_FLAGS_SMALL_DIST); + DEBUG(E_USER_STRAT, "pack support balls"); support_balls_pack(); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - printf_P(PSTR("deploy\r\n")); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < + HILL_POSX_BALLS_DOWN3, + TRAJ_FLAGS_SMALL_DIST); + DEBUG(E_USER_STRAT, "deploy support balls"); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + strat_set_acc(ad, aa); support_balls_deploy(); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + + /* wait to be near the wall */ + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200, + TRAJ_FLAGS_SMALL_DIST); + + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + + /* restore BD coefs */ + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20); + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20); + bd_set_speed_threshold(&mainboard.distance.bd, 60); /* 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); + err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST); if (orange_color == our_color) - position_set(&mainboard.pos, ROBOT_HALF_LENGTH_REAR, + strat_reset_pos(ROBOT_HALF_LENGTH_REAR, DO_NOT_SET_POS, COLOR_A(0)); else - position_set(&mainboard.pos, AREA_X - ROBOT_HALF_LENGTH_REAR, + strat_reset_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); + + trajectory_d_rel(&mainboard.traj, 250); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (orange_color == I2C_COLOR_YELLOW) + trajectory_a_rel(&mainboard.traj, 90); + else + trajectory_a_rel(&mainboard.traj, -90); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + time_wait_ms(200); + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST); + strat_reset_pos(DO_NOT_SET_POS, + COLOR_Y(ROBOT_HALF_LENGTH_REAR), + COLOR_A(90)); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + trajectory_d_rel(&mainboard.traj, 250); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); /* revert acceleration and speed */ - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + pid_set_gains(&mainboard.angle.pid, p, i, d); + pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out); 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; } @@ -752,6 +871,7 @@ uint8_t strat_main(void) /* get oranges */ if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) { err = run_to_the_hills(get_color()); + strat_db.our_oranges_count = 0; do_initturn = 0; } @@ -787,6 +907,25 @@ uint8_t strat_main(void) } } + /* if it's time to get opponent oranges, do it */ + if (time_get_s() > strat_conf.opp_orange) { + err = get_opp_oranges(); + if (err == END_ERROR) { + DEBUG(E_USER_STRAT, + "get_opp_oranges 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 oranges if it failed */ + strat_conf.opp_orange = 90; + strat_unblock(); + } + } + /**********************/ /* harvest on circuit */ /**********************/ diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 3305f1f..2fa2e5b 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -135,8 +135,8 @@ #define ACC_ANGLE 16. /* default speeds */ -#define SPEED_DIST_FAST 1500. -#define SPEED_ANGLE_FAST 1000. +#define SPEED_DIST_FAST 1200. +#define SPEED_ANGLE_FAST 800. #define SPEED_DIST_SLOW 500. #define SPEED_ANGLE_SLOW 500. #define SPEED_DIST_VERY_SLOW 200. @@ -174,6 +174,7 @@ uint8_t strat_main(void); void strat_event(void *dummy); void strat_event_enable(void); void strat_event_disable(void); +uint8_t prepare_hill(uint8_t orange_color, int16_t posx); uint8_t run_to_the_hills(uint8_t orange_color); uint8_t get_orphan_tomatoes(void); diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 2658b46..de85e30 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -1001,8 +1001,6 @@ uint8_t strat_harvest_circuit(void) 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); @@ -1040,7 +1038,6 @@ uint8_t strat_harvest_circuit(void) goto fail; strat_want_pack = 0; - printf("UNPACK UNPACK\n"); /* do all lines of circuit */ for (idx = 1; idx < len; idx ++) { @@ -1080,11 +1077,11 @@ const struct xy_point unblock_pts[] = { /* try to unblock in any situation */ uint8_t strat_unblock(void) { - int16_t x, y; + int16_t x, y, posx, posy; uint8_t i, j, k; uint16_t old_dspeed, old_aspeed; uint8_t err; - uint16_t d_min = 0xFFFF, d; + uint16_t d_min = 0x7FFF, d; const struct xy_point *pt; DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); @@ -1094,8 +1091,10 @@ uint8_t strat_unblock(void) strat_hardstop(); strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - x = position_get_x_s16(&mainboard.pos); - y = position_get_y_s16(&mainboard.pos); + posx = position_get_x_s16(&mainboard.pos); + posy = position_get_y_s16(&mainboard.pos); + x = posx; + y = posy; if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) x = -1; @@ -1104,14 +1103,11 @@ uint8_t strat_unblock(void) /* 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)); + d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y)); if (d < d_min) { d_min = d; x = pt->x; @@ -1119,6 +1115,8 @@ uint8_t strat_unblock(void) } } } + DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d", + __FUNCTION__, x, y); /* XXX if opponent is too close, go back, or wait ? */ @@ -1132,7 +1130,6 @@ uint8_t strat_unblock(void) return err; strat_set_speed(old_dspeed, old_aspeed); - strat_want_pack = 0; return END_TRAJ; } diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index acded68..8716486 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -156,6 +156,25 @@ void strat_reset_pos(int16_t x, int16_t y, int16_t a) if (a == DO_NOT_SET_POS) a = posa; + /* some issues with blocking on simulator */ +#ifdef HOST_VERSION + if (x == ROBOT_HALF_LENGTH_REAR) + x = ROBOT_HALF_LENGTH_REAR + 10; + if (x == AREA_X - ROBOT_HALF_LENGTH_REAR) + x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10; + if (y == ROBOT_HALF_LENGTH_REAR) + y = ROBOT_HALF_LENGTH_REAR + 10; + if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR) + y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10; + if (x == ROBOT_HALF_LENGTH_FRONT) + x = ROBOT_HALF_LENGTH_FRONT + 10; + if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT) + x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10; + if (y == ROBOT_HALF_LENGTH_FRONT) + y = ROBOT_HALF_LENGTH_FRONT + 10; + if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT) + y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10; +#endif DEBUG(E_USER_STRAT, "reset pos (%s%s%s)", x == DO_NOT_SET_POS ? "" : "x", y == DO_NOT_SET_POS ? "" : "y", @@ -176,14 +195,45 @@ uint8_t strat_calib(int16_t dist, uint8_t flags) int32_t max_in = pid_get_max_in(&mainboard.angle.pid); int32_t max_i = pid_get_max_I(&mainboard.angle.pid); int32_t max_out = pid_get_max_out(&mainboard.angle.pid); + uint32_t i_thres = mainboard.distance.bd.i_thres; + int32_t k1 = mainboard.distance.bd.k1; + int32_t k2 = mainboard.distance.bd.k2; + uint16_t cpt_thres = mainboard.distance.bd.cpt_thres; + uint8_t err; + /* go with a lower angle pid, and with a sensible blocking + * detection */ + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20); pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000); - pid_set_gains(&mainboard.angle.pid, 200, 0, 2000); + pid_set_gains(&mainboard.angle.pid, 500, 10, 4000); trajectory_d_rel(&mainboard.traj, dist); err = wait_traj_end(flags); + if (err != END_BLOCKING) + goto fail; + + /* we detected a blocking, reset bd, remove angle pid and + * continue */ + trajectory_d_rel(&mainboard.traj, dist); + pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095); + pid_set_gains(&mainboard.angle.pid, 1, 0, 0); + time_wait_ms(300); + strat_hardstop(); + +#ifdef HOST_VERSION + /* issue with block on simulator */ + if (dist > 0) + trajectory_d_rel(&mainboard.traj, -10); + else + trajectory_d_rel(&mainboard.traj, 10); + wait_traj_end(END_TRAJ); +#endif + + fail: pid_set_gains(&mainboard.angle.pid, p, i, d); pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out); + bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres); + return err; } diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h index f314b0e..2414c04 100644 --- a/projects/microb2010/mainboard/strat_utils.h +++ b/projects/microb2010/mainboard/strat_utils.h @@ -42,6 +42,27 @@ struct xy_point { __err; \ }) \ +#define WAIT_COND_OR_TE_TO(cond, mask, timeout) \ + ({ \ + microseconds __us = time_get_us2(); \ + uint8_t __ret = 0; \ + while ( (! (cond)) && (__ret == 0)) { \ + __ret = test_traj_end(mask); \ + if (time_get_us2() - __us > (timeout)*1000L) { \ + __ret = 0; \ + break; \ + } \ + } \ + if (!__ret) \ + DEBUG(E_USER_STRAT, "cond / timeout at line %d", \ + __LINE__); \ + else \ + DEBUG(E_USER_STRAT, "got %s (%d) at line %d", \ + get_err(__ret), __ret, __LINE__); \ + \ + __ret; \ + }) + 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); -- 2.20.1