From 30ab62f348de71844fb1db75ca24e4e680c68220 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 14 Nov 2024 17:30:27 -0800 Subject: [PATCH 1/3] Add amp4Auto and helper commands. Nonfunctional. --- src/main/java/frc/robot/autonomous/Autos.java | 153 ++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100644 src/main/java/frc/robot/autonomous/Autos.java diff --git a/src/main/java/frc/robot/autonomous/Autos.java b/src/main/java/frc/robot/autonomous/Autos.java new file mode 100644 index 00000000..d6c20c23 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/Autos.java @@ -0,0 +1,153 @@ +package frc.robot.autonomous; + +import com.choreo.lib.Choreo; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.carriage.CarriageSubsystem; +import frc.robot.subsystems.feeder.FeederSubsystem; +import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.utils.autoaim.AutoAim; + +public class Autos { + + private IntakeSubsystem intake; + private ShooterSubsystem shooter; + private FeederSubsystem feeder; + private SwerveSubsystem swerve; + private CarriageSubsystem carriage; + + private boolean isInitialized = false; + + private static final Autos instance = new Autos(); + + private Autos() {} + + public static Autos getInstance() { + if (!instance.isInitialized) { + // TODO: INITIALIZE + } + return instance; + } + + // AUTOS ------------------------------------------------------------------- + + public Command amp4Auto() { + return Commands.sequence( + autoFenderShot(), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 4 local.1"), true) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .withTimeout(1.0), + autoStaticAutoAim() + .andThen(Commands.print("Done with auto static auto aim")) + .beforeStarting(Commands.print("Before auto static auto aim!!!")), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 4 local.2")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .withTimeout(1.0), + autoStaticAutoAim(), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 4 local.3")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake().until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()), + autoStaticAutoAim() + ); + } + + // UTIL COMMANDS --------------------------------------------------------------- + + private Command autoFenderShot() { + return shooter + .runStateCmd( + AutoAim.FENDER_SHOT.getRotation(), + AutoAim.FENDER_SHOT.getLeftRPS(), + AutoAim.FENDER_SHOT.getRightRPS()) + .raceWith( + feeder + .runVelocityCmd(0.0) + .until(shooter::isAtGoal) + .andThen( + feeder + .runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY) + .raceWith( + Commands.waitUntil(() -> !feeder.getFirstBeambreak()) + .andThen(Commands.waitSeconds(0.1))))); + } + + private Command autoIntake() { + return Commands.parallel( + intake + .runVelocityCmd(50.0, 30.0) + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .asProxy(), + feeder.indexCmd().asProxy(), + carriage + .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE) + .until(feeder::getFirstBeambreak) + .asProxy(), + shooter + .runStateCmd( + () -> + Rotation2d.fromDegrees( + MathUtil.clamp( + AutoAim.shotMap + .get(swerve.getDistanceToSpeaker()) + .getRotation() + .getDegrees(), + 0, + 20)), + () -> 0.0, + () -> 0.0) + .until(feeder::getFirstBeambreak) + .unless(feeder::getFirstBeambreak) + .andThen( + shooter.runStateCmd( + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS())) + .asProxy()); + } + + private Command autoStaticAutoAim() { + return feeder + .indexCmd() + .deadlineWith( + swerve.runVelocityCmd(() -> new ChassisSpeeds()), + Commands.repeatingSequence( + shooter + .runFlywheelsCmd(() -> 0.0, () -> 0.0) + .unless(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0) + .until(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0), + shooter.runStateCmd( + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS()))) + .until(() -> feeder.getFirstBeambreak()) + .unless(() -> feeder.getFirstBeambreak()) + .withTimeout(1.0) + .andThen( + // FIXME + staticAutoAim(6.0) + .deadlineWith( + intake.runVoltageCmd(0, 0).asProxy(), carriage.runVoltageCmd(0).asProxy()) + .withTimeout(2.0) + .unless(() -> !feeder.getFirstBeambreak())) + .asProxy() + .withTimeout(4.0); + } + + + +} From 8fad9b3c82e5b5c3057c15fd656e73aa0d1900b0 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 14 Nov 2024 17:57:56 -0800 Subject: [PATCH 2/3] Add all the autos --- src/main/java/frc/robot/Robot.java | 198 +-------- src/main/java/frc/robot/autonomous/Autos.java | 414 ++++++++++++------ 2 files changed, 301 insertions(+), 311 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 614e401e..3d749301 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; -import com.choreo.lib.Choreo; import com.choreo.lib.ChoreoTrajectory; import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.hal.simulation.RoboRioDataJNI; @@ -18,7 +17,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.Timer; @@ -29,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.autonomous.Autos; import frc.robot.subsystems.carriage.CarriageIOReal; import frc.robot.subsystems.carriage.CarriageSubsystem; import frc.robot.subsystems.elevator.ElevatorIOReal; @@ -188,6 +187,9 @@ public void robotInit() { // be added. SignalLogger.setPath("/media/sda1/"); + // Initialize the Autos singleton + Autos.init(intake, shooter, feeder, swerve, carriage, this::staticAutoAim); + // Default Commands here swerve.setDefaultCommand( swerve.runVoltageTeleopFieldRelative( @@ -452,12 +454,12 @@ public void robotInit() { autoChooser.addOption("None", Commands.none()); autoChooser.addOption("Shoot Preload", teleopAutoAim()); - autoChooser.addDefaultOption("Amp 4 Wing", autoAmp4Wing()); - autoChooser.addOption("Source 3", autoSource3()); - autoChooser.addOption("Center 4", autoCenter4()); - autoChooser.addOption("Amp 5", autoAmp5()); - autoChooser.addOption("Source 4", autoSource4()); - autoChooser.addOption("Line Test Repeatedly", lineTest()); + autoChooser.addDefaultOption("Amp 4 Wing", Autos.getInstance().amp4()); + autoChooser.addOption("Source 3", Autos.getInstance().source3()); + autoChooser.addOption("Center 4", Autos.getInstance().center4()); + autoChooser.addOption("Amp 5", Autos.getInstance().amp5()); + autoChooser.addOption("Source 4", Autos.getInstance().source4()); + autoChooser.addOption("Line Test Repeatedly", Autos.getInstance().lineTest()); // Dashboard command buttons SmartDashboard.putData("Shooter shoot", shootWithDashboard()); @@ -777,186 +779,6 @@ private Command autoIntake() { .asProxy()); } - private Command autoAmp4Wing() { - return Commands.sequence( - autoFenderShot(), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 4 local.1"), true) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .withTimeout(1.0), - autoStaticAutoAim() - .andThen(Commands.print("Done with auto static auto aim")) - .beforeStarting(Commands.print("Before auto static auto aim!!!")), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 4 local.2")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .withTimeout(1.0), - autoStaticAutoAim(), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 4 local.3")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake().until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()), - autoStaticAutoAim()); - } - - private Command autoAmp5() { - return Commands.sequence( - autoFenderShot(), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 5.1"), true) - .asProxy() - .deadlineWith(autoIntake()), - autoStaticAutoAim(), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 5.2")) - .asProxy() - .deadlineWith(Commands.waitSeconds(1.0).andThen(autoIntake())), - autoStaticAutoAim(), - swerve.runChoreoTraj(Choreo.getTrajectory("amp 5.3")).asProxy().deadlineWith(autoIntake()), - autoIntake() - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .withTimeout(1.0), - autoStaticAutoAim(), - swerve.runChoreoTraj(Choreo.getTrajectory("amp 5.4")).asProxy().deadlineWith(autoIntake()), - autoIntake() - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .withTimeout(1.0), - autoStaticAutoAim()); - } - - private Command autoCenter4() { - return Commands.sequence( - autoFenderShot(), - swerve - .runChoreoTraj(Choreo.getTrajectory("center 4.1"), true) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), - swerve - .runChoreoTraj(Choreo.getTrajectory("center 4.2")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), - swerve - .runChoreoTraj(Choreo.getTrajectory("center 4.3")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim()); - } - - private Command autoSource3() { - return Commands.sequence( - autoFenderShot(), - swerve - .runChoreoTraj(Choreo.getTrajectory("source 3.1"), true) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), - swerve - .runChoreoTraj(Choreo.getTrajectory("source 3.2")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim()); - } - - private Command autoSource4() { - return Commands.sequence( - autoFenderShot(), - swerve - .runChoreoTraj(Choreo.getTrajectory("source 4.1"), true) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), - swerve - .runChoreoTraj(Choreo.getTrajectory("source 4.2")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), - swerve - .runChoreoTraj(Choreo.getTrajectory("source 4.3")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .raceWith( - Commands.sequence( - Commands.waitSeconds(0.25), - Commands.waitUntil( - () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) - .withTimeout(1.0), - autoStaticAutoAim()); - } - - private Command lineTest() { - ChoreoTrajectory traj = Choreo.getTrajectory("line test"); - return Commands.sequence( - Commands.runOnce( - () -> { - if (DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get().equals(Alliance.Red)) { - swerve.setPose(traj.flipped().getInitialPose()); - } else { - swerve.setPose(traj.getInitialPose()); - } - }), - swerve.runChoreoTraj(traj).repeatedly()); - } - private Command ampHeadingSnap(DoubleSupplier x, DoubleSupplier y) { var headingController = new ProfiledPIDController( diff --git a/src/main/java/frc/robot/autonomous/Autos.java b/src/main/java/frc/robot/autonomous/Autos.java index d6c20c23..3fd0fa7e 100644 --- a/src/main/java/frc/robot/autonomous/Autos.java +++ b/src/main/java/frc/robot/autonomous/Autos.java @@ -1,9 +1,11 @@ package frc.robot.autonomous; import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.carriage.CarriageSubsystem; @@ -12,142 +14,308 @@ import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.autoaim.AutoAim; +import java.util.function.Supplier; public class Autos { - private IntakeSubsystem intake; - private ShooterSubsystem shooter; - private FeederSubsystem feeder; - private SwerveSubsystem swerve; - private CarriageSubsystem carriage; + private IntakeSubsystem intake; + private ShooterSubsystem shooter; + private FeederSubsystem feeder; + private SwerveSubsystem swerve; + private CarriageSubsystem carriage; - private boolean isInitialized = false; + private Supplier staticAutoAim; - private static final Autos instance = new Autos(); + private static boolean isInitialized = false; - private Autos() {} + private static Autos instance = new Autos(); - public static Autos getInstance() { - if (!instance.isInitialized) { - // TODO: INITIALIZE - } - return instance; - } + private Autos() {} - // AUTOS ------------------------------------------------------------------- - - public Command amp4Auto() { - return Commands.sequence( - autoFenderShot(), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 4 local.1"), true) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .withTimeout(1.0), - autoStaticAutoAim() - .andThen(Commands.print("Done with auto static auto aim")) - .beforeStarting(Commands.print("Before auto static auto aim!!!")), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 4 local.2")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake() - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .withTimeout(1.0), - autoStaticAutoAim(), - swerve - .runChoreoTraj(Choreo.getTrajectory("amp 4 local.3")) - .asProxy() - .deadlineWith(autoIntake()), - autoIntake().until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()), - autoStaticAutoAim() - ); - } + public static void init( + IntakeSubsystem intakeSubsystem, + ShooterSubsystem shooterSubsystem, + FeederSubsystem feederSubsystem, + SwerveSubsystem swerveSubsystem, + CarriageSubsystem carriageSubsystem, + Supplier staticAutoAim) { + isInitialized = true; + instance.intake = intakeSubsystem; + instance.shooter = shooterSubsystem; + instance.feeder = feederSubsystem; + instance.swerve = swerveSubsystem; + instance.carriage = carriageSubsystem; + instance.staticAutoAim = staticAutoAim; + } - // UTIL COMMANDS --------------------------------------------------------------- - - private Command autoFenderShot() { - return shooter - .runStateCmd( - AutoAim.FENDER_SHOT.getRotation(), - AutoAim.FENDER_SHOT.getLeftRPS(), - AutoAim.FENDER_SHOT.getRightRPS()) - .raceWith( - feeder - .runVelocityCmd(0.0) - .until(shooter::isAtGoal) - .andThen( - feeder - .runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY) - .raceWith( - Commands.waitUntil(() -> !feeder.getFirstBeambreak()) - .andThen(Commands.waitSeconds(0.1))))); + public static Autos getInstance() { + if (!isInitialized) { + throw new NullPointerException("Instance is not initialized!"); } + return instance; + } - private Command autoIntake() { - return Commands.parallel( - intake - .runVelocityCmd(50.0, 30.0) - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .asProxy(), - feeder.indexCmd().asProxy(), - carriage - .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE) - .until(feeder::getFirstBeambreak) - .asProxy(), - shooter - .runStateCmd( - () -> - Rotation2d.fromDegrees( - MathUtil.clamp( - AutoAim.shotMap - .get(swerve.getDistanceToSpeaker()) - .getRotation() - .getDegrees(), - 0, - 20)), - () -> 0.0, - () -> 0.0) - .until(feeder::getFirstBeambreak) - .unless(feeder::getFirstBeambreak) - .andThen( - shooter.runStateCmd( - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS())) - .asProxy()); - } + // AUTOS ------------------------------------------------------------------- - private Command autoStaticAutoAim() { - return feeder - .indexCmd() - .deadlineWith( - swerve.runVelocityCmd(() -> new ChassisSpeeds()), - Commands.repeatingSequence( - shooter - .runFlywheelsCmd(() -> 0.0, () -> 0.0) - .unless(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0) - .until(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0), - shooter.runStateCmd( - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS()))) - .until(() -> feeder.getFirstBeambreak()) - .unless(() -> feeder.getFirstBeambreak()) - .withTimeout(1.0) - .andThen( - // FIXME - staticAutoAim(6.0) - .deadlineWith( - intake.runVoltageCmd(0, 0).asProxy(), carriage.runVoltageCmd(0).asProxy()) - .withTimeout(2.0) - .unless(() -> !feeder.getFirstBeambreak())) - .asProxy() - .withTimeout(4.0); - } + public Command amp4() { + return Commands.sequence( + autoFenderShot(), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 4 local.1"), true) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .withTimeout(1.0), + autoStaticAutoAim() + .andThen(Commands.print("Done with auto static auto aim")) + .beforeStarting(Commands.print("Before auto static auto aim!!!")), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 4 local.2")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .withTimeout(1.0), + autoStaticAutoAim(), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 4 local.3")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake().until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()), + autoStaticAutoAim()); + } + + public Command source3() { + return Commands.sequence( + autoFenderShot(), + swerve + .runChoreoTraj(Choreo.getTrajectory("source 3.1"), true) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), + swerve + .runChoreoTraj(Choreo.getTrajectory("source 3.2")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim()); + } + public Command source4() { + return Commands.sequence( + autoFenderShot(), + swerve + .runChoreoTraj(Choreo.getTrajectory("source 4.1"), true) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), + swerve + .runChoreoTraj(Choreo.getTrajectory("source 4.2")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), + swerve + .runChoreoTraj(Choreo.getTrajectory("source 4.3")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim()); + } + public Command amp5() { + return Commands.sequence( + autoFenderShot(), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 5.1"), true) + .asProxy() + .deadlineWith(autoIntake()), + autoStaticAutoAim(), + swerve + .runChoreoTraj(Choreo.getTrajectory("amp 5.2")) + .asProxy() + .deadlineWith(Commands.waitSeconds(1.0).andThen(autoIntake())), + autoStaticAutoAim(), + swerve.runChoreoTraj(Choreo.getTrajectory("amp 5.3")).asProxy().deadlineWith(autoIntake()), + autoIntake() + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .withTimeout(1.0), + autoStaticAutoAim(), + swerve.runChoreoTraj(Choreo.getTrajectory("amp 5.4")).asProxy().deadlineWith(autoIntake()), + autoIntake() + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .withTimeout(1.0), + autoStaticAutoAim()); + } + public Command center4() { + return Commands.sequence( + autoFenderShot(), + swerve + .runChoreoTraj(Choreo.getTrajectory("center 4.1"), true) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), + swerve + .runChoreoTraj(Choreo.getTrajectory("center 4.2")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), + swerve + .runChoreoTraj(Choreo.getTrajectory("center 4.3")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim()); + } + + public Command lineTest() { + ChoreoTrajectory traj = Choreo.getTrajectory("line test"); + return Commands.sequence( + Commands.runOnce( + () -> { + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + swerve.setPose(traj.flipped().getInitialPose()); + } else { + swerve.setPose(traj.getInitialPose()); + } + }), + swerve.runChoreoTraj(traj).repeatedly()); + } + + // UTIL COMMANDS --------------------------------------------------------------- + + private Command autoFenderShot() { + return shooter + .runStateCmd( + AutoAim.FENDER_SHOT.getRotation(), + AutoAim.FENDER_SHOT.getLeftRPS(), + AutoAim.FENDER_SHOT.getRightRPS()) + .raceWith( + feeder + .runVelocityCmd(0.0) + .until(shooter::isAtGoal) + .andThen( + feeder + .runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY) + .raceWith( + Commands.waitUntil(() -> !feeder.getFirstBeambreak()) + .andThen(Commands.waitSeconds(0.1))))); + } + + private Command autoIntake() { + return Commands.parallel( + intake + .runVelocityCmd(50.0, 30.0) + .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) + .asProxy(), + feeder.indexCmd().asProxy(), + carriage + .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE) + .until(feeder::getFirstBeambreak) + .asProxy(), + shooter + .runStateCmd( + () -> + Rotation2d.fromDegrees( + MathUtil.clamp( + AutoAim.shotMap + .get(swerve.getDistanceToSpeaker()) + .getRotation() + .getDegrees(), + 0, + 20)), + () -> 0.0, + () -> 0.0) + .until(feeder::getFirstBeambreak) + .unless(feeder::getFirstBeambreak) + .andThen( + shooter.runStateCmd( + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS())) + .asProxy()); + } + + private Command autoStaticAutoAim() { + return feeder + .indexCmd() + .deadlineWith( + swerve.runVelocityCmd(() -> new ChassisSpeeds()), + Commands.repeatingSequence( + shooter + .runFlywheelsCmd(() -> 0.0, () -> 0.0) + .unless(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0) + .until(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0), + shooter.runStateCmd( + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), + () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS()))) + .until(() -> feeder.getFirstBeambreak()) + .unless(() -> feeder.getFirstBeambreak()) + .withTimeout(1.0) + .andThen( + staticAutoAim + .get() + .deadlineWith( + intake.runVoltageCmd(0, 0).asProxy(), carriage.runVoltageCmd(0).asProxy()) + .withTimeout(2.0) + .unless(() -> !feeder.getFirstBeambreak())) + .asProxy() + .withTimeout(4.0); + } } From 589b9d72c9aa543f8afe73f79771e773467759ef Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 17 Nov 2024 14:06:20 -0800 Subject: [PATCH 3/3] Remove the dead code --- src/main/java/frc/robot/Robot.java | 80 ------------------------------ 1 file changed, 80 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3d749301..eba2748a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -699,86 +699,6 @@ private Command staticAutoAim(double tolerance) { return staticAutoAim(() -> tolerance); } - private Command autoStaticAutoAim() { - return feeder - .indexCmd() - .deadlineWith( - swerve.runVelocityCmd(() -> new ChassisSpeeds()), - Commands.repeatingSequence( - shooter - .runFlywheelsCmd(() -> 0.0, () -> 0.0) - .unless(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0) - .until(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0), - shooter.runStateCmd( - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS()))) - .until(() -> feeder.getFirstBeambreak()) - .unless(() -> feeder.getFirstBeambreak()) - .withTimeout(1.0) - .andThen( - staticAutoAim(6.0) - .deadlineWith( - intake.runVoltageCmd(0, 0).asProxy(), carriage.runVoltageCmd(0).asProxy()) - .withTimeout(2.0) - .unless(() -> !feeder.getFirstBeambreak())) - .asProxy() - .withTimeout(4.0); - } - - private Command autoFenderShot() { - return shooter - .runStateCmd( - AutoAim.FENDER_SHOT.getRotation(), - AutoAim.FENDER_SHOT.getLeftRPS(), - AutoAim.FENDER_SHOT.getRightRPS()) - .raceWith( - feeder - .runVelocityCmd(0.0) - .until(() -> shooter.isAtGoal()) - .andThen( - feeder - .runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY) - .raceWith( - Commands.waitUntil(() -> !feeder.getFirstBeambreak()) - .andThen(Commands.waitSeconds(0.1))))) - .asProxy(); - } - - private Command autoIntake() { - return Commands.parallel( - intake - .runVelocityCmd(50.0, 30.0) - .until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()) - .asProxy(), - feeder.indexCmd().asProxy(), - carriage - .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE) - .until(() -> feeder.getFirstBeambreak()) - .asProxy(), - shooter - .runStateCmd( - () -> - Rotation2d.fromDegrees( - MathUtil.clamp( - AutoAim.shotMap - .get(swerve.getDistanceToSpeaker()) - .getRotation() - .getDegrees(), - 0, - 20)), - () -> 0.0, - () -> 0.0) - .until(() -> feeder.getFirstBeambreak()) - .unless(() -> feeder.getFirstBeambreak()) - .andThen( - shooter.runStateCmd( - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(), - () -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS())) - .asProxy()); - } - private Command ampHeadingSnap(DoubleSupplier x, DoubleSupplier y) { var headingController = new ProfiledPIDController(