From d78a81c0df6dfe318d873a6f877e53cfb957ad8e Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 13 May 2010 06:40:42 +0200 Subject: [PATCH] cup1 --- projects/microb2010/ballboard/actuator.c | 2 +- projects/microb2010/ballboard/cs.c | 2 +- projects/microb2010/ballboard/state.c | 2 +- projects/microb2010/cobboard/spickle.c | 4 +-- projects/microb2010/cobboard/state.c | 3 +++ projects/microb2010/mainboard/beacon.c | 17 ++++++++----- projects/microb2010/mainboard/main.h | 2 +- projects/microb2010/mainboard/strat.c | 12 ++++++++- projects/microb2010/mainboard/strat_base.c | 8 +++--- projects/microb2010/mainboard/uart_config.h | 28 ++++++++++----------- 10 files changed, 50 insertions(+), 30 deletions(-) diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c index 0dc89ff..c635fe9 100644 --- a/projects/microb2010/ballboard/actuator.c +++ b/projects/microb2010/ballboard/actuator.c @@ -49,7 +49,7 @@ #define FORKROT_DEPLOYED -55000 #define FORKROT_MID -33000 -#define FORKROT_PACKED 0 +#define FORKROT_PACKED -4000 void roller_on(void) { diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index 6bfd518..262f73c 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -140,7 +140,7 @@ void microb_cs_init(void) bd_init(&ballboard.roller.bd); #define ROLLER_SPEED_THRES (ROLLER_SPEED * 0.75) bd_set_speed_threshold(&ballboard.roller.bd, ROLLER_SPEED_THRES); - bd_set_current_thresholds(&ballboard.roller.bd, 500, 1500, 1000000, 20); + bd_set_current_thresholds(&ballboard.roller.bd, 500, 1500, 1200000, 70); /* ---- CS forktrans */ /* PID */ diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index e777e49..6c229a4 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -174,7 +174,7 @@ static void state_do_eject(void) } /* timeout */ - if ((time_get_us2() - us) > 1000UL * 1000UL) { + if ((time_get_us2() - us) > 2000UL * 1000UL) { STMCH_DEBUG("%s(): eject timeout", __FUNCTION__); blocked = 1; break; diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index 058a7b7..e04e18f 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -72,8 +72,8 @@ static struct spickle_params spickle = { &cobboard.right_spickle, }, .pos_deployed = { - 500,// 7000, // 200, /* left */ - 500,// 7000, // 200, /* right */ + 7000, // 200, /* left */ + 7000, // 200, /* right */ }, .pos_mid = { 25000, /* left */ diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 2d65208..535829f 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -294,6 +294,9 @@ static void state_do_harvest(uint8_t side) shovel_down(); time_wait_ms(250); shovel_up(); + /* if eject command is received, force exit */ + if (EJECT(state_mode)) + return; } state_debug_wait_key_pressed(); diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c index 13cd4cd..cc78ca7 100644 --- a/projects/microb2010/mainboard/beacon.c +++ b/projects/microb2010/mainboard/beacon.c @@ -55,7 +55,7 @@ #include "main.h" #include "strat_utils.h" -#define BEACON_UART_NUM 0 +#define BEACON_UART_NUM 2 #define INIT 0 #define OPP0 1 @@ -69,6 +69,8 @@ #define STA4 9 #define STA5 10 +#define BEACON_ANGLE_OFFSET 449 + static volatile uint8_t opp_age = 0; static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE; static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE; @@ -78,7 +80,7 @@ static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND; static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND; static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND; -#define BEACON_OFFSET (-50.) +#define BEACON_POS_OFFSET (-50.) int8_t beacon_get_pos_double(double *x, double *y, double *a_rad) { uint8_t flags; @@ -98,8 +100,8 @@ int8_t beacon_get_pos_double(double *x, double *y, double *a_rad) dtmpy = tmpy; dtmpa = RAD((double)tmpa / 10.); - dtmpx += cos(dtmpa) * BEACON_OFFSET; - dtmpx += sin(dtmpa) * BEACON_OFFSET; + dtmpx += cos(dtmpa) * BEACON_POS_OFFSET; + dtmpx += sin(dtmpa) * BEACON_POS_OFFSET; *x = dtmpx; *y = dtmpy; @@ -215,7 +217,7 @@ static void beacon_opponent_event(void) /* if beacon is too old, remove it */ IRQ_LOCK(flags); - if (opp_age < 3) + if (opp_age < 50) opp_age ++; else { beaconboard.oppx = I2C_OPPONENT_NOT_THERE; @@ -227,6 +229,9 @@ static void beacon_opponent_event(void) id = opp_d; IRQ_UNLOCK(flags); + ia = (ia + BEACON_ANGLE_OFFSET); + if (ia > 3600) + ia -= 3600; fa = ia; fa = RAD(fa); fd = id; @@ -238,7 +243,7 @@ static void beacon_opponent_event(void) IRQ_LOCK(flags); beaconboard.oppx = ix; beaconboard.oppy = iy; - beaconboard.oppa = ia; + beaconboard.oppa = ia / 10; beaconboard.oppd = id; IRQ_UNLOCK(flags); #endif diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 03bc688..f98bd99 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -93,7 +93,7 @@ * and diameter: 55mm -> perimeter 134mm * dist_imp_mm = 4096/134 x 10 -> 304 */ /* increase it to go further */ -#define IMP_ENCODERS 1024 +#define IMP_ENCODERS 1000 #define WHEEL_DIAMETER_MM 42.9 #define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI) #define IMP_COEF 10. diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 64562f5..7f7b9d9 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -501,6 +501,7 @@ static uint8_t strat_eject(void) /* ball ejection */ if (get_ball_count() > 0) { i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(300); trajectory_a_abs(&mainboard.traj, COLOR_A(70)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) @@ -517,6 +518,10 @@ static uint8_t strat_eject(void) 2000); #endif } + else { + /* to pack spickles */ + time_wait_ms(300); + } if (get_cob_count() > 0) { /* half turn */ @@ -904,7 +909,12 @@ uint8_t run_to_the_hills(uint8_t orange_color) strat_hardstop(); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); - time_wait_ms(1800); + time_wait_ms(1100); + + trajectory_d_rel(&mainboard.traj, 15); + time_wait_ms(400); + strat_hardstop(); + time_wait_ms(200); /* reach top, go down */ trajectory_d_rel(&mainboard.traj, -HILL_LEN); diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index d68f5a6..023db36 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -414,11 +414,12 @@ uint8_t strat_obstacle(void) y_rel = sin(RAD(opp_a)) * (double)opp_d; /* opponent too far */ - if (opp_d > 500) + if (opp_d > 650) return 0; /* opponent is in front of us */ - if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) { + if ((mainboard.speed_d > 0 && opp_d < 500 && (opp_a > 325 || opp_a < 35)) && + (mainboard.speed_d > 0 && opp_d < 650 && (opp_a > 340 || opp_a < 20))) { 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); @@ -426,7 +427,8 @@ uint8_t strat_obstacle(void) return 1; } /* opponent is behind us */ - if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) { + if ((mainboard.speed_d < 0 && opp_d < 500 && (opp_a < 215 && opp_a > 145)) && + (mainboard.speed_d < 0 && opp_d < 650 && (opp_a < 200 && opp_a > 160))) { 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(); diff --git a/projects/microb2010/mainboard/uart_config.h b/projects/microb2010/mainboard/uart_config.h index dc6117a..52a083a 100644 --- a/projects/microb2010/mainboard/uart_config.h +++ b/projects/microb2010/mainboard/uart_config.h @@ -27,33 +27,33 @@ #define UART_CONFIG_H /* - * UART0 definitions + * UART2 definitions */ -/* compile uart0 fonctions, undefine it to pass compilation */ -#define UART0_COMPILE +/* compile uart2 fonctions, undefine it to pass compilation */ +#define UART2_COMPILE -/* enable uart0 if == 1, disable if == 0 */ -#define UART0_ENABLED 1 +/* enable uart2 if == 1, disable if == 0 */ +#define UART2_ENABLED 1 -/* enable uart0 interrupts if == 1, disable if == 0 */ -#define UART0_INTERRUPT_ENABLED 1 +/* enable uart2 interrupts if == 1, disable if == 0 */ +#define UART2_INTERRUPT_ENABLED 1 -#define UART0_BAUDRATE 57600 +#define UART2_BAUDRATE 57600 /* * if you enable this, the maximum baudrate you can reach is * higher, but the precision is lower. */ -#define UART0_USE_DOUBLE_SPEED 1 +#define UART2_USE_DOUBLE_SPEED 1 -#define UART0_RX_FIFO_SIZE 64 -#define UART0_TX_FIFO_SIZE 64 -#define UART0_NBITS 8 +#define UART2_RX_FIFO_SIZE 64 +#define UART2_TX_FIFO_SIZE 64 +#define UART2_NBITS 8 -#define UART0_PARITY UART_PARTITY_NONE +#define UART2_PARITY UART_PARTITY_NONE -#define UART0_STOP_BIT UART_STOP_BITS_1 +#define UART2_STOP_BIT UART_STOP_BITS_1 /* * UART1 definitions -- 2.20.1