diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index e3b194b..46f83c8 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -40,7 +40,10 @@ import frc.robot.Constants.RobotStatus; NamedCommands.registerCommand("intaker_up", intaker_up(intaker)); NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain)); NamedCommands.registerCommand("shoot", shoot(shooter)); - NamedCommands.registerCommand("auto_aim_and_shoot", autoAimAndShoot(driveControls, shooterControls)); + NamedCommands.registerCommand("Shake", Autocommand.shake(driveControls)); + NamedCommands.registerCommand("AutoAim", Autocommand.autoAim(driveControls)); + NamedCommands.registerCommand("AutoShoot", Autocommand.autoShoot(shooterControls)); + } public static Command climb_up(ClimberSubsystem climber) { @@ -131,10 +134,14 @@ import frc.robot.Constants.RobotStatus; shooter.setConveyorSpeedByRPS(0); // 停止输送 }, shooter))); } - - public static Command autoAimAndShoot(DriveControls driveControls, ShooterControls shooterControl) { - return driveControls.autoAimCommand() - .andThen(shooterControl.autoShootToHubCommand()); + public static Command autoAim(DriveControls driveControls) { + return driveControls.autoAimCommand(); + } + public static Command autoShoot(ShooterControls shooterControl) { + return shooterControl.autoShootToHubCommand(); + } + public static Command shake(DriveControls driveControls) { + return driveControls.shakeCommand(); } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 4e00c21..a73a46e 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -24,7 +24,8 @@ import frc.robot.subsystems.LimelightSupplier; import java.util.Optional; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; public class DriveControls { @@ -430,7 +431,29 @@ public class DriveControls { drivetrain.stop(); }, drivetrain)); } + public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) { + Timer timer = new Timer(); - - + return Commands.sequence( + new InstantCommand(timer::restart), + drivetrain.applyRequest(() -> { + double t = timer.get(); + boolean positive = ((int) (t / switchPeriod)) % 2 == 0; + double vy = positive ? amplitude : -amplitude; + return robotCentric + .withVelocityX(0.0) + .withVelocityY(vy) + .withRotationalRate(0.0); + }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime), + new InstantCommand(() -> { + timer.stop(); + autoControlNetworkTable.getEntry("ShakeVy").setDouble(0.0); + drivetrain.stop(); + }, drivetrain) + ); + } + public Command shakeCommand() { + return shakeCommand(0.25, 0.2, 3.0); + } + }