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);
DO_POS | DO_BD | DO_TIMER | DO_POWER;
}
-
/* call it after each strat */
void strat_exit(void)
{
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;
strat_set_speed(250, SPEED_ANGLE_FAST);
#endif
-
// strat_set_speed(600, 60); /* OK */
strat_set_speed(250, 28); /* OK */
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);
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);
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;
}
}
}
-static uint8_t strat_harvest(void)
-{
- return 0;
-}
-
-static uint8_t strat_eject(void)
-{
- return 0;
-}
-
uint8_t strat_main(void)
{
uint8_t err;