From 04f1061f2c5959ce87f632591576097f540dfc14 Mon Sep 17 00:00:00 2001 From: zer0 Date: Tue, 1 Jun 2010 19:03:07 +0200 Subject: [PATCH] french cup + idf cup --- projects/microb2010/ballboard/actuator.c | 34 +++++- projects/microb2010/ballboard/actuator.h | 5 +- .../microb2010/ballboard/commands_ballboard.c | 8 +- projects/microb2010/ballboard/cs.c | 6 +- projects/microb2010/ballboard/state.c | 26 ++++- projects/microb2010/cobboard/actuator.c | 2 +- projects/microb2010/cobboard/shovel.c | 13 ++- projects/microb2010/cobboard/shovel.h | 1 + projects/microb2010/cobboard/state.c | 54 ++++++++- projects/microb2010/mainboard/strat.c | 105 ++++++++++-------- projects/microb2010/mainboard/strat_base.c | 8 +- 11 files changed, 193 insertions(+), 69 deletions(-) diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c index c635fe9..cf9517f 100644 --- a/projects/microb2010/ballboard/actuator.c +++ b/projects/microb2010/ballboard/actuator.c @@ -48,7 +48,9 @@ #define ROLLER_REVERSE ROLLER_SPEED #define FORKROT_DEPLOYED -55000 -#define FORKROT_MID -33000 +#define FORKROT_MID1 -31000 +#define FORKROT_MID2 -27000 +#define FORKROT_EJECT -12000 #define FORKROT_PACKED -4000 void roller_on(void) @@ -76,9 +78,35 @@ void fork_pack(void) cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED); } -void fork_mid(void) +void fork_mid1(void) { - cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID); + cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID1); +} + +void fork_mid2(void) +{ + cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID2); +} + +void fork_eject(void) +{ + cs_set_consign(&ballboard.forkrot.cs, FORKROT_EJECT); +} + +static uint8_t fork_is_at_pos(int32_t pos) +{ + int32_t diff; + diff = pos - encoders_spi_get_value(FORKROT_ENCODER); + if (diff < 0) + diff = -diff; + if (diff < 500) + return 1; + return 0; +} + +uint8_t fork_is_packed(void) +{ + return fork_is_at_pos(FORKROT_PACKED); } void actuator_init(void) diff --git a/projects/microb2010/ballboard/actuator.h b/projects/microb2010/ballboard/actuator.h index bd93ddc..a8d282b 100644 --- a/projects/microb2010/ballboard/actuator.h +++ b/projects/microb2010/ballboard/actuator.h @@ -27,6 +27,9 @@ void roller_reverse(void); void fork_deploy(void); void fork_pack(void); -void fork_mid(void); +void fork_mid1(void); +void fork_mid2(void); +void fork_eject(void); +uint8_t fork_is_packed(void); void actuator_init(void); diff --git a/projects/microb2010/ballboard/commands_ballboard.c b/projects/microb2010/ballboard/commands_ballboard.c index bd7920a..95ec614 100644 --- a/projects/microb2010/ballboard/commands_ballboard.c +++ b/projects/microb2010/ballboard/commands_ballboard.c @@ -380,13 +380,15 @@ 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("middle"))) - fork_mid(); + else if (!strcmp_P(res->arg1, PSTR("mid1"))) + fork_mid1(); + else if (!strcmp_P(res->arg1, PSTR("mid2"))) + fork_mid2(); } 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#middle"; +prog_char str_fork_arg1[] = "deploy#pack#mid1#mid2"; 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 e1166da..35628d2 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -82,6 +82,7 @@ static void do_cs(void *dummy) cs_get_filtered_feedback(&ballboard.roller.cs), cs_get_out(&ballboard.roller.cs)); +#if 0 /* urgent case: stop power on blocking */ if (ballboard.flags & DO_ERRBLOCKING) { if (bd_get(&ballboard.forktrans.bd) || @@ -90,6 +91,7 @@ static void do_cs(void *dummy) ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); } } +#endif } if (ballboard.flags & DO_POWER) BRAKE_OFF(); @@ -181,8 +183,8 @@ void microb_cs_init(void) /* QUADRAMP */ quadramp_init(&ballboard.forkrot.qr); - quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 800); /* set speed */ - quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */ + quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 1000, 500); /* set speed */ + quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 100, 20); /* set accel */ /* CS */ cs_init(&ballboard.forkrot.cs); diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index 6c229a4..edcfb3a 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -166,9 +166,15 @@ static void state_do_eject(void) blocked = 0; while (1) { + /* move fork during ball ejection */ + if ((us % 600) < 300) + fork_eject(); + else + fork_pack(); /* no more balls (sensor is heavily filtered) */ - if (!sensor_get(S_LOW_BARRIER)) { + if (!sensor_get(S_LOW_BARRIER) && + !sensor_get(S_HIGH_BARRIER)) { STMCH_DEBUG("%s(): no more balls", __FUNCTION__); break; } @@ -186,6 +192,7 @@ static void state_do_eject(void) break; } } + fork_pack(); if (!blocked) break; @@ -246,8 +253,21 @@ void state_machine(void) case TAKE_FORK: roller_off(); - fork_mid(); - time_wait_ms(1300); + fork_mid1(); + time_wait_ms(666); + fork_mid2(); + time_wait_ms(666); + while (1) { + uint8_t packed; + + fork_pack(); + packed = WAIT_COND_OR_TIMEOUT(fork_is_packed(), + 500); + if (packed) + break; + fork_mid2(); + time_wait_ms(200); + } state_mode = OFF; break; diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 85b2ac1..9d3bdef 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -45,7 +45,7 @@ #include "main.h" #include "actuator.h" -#define COBROLLER_SPEED 666 +#define COBROLLER_SPEED 850 #define SERVO_DOOR_OPEN 300 #define SERVO_DOOR_CLOSED 525 diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c index 8714367..e06638d 100644 --- a/projects/microb2010/cobboard/shovel.c +++ b/projects/microb2010/cobboard/shovel.c @@ -46,8 +46,8 @@ #define SHOVEL_DOWN 100 #define SHOVEL_MID 4500 #define SHOVEL_UP 11300 -#define SHOVEL_KICKSTAND_UP 12800 -#define SHOVEL_KICKSTAND_DOWN 10000 +#define SHOVEL_KICKSTAND_UP 13400 +#define SHOVEL_KICKSTAND_DOWN 10400 static int32_t shovel_k1 = 1000; static int32_t shovel_k2 = 20; @@ -140,9 +140,9 @@ 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); + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 700, 2500); else - quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2000, 2500); + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 2500); quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 80, 15); cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); } @@ -175,6 +175,11 @@ uint8_t shovel_is_down(void) return shovel_is_at_pos(SHOVEL_DOWN); } +uint8_t shovel_is_mid(void) +{ + return shovel_is_at_pos(SHOVEL_MID); +} + void shovel_init(void) { shovel_autopos(); diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h index 0019d2d..0a61102 100644 --- a/projects/microb2010/cobboard/shovel.h +++ b/projects/microb2010/cobboard/shovel.h @@ -37,5 +37,6 @@ void shovel_kickstand_down(void); uint8_t shovel_is_up(void); uint8_t shovel_is_down(void); +uint8_t shovel_is_mid(void); #endif /* _SHOVEL_H_ */ diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 535829f..9c6af8c 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -70,6 +70,7 @@ static uint8_t cob_count; #define KICKSTAND_DOWN(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND_DOWN) uint8_t state_debug = 0; +static uint8_t state_cob_partial = 0; uint8_t state_get_cob_count(void) { @@ -98,6 +99,15 @@ static uint8_t state_no_cob_inside(void) !sensor_get(S_COB_INSIDE_R); } +static uint8_t state_cob_inside_enhanced(void) +{ + if (sensor_get(S_COB_INSIDE_L)) + state_cob_partial = 1; + if (sensor_get(S_COB_INSIDE_R)) + state_cob_partial = 1; + return state_cob_inside(); +} + static uint8_t state_spicklemode_deployed(uint8_t side) { if (side == I2C_LEFT_SIDE) @@ -214,6 +224,8 @@ uint8_t state_get_status(void) /* harvest cobs from area */ static void state_do_harvest(uint8_t side) { + uint8_t i = 0; + /* if there is no cob, return */ if (cob_falling_edge(side) == 0) return; @@ -232,11 +244,15 @@ static void state_do_harvest(uint8_t side) cobroller_on(side); /* check that cob is correctly in place */ - if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) { - if (state_no_cob_inside()) { + state_cob_partial = 0; + if (WAIT_COND_OR_TIMEOUT(state_cob_inside_enhanced(), 750) == 0) { + if (state_no_cob_inside() && state_cob_partial == 0) { STMCH_DEBUG("no cob"); return; } + else if (state_no_cob_inside() && state_cob_partial == 1) + goto cont; + STMCH_DEBUG("bad cob state"); /* while cob is not correctly placed try to extract @@ -256,6 +272,9 @@ static void state_do_harvest(uint8_t side) time_wait_ms(250); shovel_down(); time_wait_ms(250); + + if (EJECT(state_mode)) + return; } STMCH_DEBUG("cob removed"); @@ -265,6 +284,7 @@ static void state_do_harvest(uint8_t side) } } + cont: /* cob is inside, switch off roller */ cobroller_off(side); cob_count ++; @@ -289,14 +309,40 @@ static void state_do_harvest(uint8_t side) /* store it */ shovel_up(); + i = 0; while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 600) == 0) { STMCH_DEBUG("shovel blocked"); shovel_down(); - time_wait_ms(250); - shovel_up(); + + if (i == 4) + break; + /* if eject command is received, force exit */ if (EJECT(state_mode)) return; + + time_wait_ms(250); + shovel_up(); + i ++; + } + + /* bad state, try to eject to cobs */ + if (i == 4) { + servo_door_open(); + shovel_mid(); + + while (WAIT_COND_OR_TIMEOUT(shovel_is_mid(), 600) == 0) { + STMCH_DEBUG("ejecting cobs"); + + shovel_down(); + time_wait_ms(250); + if (state_no_cob_inside()) { + servo_door_close(); + cob_count = 0; + return; + } + shovel_mid(); + } } state_debug_wait_key_pressed(); diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 41e7e8b..6090ca8 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -223,6 +223,20 @@ static void check_tomato(void) #endif } +static void remove_cob(uint8_t idx) +{ + if (strat_db.corn_table[idx]->corn.color == I2C_COB_BLACK) + return; + + if (strat_db.corn_table[idx]->time_removed == -1) { + strat_db.corn_table[idx]->time_removed = time_get_s(); +#ifdef HOST_VERSION + cobboard.cob_count ++; + printf("add cob %d\n", idx); +#endif + } +} + /* mark corn as not present and give correct commands to the cobboard * for spickles */ static void check_corn(void) @@ -274,7 +288,7 @@ static void check_corn(void) lcob == I2C_COB_WHITE ? "white" : "black", lidx); corn_set_color(strat_db.corn_table[lidx], lcob); } - if (!__y_is_more_than(l_yspickle, 600)) + if (cur_time > 5 && !__y_is_more_than(l_yspickle, 600)) l_y_too_high_pack = 1; /* detect cob on right side */ @@ -286,37 +300,38 @@ static void check_corn(void) rcob == I2C_COB_WHITE ? "white" : "black", ridx); corn_set_color(strat_db.corn_table[ridx], rcob); } - if (!__y_is_more_than(r_yspickle, 600)) + if (cur_time > 5 && !__y_is_more_than(r_yspickle, 600)) r_y_too_high_pack = 1; + /* re-enable white cob */ + if (lcob == I2C_COB_WHITE && lcob_near && + strat_db.corn_table[lidx]->present == 0) { + strat_db.corn_table[lidx]->present = 1; + strat_db.corn_table[lidx]->time_removed = -1; + } + if (rcob == I2C_COB_WHITE && rcob_near && + strat_db.corn_table[ridx]->present == 0) { + strat_db.corn_table[ridx]->present = 1; + strat_db.corn_table[ridx]->time_removed = -1; + } + /* control the cobboard mode for left spickle */ need_lpack = get_cob_count() >= 5 || strat_want_pack || strat_lpack60 || strat_opponent_lpack || l_y_too_high_pack; - if (lcob_near && strat_db.corn_table[lidx]->present) { + if (lcob_near && + (strat_db.corn_table[lidx]->present || + strat_db.corn_table[lidx]->time_removed == -1)) { if (need_lpack) { /* nothing */ } else { /* deploy spickle and harvest white ones */ - if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) { + if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE); - 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); -#else - strat_db.corn_table[lidx]->present = 0; -#endif - } - } else i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE); + remove_cob(lidx); } } else { @@ -330,30 +345,19 @@ static void check_corn(void) /* control the cobboard mode for right spickle */ need_rpack = get_cob_count() >= 5 || strat_want_pack || strat_rpack60 || strat_opponent_rpack || r_y_too_high_pack; - if (rcob_near && strat_db.corn_table[ridx]->present) { + if (rcob_near && + (strat_db.corn_table[ridx]->present || + strat_db.corn_table[ridx]->time_removed == -1)) { if (need_rpack) { /* nothing */ } else { /* deploy spickle and harvest white ones */ - if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) { + if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE); - 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); -#else - strat_db.corn_table[ridx]->present = 0; -#endif - } - } else i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE); + remove_cob(ridx); } } else { @@ -522,8 +526,6 @@ static uint8_t strat_eject(void) #ifdef HOST_VERSION time_wait_ms(2000); #else - WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY, - 2000); WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY, 2000); #endif @@ -533,7 +535,9 @@ static uint8_t strat_eject(void) time_wait_ms(300); } - if (get_cob_count() > 0) { + if (get_cob_count() > 0 || + cobboard.status == I2C_COBBOARD_STATUS_LBUSY || + cobboard.status == I2C_COBBOARD_STATUS_RBUSY) { /* half turn */ trajectory_a_abs(&mainboard.traj, COLOR_A(-110)); err = wait_traj_end(END_INTR|END_TRAJ); @@ -574,7 +578,8 @@ static uint8_t strat_beginning(uint8_t do_initturn) if (do_initturn) { //strat_set_speed(600, 60); - strat_set_speed(450, 50); + //strat_set_speed(450, 50); + strat_set_speed(350, 40); trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20)); err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), TRAJ_FLAGS_STD); @@ -919,15 +924,15 @@ uint8_t run_to_the_hills(uint8_t orange_color) strat_hardstop(); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); - time_wait_ms(1100); + time_wait_ms(500); trajectory_d_rel(&mainboard.traj, 15); - time_wait_ms(400); + time_wait_ms(700); strat_hardstop(); time_wait_ms(200); /* reach top, go down */ - trajectory_d_rel(&mainboard.traj, -HILL_LEN); + trajectory_d_a_rel(&mainboard.traj, -HILL_LEN, -2); if (orange_color == our_color) err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < @@ -949,10 +954,18 @@ uint8_t run_to_the_hills(uint8_t orange_color) TRAJ_FLAGS_SMALL_DIST); DEBUG(E_USER_STRAT, "deploy support balls"); - strat_set_acc(ad, aa); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); support_balls_deploy(); + if (orange_color == I2C_COLOR_YELLOW) { + strat_set_acc(ad, aa); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + } + else { + strat_set_acc(ad, 0.4); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_a_rel(&mainboard.traj, -500, 20); + } + /* wait to be near the wall */ if (orange_color == our_color) err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300, @@ -962,6 +975,9 @@ uint8_t run_to_the_hills(uint8_t orange_color) AREA_X - 300, TRAJ_FLAGS_SMALL_DIST); + strat_set_acc(ad, aa); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + /* 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); @@ -984,6 +1000,7 @@ uint8_t run_to_the_hills(uint8_t orange_color) trajectory_d_rel(&mainboard.traj, -250); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); +#if 0 if (orange_color == our_color) trajectory_a_abs(&mainboard.traj, 180); else @@ -1005,7 +1022,7 @@ uint8_t run_to_the_hills(uint8_t orange_color) trajectory_d_rel(&mainboard.traj, -250); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - +#endif /* revert acceleration and speed */ pid_set_gains(&mainboard.angle.pid, p, i, d); pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out); diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 5a819c9..90762a6 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -456,8 +456,8 @@ uint8_t strat_obstacle(void) } /* opponent is in front of us */ - if ((mainboard.speed_d >= 0 && opp_d < 500 && (opp_a > 315 || opp_a < 45)) || - (mainboard.speed_d >= 0 && opp_d < 650 && (opp_a > 330 || opp_a < 30))) { + if ((mainboard.speed_d >= 0 && opp_d < 600 && (opp_a > 315 || opp_a < 45)) || + (mainboard.speed_d >= 0 && opp_d < 800 && (opp_a > 330 || opp_a < 30))) { DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d " "xrel=%d yrel=%d (speed_d=%d)", opp_d, opp_a, x_rel, y_rel, mainboard.speed_d); @@ -465,8 +465,8 @@ uint8_t strat_obstacle(void) return 1; } /* opponent is behind us */ - if ((mainboard.speed_d < 0 && opp_d < 500 && (opp_a < 225 && opp_a > 135)) || - (mainboard.speed_d < 0 && opp_d < 650 && (opp_a < 210 && opp_a > 150))) { + if ((mainboard.speed_d < 0 && opp_d < 600 && (opp_a < 225 && opp_a > 135)) || + (mainboard.speed_d < 0 && opp_d < 800 && (opp_a < 210 && opp_a > 150))) { DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", opp_d, opp_a, x_rel, y_rel); sensor_obstacle_disable(); -- 2.20.1