From 71deb6e552fcd65c853b35c5206ee47b735f2ff5 Mon Sep 17 00:00:00 2001 From: zer0 Date: Fri, 30 Apr 2010 21:42:49 +0200 Subject: [PATCH] ballboard: ramp on roller, and maybe fix race on state machine --- projects/microb2010/ballboard/cs.c | 6 ++ projects/microb2010/ballboard/state.c | 7 ++ projects/microb2010/mainboard/strat.c | 97 ++++++++++----------------- 3 files changed, 50 insertions(+), 60 deletions(-) diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index cf122e6..f6edc88 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -122,8 +122,14 @@ void microb_cs_init(void) pid_set_out_shift(&ballboard.roller.pid, 6); pid_set_derivate_filter(&ballboard.roller.pid, 6); + /* QUADRAMP (used as a ramp filter) */ + quadramp_init(&ballboard.roller.qr); + quadramp_set_1st_order_vars(&ballboard.roller.qr, 20, 20); + quadramp_set_2nd_order_vars(&ballboard.roller.qr, 0, 0); + /* CS */ cs_init(&ballboard.roller.cs); + cs_set_consign_filter(&ballboard.roller.cs, quadramp_do_filter, &ballboard.roller.qr); cs_set_correct_filter(&ballboard.roller.cs, pid_do_filter, &ballboard.roller.pid); cs_set_process_in(&ballboard.roller.cs, pwm_ng_set, ROLLER_PWM); cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER); diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index 57d44ff..08c104d 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -144,8 +144,15 @@ static void state_do_eject(void) /* main state machine */ void state_machine(void) { + uint8_t mode = 0; + while (state_want_exit() == 0) { + if (state_mode != mode) { + mode = state_mode; + STMCH_DEBUG("%s(): mode=%x ", __FUNCTION__, mode); + } + switch (state_mode) { case INIT: diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index ea84669..423b0f7 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -121,7 +121,6 @@ void strat_init(void) DO_POS | DO_BD | DO_TIMER | DO_POWER; } - /* call it after each strat */ void strat_exit(void) { @@ -209,6 +208,41 @@ void strat_event(void *dummy) strat_limit_speed(); } + +static uint8_t strat_harvest(void) +{ + return 0; +} + +static uint8_t strat_eject(void) +{ + trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + err = wait_traj_end(END_INTR|END_TRAJ); + + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + strat_hardstop(); + strat_set_speed(600, SPEED_ANGLE_FAST); + + /* ball ejection */ + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(2000); + + /* half turn */ + i2c_cobboard_pack(I2C_LEFT_SIDE); + i2c_cobboard_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_set_mode(I2C_COBBOARD_MODE_EJECT); + time_wait_ms(2000); + + return 0; +} + static uint8_t strat_beginning(void) { uint8_t err; @@ -221,7 +255,6 @@ static uint8_t strat_beginning(void) strat_set_speed(250, SPEED_ANGLE_FAST); #endif - // strat_set_speed(600, 60); /* OK */ strat_set_speed(250, 28); /* OK */ @@ -256,11 +289,6 @@ static uint8_t strat_beginning(void) goto l2; } - WAIT_COND_OR_TRAJ_END(distance_from_robot(2625, COLOR_Y(1847)) < 100, - TRAJ_FLAGS_STD); - trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); - err = wait_traj_end(END_INTR|END_TRAJ); - #else strat_set_speed(600, SPEED_ANGLE_FAST); err = line2line(LINE_UP, 0, LINE_R_DOWN, 3); @@ -277,26 +305,7 @@ static uint8_t strat_beginning(void) return END_TRAJ; #endif - DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); - strat_hardstop(); - strat_set_speed(600, SPEED_ANGLE_FAST); - - /* ball ejection */ - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); - time_wait_ms(2000); - - /* half turn */ - i2c_cobboard_pack(I2C_LEFT_SIDE); - i2c_cobboard_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_set_mode(I2C_COBBOARD_MODE_EJECT); - time_wait_ms(2000); + strat_eject(); strat_set_speed(250, SPEED_ANGLE_FAST); @@ -330,29 +339,7 @@ static uint8_t strat_beginning(void) WAIT_COND_OR_TRAJ_END(distance_from_robot(2625, COLOR_Y(1847)) < 100, TRAJ_FLAGS_STD); - trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); - err = wait_traj_end(END_INTR|END_TRAJ); - - DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); - strat_hardstop(); - strat_set_speed(600, SPEED_ANGLE_FAST); - - /* ball ejection */ - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); - time_wait_ms(2000); - - /* half turn */ - i2c_cobboard_pack(I2C_LEFT_SIDE); - i2c_cobboard_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_set_mode(I2C_COBBOARD_MODE_EJECT); - time_wait_ms(2000); + strat_eject(); return END_TRAJ; } @@ -390,16 +377,6 @@ static uint8_t need_more_elements(void) } } -static uint8_t strat_harvest(void) -{ - return 0; -} - -static uint8_t strat_eject(void) -{ - return 0; -} - uint8_t strat_main(void) { uint8_t err; -- 2.39.5