From 43a041cfdee91daafd578c263ac3c30532e1adf3 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 27 Jul 2024 21:27:03 -0400 Subject: [PATCH 01/11] got rid of arm overrides --- src/main/java/com/stuypulse/robot/Robot.java | 2 +- .../com/stuypulse/robot/RobotContainer.java | 17 ------- .../commands/arm/ArmDisableOverride.java | 19 -------- .../robot/commands/arm/ArmEnableOverride.java | 19 -------- .../robot/commands/arm/ArmSetState.java | 2 +- .../robot/commands/arm/ArmToFerry.java | 4 +- .../robot/commands/arm/ArmToSpeaker.java | 4 +- .../commands/shooter/ShooterAutoShoot.java | 2 +- .../commands/shooter/ShooterScoreAmp.java | 21 +++++---- .../swerve/SwerveDriveAutoAlignment.java | 2 +- .../stuypulse/robot/constants/Settings.java | 4 -- .../stuypulse/robot/subsystems/arm/Arm.java | 37 +++------------ .../robot/subsystems/arm/ArmImpl.java | 47 ++----------------- .../robot/subsystems/shooter/ShooterImpl.java | 4 +- 14 files changed, 32 insertions(+), 152 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 1f1b6116..0971a6e4 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -37,7 +37,7 @@ public void robotInit() { @Override public void robotPeriodic() { - if (Arm.getInstance().getActualState() == Arm.State.FEED + if (Arm.getInstance().getState() == Arm.State.FEED && Arm.getInstance().atTarget() && !Shooter.getInstance().hasNote() && Intake.getInstance().hasNote() diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 96453de6..78e8fdfd 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -2,8 +2,6 @@ import com.ctre.phoenix6.Utils; import com.stuypulse.robot.commands.BuzzController; -import com.stuypulse.robot.commands.arm.ArmDisableOverride; -import com.stuypulse.robot.commands.arm.ArmEnableOverride; import com.stuypulse.robot.commands.arm.ArmToAmp; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.arm.ArmToFerry; @@ -125,29 +123,14 @@ private void configureButtonBindings() { driver.getTopButton() .onTrue(new ArmToSpeaker()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - - driver.getTopButton() - .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) - .onTrue(new ArmEnableOverride()) - .onFalse(new ArmDisableOverride()); driver.getLeftButton() .onTrue(new ArmToAmp()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - - driver.getLeftButton() - .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) - .onTrue(new ArmEnableOverride()) - .onFalse(new ArmDisableOverride()); driver.getRightButton() .whileTrue(new ArmToFerry()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - - driver.getRightButton() - .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) - .onTrue(new ArmEnableOverride()) - .onFalse(new ArmDisableOverride()); driver.getBottomButton().onTrue(new ArmToFeed()); diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java deleted file mode 100644 index 0babc58f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java +++ /dev/null @@ -1,19 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ArmDisableOverride extends InstantCommand{ - - private final Arm arm; - - public ArmDisableOverride() { - this.arm = Arm.getInstance(); - } - - @Override - public void initialize() { - arm.setOverriding(false); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java deleted file mode 100644 index 9c89f6d4..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java +++ /dev/null @@ -1,19 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ArmEnableOverride extends InstantCommand{ - - private final Arm arm; - - public ArmEnableOverride() { - this.arm = Arm.getInstance(); - } - - @Override - public void initialize() { - arm.setOverriding(true); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java index 3f94c534..af42b187 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java @@ -16,6 +16,6 @@ public ArmSetState(Arm.State state) { @Override public void initialize() { - arm.setRequestedState(state); + arm.setState(state); } } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java index d39a34f3..009e4010 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java @@ -22,10 +22,10 @@ public ArmToFerry() { @Override public void initialize() { if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) { - arm.setRequestedState(Arm.State.LOB_FERRY); + arm.setState(Arm.State.LOB_FERRY); } else { - arm.setRequestedState(Arm.State.LOW_FERRY); + arm.setState(Arm.State.LOW_FERRY); } lastClick = stopWatch.getTime(); } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java index eafa55aa..7282a898 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java @@ -22,10 +22,10 @@ public ArmToSpeaker() { @Override public void initialize() { if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) { - arm.setRequestedState(Arm.State.SPEAKER_HIGH); + arm.setState(Arm.State.SPEAKER_HIGH); } else { - arm.setRequestedState(Arm.State.SPEAKER_LOW); + arm.setState(Arm.State.SPEAKER_LOW); } lastClick = stopWatch.getTime(); } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java index 1377464c..7d0dd506 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java @@ -12,7 +12,7 @@ public class ShooterAutoShoot extends InstantCommand{ @Override public void initialize() { - switch (Arm.getInstance().getActualState()) { + switch (Arm.getInstance().getState()) { case AMP: CommandScheduler.getInstance().schedule(new ShooterScoreAmp()); break; diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java index b64e5773..82ee480c 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java @@ -1,19 +1,20 @@ package com.stuypulse.robot.commands.shooter; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ShooterScoreAmp extends SequentialCommandGroup { +public class ShooterScoreAmp extends InstantCommand { + + private final Shooter shooter; public ShooterScoreAmp() { - addCommands( - new ShooterFeederDeacquire(), - new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()), - new WaitUntilCommand(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME), - new ShooterFeederStop() - ); + shooter = Shooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.feederDeacquire(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java index c2feab30..218f440c 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java @@ -22,7 +22,7 @@ public SwerveDriveAutoAlignment(Gamepad driver) { @Override public void initialize() { - switch (Arm.getInstance().getActualState()) { + switch (Arm.getInstance().getState()) { case AMP: CommandScheduler.getInstance().schedule(new SwerveDriveDriveAmpAligned(driver)); break; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6a5916ad..138aa977 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -71,9 +71,6 @@ public interface Arm { SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 17); SmartNumber PODIUM_SHOT_ANGLE = new SmartNumber("Arm/Podium Shot Angle", -60); - double SHOULD_RETURN_TO_FEED_TIME = 1.0; - double EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP = 2.5; - SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); double MAX_WAIT_TO_REACH_TARGET = 2.0; @@ -328,7 +325,6 @@ public interface Rotation { } public interface Driver { - double HOLD_TO_OVERRIDE_TIME = 0.55; double DOUBLE_CLICK_TIME_BETWEEN_CLICKS = 0.5; public interface Drive { diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 842ee3a1..66f0a362 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -1,13 +1,10 @@ package com.stuypulse.robot.subsystems.arm; -import com.stuypulse.robot.subsystems.shooter.Shooter; - import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Arm extends SubsystemBase { - /* SINGLETON */ private static final Arm instance; static { @@ -30,44 +27,24 @@ public enum State { RESETTING } - protected State requestedState; - protected State actualState; - protected boolean overriding; + protected State state; protected Arm() { - requestedState = State.RESETTING; - actualState = State.RESETTING; - overriding = false; - } - - public void setRequestedState(State state) { - this.requestedState = state; - } - - public State getRequestedState() { - return this.requestedState; - } - - public State getActualState() { - return this.actualState; + state = State.RESETTING; } - public void setOverriding(boolean overriding) { - this.overriding = overriding; + public void setState(State state) { + this.state = state; } - public boolean isOverriding() { - return this.overriding; + public State getState() { + return this.state; } public abstract boolean atTarget(); - public abstract boolean shouldReturnToFeed(); - @Override public void periodic() { - SmartDashboard.putBoolean("Arm/Is Overriding", overriding); - SmartDashboard.putString("Arm/Requested State", getRequestedState().toString()); - SmartDashboard.putString("Arm/Actual State", getActualState().toString()); + SmartDashboard.putString("Arm/State", state.toString()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 26b8c0d4..d968d402 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -39,9 +39,6 @@ public class ArmImpl extends Arm { private final Controller controller; private final MotionProfile motionProfile; - private final BStream shouldGoBackToFeed; - private final BStream shouldGoBackToFeedFromAmp; - protected ArmImpl() { super(); leftMotor = new CANSparkMax(Ports.Arm.LEFT_MOTOR, MotorType.kBrushless); @@ -66,14 +63,6 @@ protected ArmImpl() { .add(new ArmEncoderFeedforward(Settings.Arm.Feedforward.kG)) .add(new PIDController(Settings.Arm.PID.kP, Settings.Arm.PID.kI, Settings.Arm.PID.kD)) .setSetpointFilter(motionProfile); - - shouldGoBackToFeed = BStream.create(() -> !Shooter.getInstance().hasNote()) - .filtered(new BDebounce.Rising(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME)); - - shouldGoBackToFeedFromAmp = BStream.create(shouldGoBackToFeed) - .filtered(new BDebounce.Rising(Settings.Arm.EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP)) - .and(BStream.create(() -> !overriding) - .filtered(new BDebounce.Rising(Settings.Arm.EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP))); } @Override @@ -81,13 +70,8 @@ public boolean atTarget() { return Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.getAsDouble(); } - @Override - public boolean shouldReturnToFeed() { - return shouldGoBackToFeed.get(); - } - private double getTargetDegrees() { - switch (actualState) { + switch (state) { case AMP: return Settings.Arm.AMP_ANGLE.getAsDouble(); case SPEAKER_LOW: @@ -169,38 +153,15 @@ public void periodic() { if (bumpSwitchTriggered.get()) { armEncoder.setPosition(Settings.Arm.MIN_ANGLE.get()/360); - if (actualState == State.RESETTING) { - actualState = State.FEED; - requestedState = State.FEED; + if (state == State.RESETTING) { + state = State.FEED; } } - if (actualState == State.RESETTING) { + if (state == State.RESETTING) { setVoltage(-1.5); } else { - if (overriding - || requestedState == State.STOW - || requestedState == State.PRE_CLIMB - || requestedState == State.FEED - || requestedState == State.RESETTING - || Shooter.getInstance().hasNote() - ) { - this.actualState = this.requestedState; - } - else if (shouldGoBackToFeed.get()) { - if (actualState == State.AMP) { - if (shouldGoBackToFeedFromAmp.get()) { - requestedState = State.FEED; - actualState = State.FEED; - } - } - else if (actualState != State.FEED) { - requestedState = State.FEED; - actualState = State.FEED; - } - } - controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); setVoltage(controller.getOutput()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 59854b2e..f1a98bc9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -140,7 +140,7 @@ public ShooterSpeeds getFerrySpeeds() { double distanceToFerryInInches = Units.metersToInches(SwerveDrive.getInstance().getPose().getTranslation().getDistance(ferryZone)); - if (Arm.getInstance().getActualState() == Arm.State.LOB_FERRY) { + if (Arm.getInstance().getState() == Arm.State.LOB_FERRY) { double targetRPM = ShooterLobFerryInterpolation.getRPM(distanceToFerryInInches); return new ShooterSpeeds(targetRPM, 500); } @@ -154,7 +154,7 @@ public ShooterSpeeds getFerrySpeeds() { public void periodic () { super.periodic(); - Arm.State armState = Arm.getInstance().getActualState(); + Arm.State armState = Arm.getInstance().getState(); if (Settings.Shooter.ALWAYS_KEEP_AT_SPEED) { if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { From a664c57cef6372cd689a1834fc197c5c4cf36dbd Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 27 Jul 2024 21:55:46 -0400 Subject: [PATCH 02/11] return to feed when intake pressed --- src/main/java/com/stuypulse/robot/RobotContainer.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 78e8fdfd..3475223c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -105,6 +105,7 @@ private void configureButtonBindings() { driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver)); driver.getLeftTriggerButton() + .onTrue(new ArmToFeed()) .whileTrue(new IntakeAcquire() .andThen(new BuzzController(driver)) ); @@ -123,19 +124,16 @@ private void configureButtonBindings() { driver.getTopButton() .onTrue(new ArmToSpeaker()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - driver.getLeftButton() .onTrue(new ArmToAmp()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - driver.getRightButton() - .whileTrue(new ArmToFerry()) + .onTrue(new ArmToFerry()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - driver.getBottomButton().onTrue(new ArmToFeed()); - driver.getDPadUp().whileTrue(new ArmToPreClimb()); - driver.getDPadDown().whileTrue(new ArmToStow()); + driver.getDPadUp().onTrue(new ArmToPreClimb()); + driver.getDPadDown().onTrue(new ArmToStow()); } From d50267ecec233e7d4af3698c08370dfb16e2dc9b Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 27 Jul 2024 23:37:01 -0400 Subject: [PATCH 03/11] alignment with shoot commands --- .../com/stuypulse/robot/RobotContainer.java | 27 +++- .../swerve/SwerveDriveDriveAligned.java | 5 - .../SwerveDriveDriveAlignedLobFerry.java | 1 - .../SwerveDriveDriveAlignedLowFerry.java | 7 +- .../SwerveDriveDriveAndLobFerry.java | 35 +++++ .../SwerveDriveDriveAndLowFerry.java | 35 +++++ .../SwerveDriveDriveAndScore.java | 135 ++++++++++++++++++ .../SwerveDriveDriveAndScoreSpeakerHigh.java | 30 ++++ .../SwerveDriveDriveAndScoreSpeakerLow.java | 30 ++++ .../stuypulse/robot/constants/Settings.java | 1 + 10 files changed, 293 insertions(+), 13 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 3475223c..bd2f53b5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -28,6 +28,10 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerHigh; import com.stuypulse.robot.commands.swerve.SwerveDriveXMode; +import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLobFerry; +import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLowFerry; +import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerHigh; +import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerLow; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerLow; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.constants.Ports; @@ -124,12 +128,29 @@ private void configureButtonBindings() { driver.getTopButton() .onTrue(new ArmToSpeaker()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - driver.getLeftButton() - .onTrue(new ArmToAmp()) - .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); + + driver.getTopButton() + .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) + .whileTrue(new ConditionalCommand( + new SwerveDriveDriveAndScoreSpeakerLow(driver), + new SwerveDriveDriveAndScoreSpeakerHigh(driver), + () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW)); + driver.getRightButton() .onTrue(new ArmToFerry()) .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); + + driver.getRightButton() + .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) + .whileTrue(new ConditionalCommand( + new SwerveDriveDriveAndLowFerry(driver), + new SwerveDriveDriveAndLobFerry(driver), + () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); + + driver.getLeftButton() + .onTrue(new ArmToAmp()) + .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); + driver.getBottomButton().onTrue(new ArmToFeed()); driver.getDPadUp().onTrue(new ArmToPreClimb()); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java index 584cd721..bd8ab3fc 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java @@ -86,9 +86,4 @@ public void execute() { ) ); } - - @Override - public boolean isFinished() { - return Math.abs(driver.getRightX()) > Settings.Driver.Turn.DEADBAND.getAsDouble(); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java index 860c2c41..d24f156e 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java @@ -14,7 +14,6 @@ public SwerveDriveDriveAlignedLobFerry(Gamepad driver) { super(driver); } - // returns pose of close amp corner private Translation2d getAmpCornerPose() { Translation2d targetPose = Robot.isBlue() ? new Translation2d(0.0, Field.WIDTH - 1.5) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java index fc75a02f..750953af 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java @@ -14,8 +14,7 @@ public SwerveDriveDriveAlignedLowFerry(Gamepad driver) { super(driver); } - // returns pose of close amp corner - private Translation2d getTargetPose() { + private Translation2d getAmpCornerPose() { Translation2d targetPose = Robot.isBlue() ? new Translation2d(0.0, Field.WIDTH - 1.5) : new Translation2d(0.0, 1.5); @@ -25,11 +24,11 @@ private Translation2d getTargetPose() { @Override protected Rotation2d getTargetAngle() { - return SwerveDrive.getInstance().getPose().getTranslation().minus(getTargetPose()).getAngle(); + return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle(); } @Override protected double getDistanceToTarget() { - return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getTargetPose()); + return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java new file mode 100644 index 00000000..684e3ee0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java @@ -0,0 +1,35 @@ +package com.stuypulse.robot.commands.swerve.driveAndScore; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveDriveDriveAndLobFerry extends SwerveDriveDriveAndScore{ + + public SwerveDriveDriveAndLobFerry(Gamepad driver) { + super(driver, Arm.State.SPEAKER_HIGH); + } + + private Translation2d getAmpCornerPose() { + Translation2d targetPose = Robot.isBlue() + ? new Translation2d(0.0, Field.WIDTH - 1.5) + : new Translation2d(0.0, 1.5); + + return targetPose; + } + + @Override + protected Rotation2d getTargetAngle() { + return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); + } + + @Override + protected double getDistanceToTarget() { + return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java new file mode 100644 index 00000000..49e5241b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java @@ -0,0 +1,35 @@ +package com.stuypulse.robot.commands.swerve.driveAndScore; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveDriveDriveAndLowFerry extends SwerveDriveDriveAndScore{ + + public SwerveDriveDriveAndLowFerry(Gamepad driver) { + super(driver, Arm.State.SPEAKER_HIGH); + } + + private Translation2d getAmpCornerPose() { + Translation2d targetPose = Robot.isBlue() + ? new Translation2d(0.0, Field.WIDTH - 1.5) + : new Translation2d(0.0, 1.5); + + return targetPose; + } + + @Override + protected Rotation2d getTargetAngle() { + return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle(); + } + + @Override + protected double getDistanceToTarget() { + return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java new file mode 100644 index 00000000..aef98a76 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java @@ -0,0 +1,135 @@ +package com.stuypulse.robot.commands.swerve.driveAndScore; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.commands.shooter.ShooterAutoShoot; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import com.stuypulse.stuylib.util.AngleVelocity; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public abstract class SwerveDriveDriveAndScore extends Command { + + private final SwerveDrive swerve; + private final Arm arm; + private final Shooter shooter; + + private final Arm.State armState; + + protected final Gamepad driver; + private final VStream velocity; + private final IStream angleVelocity; + + private final BStream isAligned; + + private final SwerveRequest.FieldCentric drive; + + private final AngleController controller; + + public SwerveDriveDriveAndScore(Gamepad driver, Arm.State armState) { + swerve = SwerveDrive.getInstance(); + shooter = Shooter.getInstance(); + arm = Arm.getInstance(); + this.driver = driver; + this.armState = armState; + + velocity = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), + new VLowPassFilter(Drive.RC.get())); + + drive = new SwerveRequest.FieldCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kP) + .setOutputFilter(x -> -x); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); + + AngleVelocity derivative = new AngleVelocity(); + + angleVelocity = IStream.create(() -> derivative.get(Angle.fromRotation2d(getTargetAngle()))) + .filtered(new LowPassFilter(Assist.ANGLE_DERIV_RC)) + // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST + // so that angular velocity doesn't oscillate + .filtered(x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST)) + .filtered(x -> -x); + + addRequirements(swerve); + } + + protected abstract Rotation2d getTargetAngle(); + + protected abstract double getDistanceToTarget(); + + private boolean isAligned() { + return controller.isDoneDegrees(Alignment.ANGLE_TOLERANCE.get()); + } + + @Override + public void initialize() { + arm.setState(armState); + if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { + shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); + } + else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { + shooter.setTargetSpeeds(shooter.getFerrySpeeds()); + } + else { + shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); + } + } + + @Override + public void execute() { + swerve.setControl( + drive.withVelocityX(velocity.get().y) + .withVelocityY(-velocity.get().x) + .withRotationalRate( + angleVelocity.get() + + controller.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(swerve.getPose().getRotation())) + ) + ); + + if (isAligned.get()) { + CommandScheduler.getInstance().schedule(new ShooterAutoShoot()); + } + + } + + @Override + public void end(boolean interrupted) { + shooter.setTargetSpeeds(new ShooterSpeeds()); + shooter.feederStop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java new file mode 100644 index 00000000..dc70c1df --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.swerve.driveAndScore; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveDriveDriveAndScoreSpeakerHigh extends SwerveDriveDriveAndScore{ + + public SwerveDriveDriveAndScoreSpeakerHigh(Gamepad driver) { + super(driver, Arm.State.SPEAKER_HIGH); + } + + @Override + protected Rotation2d getTargetAngle() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return speakerPose.minus(currentPose).getAngle().plus(Rotation2d.fromDegrees(180)); + } + + @Override + protected double getDistanceToTarget() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return currentPose.getDistance(speakerPose); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java new file mode 100644 index 00000000..a29dfabf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.swerve.driveAndScore; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveDriveDriveAndScoreSpeakerLow extends SwerveDriveDriveAndScore{ + + public SwerveDriveDriveAndScoreSpeakerLow(Gamepad driver) { + super(driver, Arm.State.SPEAKER_LOW); + } + + @Override + protected Rotation2d getTargetAngle() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return speakerPose.minus(currentPose).getAngle(); + } + + @Override + protected double getDistanceToTarget() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return currentPose.getDistance(speakerPose); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 138aa977..1b26112e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -326,6 +326,7 @@ public interface Rotation { public interface Driver { double DOUBLE_CLICK_TIME_BETWEEN_CLICKS = 0.5; + double HOLD_TIME_FOR_AUTOMATED_SCORING = 0.7; public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.015); From 4d836f65cb68629bef40071cc4f69c8c46813e54 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sun, 28 Jul 2024 00:52:36 -0400 Subject: [PATCH 04/11] make sure shoot command stops when driver lets go for driveAndScore commands --- .../swerve/driveAndScore/SwerveDriveDriveAndScore.java | 4 ++-- .../com/stuypulse/robot/subsystems/shooter/Shooter.java | 2 -- .../stuypulse/robot/subsystems/shooter/ShooterImpl.java | 7 ------- 3 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java index aef98a76..4ca9f8c9 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.stuypulse.robot.commands.shooter.ShooterAutoShoot; +import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Driver.Drive; @@ -129,7 +130,6 @@ public void execute() { @Override public void end(boolean interrupted) { - shooter.setTargetSpeeds(new ShooterSpeeds()); - shooter.feederStop(); + CommandScheduler.getInstance().schedule(new ShooterStop()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 41f6b74d..bf92c554 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -55,6 +55,4 @@ public void stop() { public abstract void feederStop(); public abstract boolean hasNote(); - - public abstract boolean noteShot(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index f1a98bc9..354c6c6c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -127,11 +127,6 @@ public boolean hasNote() { return hasNote.get(); } - @Override - public boolean noteShot() { - return getLeftTargetRPM() > 0 && getRightTargetRPM() > 0 && hasNote.get() == false; - } - @Override public ShooterSpeeds getFerrySpeeds() { Translation2d ferryZone = Robot.isBlue() @@ -199,8 +194,6 @@ else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { SmartDashboard.putNumber("Shooter/Left Current", leftMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Right Current", rightMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Feeder Current", feederMotor.getOutputCurrent()); - - SmartDashboard.putBoolean("Shooter/Note Shot", noteShot()); } } \ No newline at end of file From 1f0f90cda7d7d7587badceb58afbb53359ef5cba Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sun, 28 Jul 2024 12:36:08 -0400 Subject: [PATCH 05/11] rebind to match izzi controls, organize swerve commands into folders --- .../com/stuypulse/robot/RobotContainer.java | 89 +++++++++++++------ .../commands/arm/ArmToSubwooferShot.java | 10 +++ .../swerve/SwerveDriveDriveRobotRelative.java | 67 ++++++++++++++ .../SwerveDriveAutoAlignment.java | 4 +- .../SwerveDriveDriveAligned.java | 7 +- .../SwerveDriveDriveAlignedAmp.java} | 6 +- .../SwerveDriveDriveAlignedLobFerry.java | 2 +- .../SwerveDriveDriveAlignedLowFerry.java | 2 +- .../SwerveDriveDriveAlignedSpeakerHigh.java | 2 +- .../SwerveDriveDriveAlignedSpeakerLow.java | 2 +- .../SwerveDriveDriveAndLobFerry.java | 2 +- .../SwerveDriveDriveAndLobFerryManual.java | 35 ++++++++ .../SwerveDriveDriveAndLowFerry.java | 2 +- .../SwerveDriveDriveAndLowFerryManual.java | 35 ++++++++ .../com/stuypulse/robot/constants/Field.java | 15 ++-- .../stuypulse/robot/constants/Settings.java | 2 +- .../stuypulse/robot/subsystems/arm/Arm.java | 1 + .../robot/subsystems/arm/ArmImpl.java | 4 +- 18 files changed, 237 insertions(+), 50 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToSubwooferShot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java rename src/main/java/com/stuypulse/robot/commands/swerve/{ => driveAligned}/SwerveDriveAutoAlignment.java (94%) rename src/main/java/com/stuypulse/robot/commands/swerve/{ => driveAligned}/SwerveDriveDriveAligned.java (94%) rename src/main/java/com/stuypulse/robot/commands/swerve/{SwerveDriveDriveAmpAligned.java => driveAligned/SwerveDriveDriveAlignedAmp.java} (72%) rename src/main/java/com/stuypulse/robot/commands/swerve/{ => driveAligned}/SwerveDriveDriveAlignedLobFerry.java (94%) rename src/main/java/com/stuypulse/robot/commands/swerve/{ => driveAligned}/SwerveDriveDriveAlignedLowFerry.java (94%) rename src/main/java/com/stuypulse/robot/commands/swerve/{ => driveAligned}/SwerveDriveDriveAlignedSpeakerHigh.java (94%) rename src/main/java/com/stuypulse/robot/commands/swerve/{ => driveAligned}/SwerveDriveDriveAlignedSpeakerLow.java (94%) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerryManual.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bd2f53b5..c1834c74 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,6 +13,7 @@ import com.stuypulse.robot.commands.arm.ArmToSpeaker; import com.stuypulse.robot.commands.arm.ArmToSpeakerLow; import com.stuypulse.robot.commands.arm.ArmToStow; +import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.intake.IntakeAcquire; @@ -22,17 +23,23 @@ import com.stuypulse.robot.commands.shooter.ShooterAutoShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; +import com.stuypulse.robot.commands.shooter.ShooterScoreAmp; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterSetRPM; -import com.stuypulse.robot.commands.swerve.SwerveDriveAutoAlignment; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerHigh; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; import com.stuypulse.robot.commands.swerve.SwerveDriveXMode; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveAutoAlignment; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerHigh; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerLow; import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLobFerry; +import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLobFerryManual; import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLowFerry; +import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLowFerryManual; import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerHigh; import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerLow; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerLow; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -103,59 +110,87 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - driver.getRightBumper().whileTrue(new SwerveDriveXMode()); + configureOperatorBindings(); + configureDriverBindings(); + } + private void configureDriverBindings() { driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative()); - driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver)); - driver.getLeftTriggerButton() + // intake field relative + driver.getRightTriggerButton() .onTrue(new ArmToFeed()) .whileTrue(new IntakeAcquire() .andThen(new BuzzController(driver)) ); - driver.getLeftBumper() - .whileTrue(new IntakeDeacquire()) - .onFalse(new IntakeStop()); - - driver.getRightTriggerButton() - .whileTrue(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) - .andThen(new ShooterAutoShoot()) + // intake robot relative + driver.getLeftTriggerButton() + .onTrue(new ArmToFeed()) + .whileTrue(new IntakeAcquire() + .andThen(new BuzzController(driver)) ) - .onFalse(new ShooterFeederStop()) - .onFalse(new ShooterSetRPM(new ShooterSpeeds()).onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .whileTrue(new SwerveDriveDriveRobotRelative(driver)); - driver.getTopButton() - .onTrue(new ArmToSpeaker()) - .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); + // deacquire + driver.getDPadLeft() + .whileTrue(new IntakeDeacquire()) + .onFalse(new IntakeStop()); - driver.getTopButton() + // speaker align and score + driver.getRightBumper() + .onTrue(new ArmToSpeaker()); + driver.getRightBumper() .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) .whileTrue(new ConditionalCommand( new SwerveDriveDriveAndScoreSpeakerLow(driver), new SwerveDriveDriveAndScoreSpeakerHigh(driver), () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW)); - driver.getRightButton() - .onTrue(new ArmToFerry()) - .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - - driver.getRightButton() + // ferry align and shoot + driver.getTopButton() + .onTrue(new ArmToFerry()); + driver.getTopButton() .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) .whileTrue(new ConditionalCommand( new SwerveDriveDriveAndLowFerry(driver), new SwerveDriveDriveAndLobFerry(driver), () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); - driver.getLeftButton() + // arm to amp and alignment + driver.getLeftTriggerButton() .onTrue(new ArmToAmp()) - .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); + .onTrue(new SwerveDriveDriveAlignedAmp(driver)); + + // manual speaker at subwoofer + // score amp + // rebind to a button on the back later + driver.getRightMenuButton() + .whileTrue(new ConditionalCommand( + new ShooterScoreAmp(), + new ArmToSubwooferShot() + .andThen(new ShooterScoreSpeaker()), + () -> Arm.getInstance().getState() == Arm.State.AMP)); + + // manual ferry + driver.getLeftButton() + .onTrue(new ArmToFerry()); + driver.getLeftButton() + .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) + .whileTrue(new ConditionalCommand( + new SwerveDriveDriveAndLowFerryManual(driver), + new SwerveDriveDriveAndLobFerryManual(driver), + () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); driver.getBottomButton().onTrue(new ArmToFeed()); driver.getDPadUp().onTrue(new ArmToPreClimb()); driver.getDPadDown().onTrue(new ArmToStow()); + } + private void configureOperatorBindings() { + operator.getLeftTriggerButton().whileTrue(new IntakeDeacquire()); + operator.getRightTriggerButton().whileTrue(new IntakeAcquire()); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSubwooferShot.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSubwooferShot.java new file mode 100644 index 00000000..a30531a2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSubwooferShot.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; + +public class ArmToSubwooferShot extends ArmSetState{ + + public ArmToSubwooferShot(){ + super(Arm.State.SUBWOOFER_SHOT); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java new file mode 100644 index 00000000..fd9b91eb --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java @@ -0,0 +1,67 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveRobotRelative extends Command { + + private final SwerveDrive swerve; + + private final Gamepad driver; + + private final SwerveRequest.RobotCentric drive; + + private final VStream speed; + private final IStream turn; + + public SwerveDriveDriveRobotRelative(Gamepad driver) { + swerve = SwerveDrive.getInstance(); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), + new VLowPassFilter(Drive.RC.get())); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND.get()), + x -> SLMath.spow(x, Turn.POWER.get()), + x -> x * Turn.MAX_TELEOP_TURNING.get(), + new LowPassFilter(Turn.RC.get())); + + this.driver = driver; + + drive = new SwerveRequest.RobotCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + addRequirements(swerve); + } + + @Override + public void execute() { + swerve.setControl(drive.withVelocityX(speed.get().y) + .withVelocityY(-speed.get().x) + .withRotationalRate(turn.get()) + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveAutoAlignment.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveAutoAlignment.java index 218f440c..01061d45 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveAutoAlignment.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.subsystems.arm.Arm; @@ -24,7 +24,7 @@ public SwerveDriveAutoAlignment(Gamepad driver) { public void initialize() { switch (Arm.getInstance().getState()) { case AMP: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAmpAligned(driver)); + CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedAmp(driver)); break; case SPEAKER_HIGH: CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedSpeakerHigh(driver)); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index bd8ab3fc..d663dfe5 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; @@ -73,6 +73,11 @@ protected double getAngleError() { return controller.getError().getRotation2d().getDegrees(); } + @Override + public boolean isFinished() { + return Math.abs(driver.getRightX()) > Settings.Driver.Turn.DEADBAND.getAsDouble(); + } + @Override public void execute() { swerve.setControl( diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java similarity index 72% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java index a86f2f84..d6a93797 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; @@ -6,9 +6,9 @@ import edu.wpi.first.math.geometry.Rotation2d; -public class SwerveDriveDriveAmpAligned extends SwerveDriveDriveAligned { +public class SwerveDriveDriveAlignedAmp extends SwerveDriveDriveAligned { - public SwerveDriveDriveAmpAligned(Gamepad driver) { + public SwerveDriveDriveAlignedAmp(Gamepad driver) { super(driver); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java index d24f156e..941187fd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java index 750953af..701eacbc 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerHigh.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerHigh.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerHigh.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerHigh.java index e45037ab..7e61e70b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerHigh.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerHigh.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerLow.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerLow.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerLow.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerLow.java index 75b9e500..9586608d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerLow.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerLow.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java index 684e3ee0..4dfc29ed 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java @@ -12,7 +12,7 @@ public class SwerveDriveDriveAndLobFerry extends SwerveDriveDriveAndScore{ public SwerveDriveDriveAndLobFerry(Gamepad driver) { - super(driver, Arm.State.SPEAKER_HIGH); + super(driver, Arm.State.LOB_FERRY); } private Translation2d getAmpCornerPose() { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java new file mode 100644 index 00000000..bda4074a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java @@ -0,0 +1,35 @@ +package com.stuypulse.robot.commands.swerve.driveAndScore; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveDriveDriveAndLobFerryManual extends SwerveDriveDriveAndScore{ + + public SwerveDriveDriveAndLobFerryManual(Gamepad driver) { + super(driver, Arm.State.SPEAKER_HIGH); + } + + private Translation2d getAmpCornerPose() { + Translation2d targetPose = Robot.isBlue() + ? new Translation2d(0.0, Field.WIDTH - 1.5) + : new Translation2d(0.0, 1.5); + + return targetPose; + } + + @Override + protected Rotation2d getTargetAngle() { + return Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); + } + + @Override + protected double getDistanceToTarget() { + return Field.getManualFerryPosition().getDistance(getAmpCornerPose()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java index 49e5241b..8142034d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java @@ -12,7 +12,7 @@ public class SwerveDriveDriveAndLowFerry extends SwerveDriveDriveAndScore{ public SwerveDriveDriveAndLowFerry(Gamepad driver) { - super(driver, Arm.State.SPEAKER_HIGH); + super(driver, Arm.State.LOW_FERRY); } private Translation2d getAmpCornerPose() { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerryManual.java new file mode 100644 index 00000000..b62efbb4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerryManual.java @@ -0,0 +1,35 @@ +package com.stuypulse.robot.commands.swerve.driveAndScore; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveDriveDriveAndLowFerryManual extends SwerveDriveDriveAndScore{ + + public SwerveDriveDriveAndLowFerryManual(Gamepad driver) { + super(driver, Arm.State.SPEAKER_HIGH); + } + + private Translation2d getAmpCornerPose() { + Translation2d targetPose = Robot.isBlue() + ? new Translation2d(0.0, Field.WIDTH - 1.5) + : new Translation2d(0.0, 1.5); + + return targetPose; + } + + @Override + protected Rotation2d getTargetAngle() { + return Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle(); + } + + @Override + protected double getDistanceToTarget() { + return Field.getManualFerryPosition().getDistance(getAmpCornerPose()); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 31a4d931..ba389642 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -269,16 +269,13 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria /*** FERRYING ***/ - // MIDLINE: 8.27 - - // BEFORE: 9 - // MIDLINE + 60% of WING_TO_CENTERLINE distance - double FERRY_SHOT_MAX_X = LENGTH / 2.0 + WING_TO_CENTERLINE * 0.60; - double FERRY_SHOT_MIN_X = 6.0; - double FERRY_SHOT_MIN_FAR_X = 8.5; + public static Translation2d getManualFerryPosition() { + return Robot.isBlue() + ? new Translation2d(LENGTH / 2 + WING_TO_CENTERLINE * 0.8, 1) + : new Translation2d(LENGTH / 2 - WING_TO_CENTERLINE * 0.8, 1); + } - double CONST_FERRY_X = (FERRY_SHOT_MAX_X + FERRY_SHOT_MIN_FAR_X) / 2.0; - double FERRY_CUTOFF = 1.8; + // MIDLINE: 8.27 /**** EMPTY FIELD POSES ****/ diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1b26112e..fb9ae1e6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -69,7 +69,7 @@ public interface Arm { SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 17); - SmartNumber PODIUM_SHOT_ANGLE = new SmartNumber("Arm/Podium Shot Angle", -60); + SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -60); SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 66f0a362..745bfe8d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -17,6 +17,7 @@ public static Arm getInstance(){ public enum State { AMP, + SUBWOOFER_SHOT, SPEAKER_HIGH, SPEAKER_LOW, LOW_FERRY, diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index d968d402..2eeb2d1d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -74,6 +74,8 @@ private double getTargetDegrees() { switch (state) { case AMP: return Settings.Arm.AMP_ANGLE.getAsDouble(); + case SUBWOOFER_SHOT: + return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); case SPEAKER_LOW: return getSpeakerAngle(true); case SPEAKER_HIGH: @@ -134,7 +136,7 @@ private double getSpeakerAngle(boolean getLowAngle) { } catch (Exception exception) { exception.printStackTrace(); - return Settings.Arm.PODIUM_SHOT_ANGLE.get(); + return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); } } From 383efb4a7690b1f9a20228da2b42fc87f6d0cd66 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sun, 28 Jul 2024 13:00:02 -0400 Subject: [PATCH 06/11] note detection for intaking --- .../com/stuypulse/robot/RobotContainer.java | 2 + .../noteAlignment/SwerveDriveDriveToNote.java | 116 ++++++++++++++++++ .../robot/subsystems/vision/LLNoteVision.java | 21 +++- .../robot/subsystems/vision/NoteVision.java | 4 +- .../robot/util/HolonomicController.java | 79 ++++++++++++ 5 files changed, 216 insertions(+), 6 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java create mode 100644 src/main/java/com/stuypulse/robot/util/HolonomicController.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c1834c74..122168d1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -40,6 +40,7 @@ import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLowFerryManual; import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerHigh; import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerLow; +import com.stuypulse.robot.commands.swerve.noteAlignment.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -120,6 +121,7 @@ private void configureDriverBindings() { // intake field relative driver.getRightTriggerButton() .onTrue(new ArmToFeed()) + .whileTrue(new SwerveDriveDriveToNote(driver)) .whileTrue(new IntakeAcquire() .andThen(new BuzzController(driver)) ); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java new file mode 100644 index 00000000..79b4c7dd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java @@ -0,0 +1,116 @@ +package com.stuypulse.robot.commands.swerve.noteAlignment; + +import static com.stuypulse.robot.constants.Settings.Alignment.*; +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.NoteDetection; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.HolonomicController; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToNote extends Command { + + private final SwerveDrive swerve; + private final NoteVision vision; + private final Intake intake; + + private final SwerveRequest.FieldCentric drive; + + private final VStream speed; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveDriveToNote(Gamepad driver) { + this.swerve = SwerveDrive.getInstance(); + this.vision = NoteVision.getInstance(); + this.intake = Intake.getInstance(); + + drive = new SwerveRequest.FieldCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), + new VLowPassFilter(Drive.RC.get())); + + controller = new HolonomicController( + new PIDController(NoteDetection.Translation.kP, NoteDetection.Translation.kI, NoteDetection.Translation.kD), + new PIDController(NoteDetection.Translation.kP, NoteDetection.Translation.kI, NoteDetection.Translation.kD), + new AnglePIDController(NoteDetection.Rotation.kP, NoteDetection.Rotation.kI, NoteDetection.Rotation.kD)); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d targetTranslation = swerve.getPose() + .getTranslation().plus( + new Translation2d(Settings.CENTER_TO_FRONT_OF_INTAKE, 0) + .rotateBy(swerve.getPose().getRotation())); + + Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); + + if (vision.hasNoteData()) { + ChassisSpeeds speeds = controller.update(targetPose, swerve.getPose()); + + if (vision.withinIntakePath()) + speeds.omegaRadiansPerSecond = Math.signum(speeds.omegaRadiansPerSecond) * Math.toRadians(5.0); + + // drive to note + swerve.setControl(drive.withVelocityX(speeds.vxMetersPerSecond) + .withVelocityY(speeds.vyMetersPerSecond) + .withRotationalRate(speeds.omegaRadiansPerSecond) + ); + } else { + swerve.setControl(drive.withVelocityX(speed.get().y) + .withVelocityY(-speed.get().x) + .withRotationalRate(0) + ); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get() || intake.hasNote(); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java index 5b74f6c5..9920ca15 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java @@ -76,6 +76,16 @@ private boolean hasNoteDataRaw() { return false; } + /** + * Get the estimated pose of the note. + * + * @return the estimated pose of the note + */ + @Override + public Translation2d getEstimatedNotePose() { + return notePose.get().getTranslation2d(); + } + @Override public Translation2d getRobotRelativeNotePose() { Translation2d sum = new Translation2d(); @@ -83,7 +93,6 @@ public Translation2d getRobotRelativeNotePose() { for (Limelight limelight : limelights) { sum = sum.plus(new Translation2d( limelight.getDistanceToNote(), - // Angle.fromRotation2d(Rotation2d.fromDegrees(limelight.getXAngle()).plus(new Rotation2d(Math.PI))).getRotation2d())); Rotation2d.fromDegrees(limelight.getXAngle()))); } @@ -98,6 +107,8 @@ private void updateNotePose() { Translation2d sum = new Translation2d(); for (Limelight limelight : limelights) { + SwerveDrive swerve = SwerveDrive.getInstance(); + Translation2d limelightToNote = new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle())); Translation2d robotToNote = limelightToNote @@ -105,8 +116,8 @@ private void updateNotePose() { .rotateBy(limelight.getRobotRelativePose().getRotation().toRotation2d()); Translation2d fieldToNote = robotToNote - .rotateBy(SwerveDrive.getInstance().getPose().getRotation()) - .plus(SwerveDrive.getInstance().getPose().getTranslation()); + .rotateBy(swerve.getPose().getRotation()) + .plus(swerve.getPose().getTranslation()); sum = sum.plus(fieldToNote); } @@ -122,7 +133,7 @@ public void periodic() { limelights[i].updateData(); } - note.setPose(new Pose2d(SwerveDrive.getInstance().getPose().getTranslation().plus(getRobotRelativeNotePose()), new Rotation2d())); + note.setPose(new Pose2d(getEstimatedNotePose(), new Rotation2d())); if (hasNoteDataRaw()) updateNotePose(); updateTelemetry(); @@ -133,6 +144,8 @@ private void updateTelemetry() { SmartDashboard.putNumber("Note Detection/X Angle", limelights[0].getXAngle()); SmartDashboard.putNumber("Note Detection/Y Angle", limelights[0].getYAngle()); SmartDashboard.putNumber("Note Detection/Distance", limelights[0].getDistanceToNote()); + SmartDashboard.putNumber("Note Detection/Estimated X", getEstimatedNotePose().getX()); + SmartDashboard.putNumber("Note Detection/Estimated Y", getEstimatedNotePose().getY()); } else { SmartDashboard.putNumber("Note Detection/X Angle", 0); SmartDashboard.putNumber("Note Detection/Y Angle", 0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java index 4eeca3ed..813a2531 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java @@ -31,10 +31,11 @@ public final boolean withinIntakePath() { public abstract boolean hasNoteData(); + public abstract Translation2d getEstimatedNotePose(); + public abstract Translation2d getRobotRelativeNotePose(); public final Rotation2d getRotationToNote() { - // return Angle.fromRotation2d(getRobotRelativeNotePose().getAngle()).add(Angle.fromDegrees(180)).getRotation2d(); return getRobotRelativeNotePose().getAngle(); } @@ -42,6 +43,5 @@ public final Rotation2d getRotationToNote() { public void periodic() { SmartDashboard.putBoolean("Note Detection/Has Note Data", hasNoteData()); SmartDashboard.putBoolean("Note Detection/Is in Intake Path", withinIntakePath()); - SmartDashboard.putNumber("Note Detection/angle to note", getRotationToNote().getDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/util/HolonomicController.java b/src/main/java/com/stuypulse/robot/util/HolonomicController.java new file mode 100644 index 00000000..9074fe58 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/HolonomicController.java @@ -0,0 +1,79 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.Angle; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public class HolonomicController implements Sendable { + private PIDController xController; + private PIDController yController; + private AnglePIDController angleController; + + public HolonomicController(PIDController xController, PIDController yController, AnglePIDController angleController) { + this.xController = xController; + this.yController = yController; + this.angleController = angleController; + } + + public ChassisSpeeds update(Pose2d setpoint, Pose2d measurement) { + xController.update(setpoint.getX(), measurement.getX()); + yController.update(setpoint.getY(), measurement.getY()); + angleController.update( + Angle.fromRotation2d(setpoint.getRotation()), + Angle.fromRotation2d(measurement.getRotation())); + + return getOutput(); + } + + public ChassisSpeeds getOutput() { + return ChassisSpeeds.fromFieldRelativeSpeeds( + xController.getOutput(), + yController.getOutput(), + angleController.getOutput(), + angleController.getMeasurement().getRotation2d()); + } + + public ChassisSpeeds getError() { + return ChassisSpeeds.fromFieldRelativeSpeeds( + xController.getError(), + yController.getError(), + angleController.getError().toDegrees(), + angleController.getMeasurement().getRotation2d()); + } + + public boolean isDone(double xToleranceMeters, double yToleranceMeters, double angleToleranceDegrees) { + return xController.isDone(xToleranceMeters) + && yController.isDone(yToleranceMeters) + && angleController.isDoneDegrees(angleToleranceDegrees); + } + + public HolonomicController setTranslationConstants(double p, double i, double d) { + xController.setPID(p, i, d); + yController.setPID(p, i, d); + return this; + } + + public HolonomicController setRotationConstants(double p, double i, double d) { + angleController.setPID(p, i, d); + return this; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Holonomic Controller"); + builder.addDoubleProperty("Angle Setpoint (degrees)", () -> angleController.getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Angle Measurement (degrees)", () -> angleController.getMeasurement().toDegrees(), null); + builder.addDoubleProperty("X Setpoint (meters)", () -> xController.getSetpoint(), null); + builder.addDoubleProperty("X Measurement (meters)", () -> xController.getMeasurement(), null); + builder.addDoubleProperty("Y Setpoint (meters)", () -> yController.getSetpoint(), null); + builder.addDoubleProperty("Y Measurement (meters)", () -> yController.getMeasurement(), null); + builder.addDoubleProperty("X Error (meters)", () -> xController.getError(), null); + builder.addDoubleProperty("Y Error (meters)", () -> yController.getError(), null); + builder.addDoubleProperty("Angle Error (degrees)", () -> angleController.getError().getRotation2d().getDegrees(), null); + } +} From f01a00c3c20c5f99db974715ee23eec5b6920ef0 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sun, 28 Jul 2024 14:19:09 -0400 Subject: [PATCH 07/11] add climbing commands --- .../com/stuypulse/robot/RobotContainer.java | 23 ++-- .../robot/commands/arm/ArmToClimbing.java | 10 ++ .../swerve/SwerveDriveDriveToChain.java | 54 ++++++++ .../swerve/SwerveDriveDriveToClimb.java | 115 ++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 20 +-- .../stuypulse/robot/subsystems/arm/Arm.java | 1 + .../robot/subsystems/arm/ArmImpl.java | 2 + 7 files changed, 203 insertions(+), 22 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 122168d1..4bfea604 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.Utils; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.arm.ArmToAmp; +import com.stuypulse.robot.commands.arm.ArmToClimbing; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.arm.ArmToFerry; import com.stuypulse.robot.commands.arm.ArmToLobFerry; @@ -28,6 +29,8 @@ import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToClimb; import com.stuypulse.robot.commands.swerve.SwerveDriveXMode; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveAutoAlignment; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp; @@ -150,9 +153,10 @@ private void configureDriverBindings() { () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW)); // ferry align and shoot - driver.getTopButton() + // move to back of controller + driver.getRightMenuButton() .onTrue(new ArmToFerry()); - driver.getTopButton() + driver.getRightMenuButton() .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) .whileTrue(new ConditionalCommand( new SwerveDriveDriveAndLowFerry(driver), @@ -175,19 +179,24 @@ private void configureDriverBindings() { () -> Arm.getInstance().getState() == Arm.State.AMP)); // manual ferry - driver.getLeftButton() + driver.getTopButton() .onTrue(new ArmToFerry()); - driver.getLeftButton() + driver.getTopButton() .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) .whileTrue(new ConditionalCommand( new SwerveDriveDriveAndLowFerryManual(driver), new SwerveDriveDriveAndLobFerryManual(driver), () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); - driver.getBottomButton().onTrue(new ArmToFeed()); + driver.getBottomButton() + .onTrue(new ArmToPreClimb()); + driver.getBottomButton() + .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) + .whileTrue(new SwerveDriveDriveToChain()); + + driver.getRightButton().whileTrue(new SwerveDriveDriveToClimb()); - driver.getDPadUp().onTrue(new ArmToPreClimb()); - driver.getDPadDown().onTrue(new ArmToStow()); + driver.getLeftBumper().onTrue(new ArmToClimbing()); } private void configureOperatorBindings() { diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java new file mode 100644 index 00000000..62eef26b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; + +public class ArmToClimbing extends ArmSetState{ + + public ArmToClimbing(){ + super(Arm.State.CLIMBING); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java new file mode 100644 index 00000000..62f07b38 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java @@ -0,0 +1,54 @@ +package com.stuypulse.robot.commands.swerve; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToChain extends Command { + + private final SwerveDrive swerve; + + private final SwerveRequest.FieldCentric drive; + + private Pose2d trapPose; + + public SwerveDriveDriveToChain() { + swerve = SwerveDrive.getInstance(); + drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + addRequirements(swerve); + } + + @Override + public void initialize() { + trapPose = Field.getClosestAllianceTrapPose(swerve.getPose()); + } + + @Override + public void execute() { + Rotation2d translationAngle = trapPose.getTranslation().minus(swerve.getPose().getTranslation()).getAngle(); + Translation2d translation = new Translation2d(Alignment.INTO_CHAIN_SPEED.get(), translationAngle); + + swerve.setControl(drive.withVelocityX(translation.getX()) + .withVelocityY(translation.getY()) + .withRotationalRate(0) + ); + } + + private double getDistanceToTrap() { + return swerve.getPose().getTranslation().minus(trapPose.getTranslation()).getNorm(); + } + + @Override + public boolean isFinished() { + return false; + // return getDistanceToTrap() <= Alignment.TRAP_CLIMB_DISTANCE.get(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java new file mode 100644 index 00000000..780720ed --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java @@ -0,0 +1,115 @@ +package com.stuypulse.robot.commands.swerve; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToClimb extends Command { + + private final SwerveDrive swerve; + + private final SwerveRequest.FieldCentric drive; + + private final HolonomicController controller; + + private final FieldObject2d targetPose2d; + + private Pose2d targetPose; + + private final double distance; + + public SwerveDriveDriveToClimb() { + this(Alignment.CLIMB_SETUP_DISTANCE.get()); + } + + public SwerveDriveDriveToClimb(double distance) { + swerve = SwerveDrive.getInstance(); + + drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + this.distance = distance; + + targetPose2d = swerve.getField().getObject("Target Pose"); + + controller = new HolonomicController( + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new AnglePIDController(Rotation.kP, Rotation.kI, Rotation.kD)); + } + + private Pose2d getTargetPose() { + Pose2d closestTrap = Field.getClosestAllianceTrapPose(swerve.getPose()); + Translation2d offsetTranslation = new Translation2d(distance, closestTrap.getRotation()); + + return new Pose2d(closestTrap.getTranslation().plus(offsetTranslation), closestTrap.getRotation()); + } + + private boolean shouldSlow() { + double toTarget = getTargetPose() + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm(); + + return toTarget < Units.inchesToMeters(14.0); + } + + @Override + public void initialize() { + targetPose = getTargetPose(); + } + + @Override + public void execute() { + targetPose2d.setPose(targetPose); + controller.update(targetPose, swerve.getPose()); + + double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get()); + if (Math.abs(rotation) < Alignment.Rotation.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond) + .clamp(2.0); + + if (shouldSlow()) + speed = speed.clamp(0.3); + + SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance()); + + swerve.setControl(drive.withVelocityX(speed.x) + .withVelocityY(speed.y) + .withRotationalRate(rotation) + ); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + Field.clearFieldObject(targetPose2d); + } +} + + diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index fb9ae1e6..44587d3e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -68,7 +68,8 @@ public interface Arm { SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50); SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 17); + SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7); + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 17); SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -60); SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); @@ -293,7 +294,8 @@ public interface Alignment { SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); - SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Trap/Into Chain Speed", 0.25); + SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); + SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); double MAX_ALIGNMENT_SPEED = 2.5; @@ -307,20 +309,8 @@ public interface Rotation { SmartNumber kP = new SmartNumber("Alignment/Rotation/kP", 6.0); SmartNumber kI = new SmartNumber("Alignment/Rotation/kI", 0.0); SmartNumber kD = new SmartNumber("Alignment/Rotation/kD", 0.4); - } - public interface Shoot { - public interface Translation { - SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); - SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.7); - } - - public interface Rotation { - SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); - SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); - } + SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Alignment/Rotation/Omega Deadband", 0.05); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 745bfe8d..9badf1e2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -25,6 +25,7 @@ public enum State { FEED, STOW, PRE_CLIMB, + CLIMBING, RESETTING } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 2eeb2d1d..e1c8abc4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -90,6 +90,8 @@ private double getTargetDegrees() { return Settings.Arm.MIN_ANGLE.getAsDouble(); case PRE_CLIMB: return Settings.Arm.PRE_CLIMB_ANGLE.getAsDouble(); + case CLIMBING: + return Settings.Arm.POST_CLIMB_ANGLE.get(); default: return Settings.Arm.MIN_ANGLE.getAsDouble(); } From 900f29d8b76bd0b100f4e56464f85d80da98c109 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sun, 28 Jul 2024 14:26:13 -0400 Subject: [PATCH 08/11] switch arm to amp button --- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 4bfea604..e261c71e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -164,7 +164,7 @@ private void configureDriverBindings() { () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); // arm to amp and alignment - driver.getLeftTriggerButton() + driver.getLeftBumper() .onTrue(new ArmToAmp()) .onTrue(new SwerveDriveDriveAlignedAmp(driver)); @@ -196,7 +196,7 @@ private void configureDriverBindings() { driver.getRightButton().whileTrue(new SwerveDriveDriveToClimb()); - driver.getLeftBumper().onTrue(new ArmToClimbing()); + driver.getLeftMenuButton().onTrue(new ArmToClimbing()); } private void configureOperatorBindings() { From 76e2c87884c8daafab535c1884bdde114d989df3 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sun, 28 Jul 2024 15:10:28 -0400 Subject: [PATCH 09/11] low and high shot modes for ferry and speaker shots instead of double clicking --- .../com/stuypulse/robot/RobotContainer.java | 59 ++++++------------- .../commands/arm/ArmSetShootHeightToHigh.java | 19 ++++++ .../commands/arm/ArmSetShootHeightToLow.java | 19 ++++++ .../robot/commands/arm/ArmToFerry.java | 31 ++-------- .../robot/commands/arm/ArmToLobFerry.java | 10 ---- .../robot/commands/arm/ArmToLowFerry.java | 10 ---- .../robot/commands/arm/ArmToSpeaker.java | 31 ++-------- .../robot/commands/arm/ArmToSpeakerHigh.java | 10 ---- .../robot/commands/arm/ArmToSpeakerLow.java | 10 ---- .../commands/shooter/ShooterAutoShoot.java | 10 +--- .../swerve/SwerveDriveDriveToClimb.java | 1 - .../SwerveDriveAutoAlignment.java | 46 --------------- .../SwerveDriveDriveAndLobFerryManual.java | 35 ----------- .../SwerveDriveDriveAndLowFerry.java | 35 ----------- .../SwerveDriveDriveAndScoreSpeakerLow.java | 30 ---------- .../SwerveDriveDriveAndFerry.java} | 12 ++-- .../SwerveDriveDriveAndFerryManual.java} | 12 ++-- .../SwerveDriveDriveAndScoreSpeaker.java} | 12 ++-- .../SwerveDriveDriveAndShoot.java} | 25 ++++---- .../stuypulse/robot/constants/Settings.java | 3 +- .../stuypulse/robot/subsystems/arm/Arm.java | 25 ++++++-- .../robot/subsystems/arm/ArmImpl.java | 14 ++--- .../robot/subsystems/shooter/ShooterImpl.java | 17 +++--- 23 files changed, 137 insertions(+), 339 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveAutoAlignment.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java rename src/main/java/com/stuypulse/robot/commands/swerve/{driveAndScore/SwerveDriveDriveAndLobFerry.java => driveAndShoot/SwerveDriveDriveAndFerry.java} (59%) rename src/main/java/com/stuypulse/robot/commands/swerve/{driveAndScore/SwerveDriveDriveAndLowFerryManual.java => driveAndShoot/SwerveDriveDriveAndFerryManual.java} (60%) rename src/main/java/com/stuypulse/robot/commands/swerve/{driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java => driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java} (64%) rename src/main/java/com/stuypulse/robot/commands/swerve/{driveAndScore/SwerveDriveDriveAndScore.java => driveAndShoot/SwerveDriveDriveAndShoot.java} (90%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e261c71e..66c103fb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -2,17 +2,14 @@ import com.ctre.phoenix6.Utils; import com.stuypulse.robot.commands.BuzzController; +import com.stuypulse.robot.commands.arm.ArmSetShootHeightToHigh; +import com.stuypulse.robot.commands.arm.ArmSetShootHeightToLow; import com.stuypulse.robot.commands.arm.ArmToAmp; import com.stuypulse.robot.commands.arm.ArmToClimbing; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.arm.ArmToFerry; -import com.stuypulse.robot.commands.arm.ArmToLobFerry; -import com.stuypulse.robot.commands.arm.ArmToLowFerry; import com.stuypulse.robot.commands.arm.ArmToPreClimb; import com.stuypulse.robot.commands.arm.ArmToSpeaker; -import com.stuypulse.robot.commands.arm.ArmToSpeakerHigh; -import com.stuypulse.robot.commands.arm.ArmToSpeaker; -import com.stuypulse.robot.commands.arm.ArmToSpeakerLow; import com.stuypulse.robot.commands.arm.ArmToStow; import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; @@ -32,17 +29,13 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToClimb; import com.stuypulse.robot.commands.swerve.SwerveDriveXMode; -import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveAutoAlignment; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerHigh; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerLow; -import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLobFerry; -import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLobFerryManual; -import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLowFerry; -import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndLowFerryManual; -import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerHigh; -import com.stuypulse.robot.commands.swerve.driveAndScore.SwerveDriveDriveAndScoreSpeakerLow; +import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerry; +import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerryManual; +import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndScoreSpeaker; import com.stuypulse.robot.commands.swerve.noteAlignment.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.constants.Ports; @@ -119,7 +112,7 @@ private void configureButtonBindings() { } private void configureDriverBindings() { - driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative()); + driver.getRightMenuButton().onTrue(new SwerveDriveSeedFieldRelative()); // intake field relative driver.getRightTriggerButton() @@ -142,26 +135,17 @@ private void configureDriverBindings() { .whileTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); + driver.getDPadUp().onTrue(new ArmSetShootHeightToHigh()); + driver.getDPadDown().onTrue(new ArmSetShootHeightToLow()); + // speaker align and score driver.getRightBumper() - .onTrue(new ArmToSpeaker()); - driver.getRightBumper() - .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) - .whileTrue(new ConditionalCommand( - new SwerveDriveDriveAndScoreSpeakerLow(driver), - new SwerveDriveDriveAndScoreSpeakerHigh(driver), - () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW)); + .whileTrue(new SwerveDriveDriveAndScoreSpeaker(driver)); // ferry align and shoot // move to back of controller - driver.getRightMenuButton() - .onTrue(new ArmToFerry()); - driver.getRightMenuButton() - .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) - .whileTrue(new ConditionalCommand( - new SwerveDriveDriveAndLowFerry(driver), - new SwerveDriveDriveAndLobFerry(driver), - () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); + driver.getRightStickButton() + .whileTrue(new SwerveDriveDriveAndFerry(driver)); // arm to amp and alignment driver.getLeftBumper() @@ -171,7 +155,7 @@ private void configureDriverBindings() { // manual speaker at subwoofer // score amp // rebind to a button on the back later - driver.getRightMenuButton() + driver.getLeftStickButton() .whileTrue(new ConditionalCommand( new ShooterScoreAmp(), new ArmToSubwooferShot() @@ -180,22 +164,15 @@ private void configureDriverBindings() { // manual ferry driver.getTopButton() - .onTrue(new ArmToFerry()); - driver.getTopButton() - .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) - .whileTrue(new ConditionalCommand( - new SwerveDriveDriveAndLowFerryManual(driver), - new SwerveDriveDriveAndLobFerryManual(driver), - () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); + .whileTrue(new SwerveDriveDriveAndFerryManual(driver)); - driver.getBottomButton() + driver.getRightButton() .onTrue(new ArmToPreClimb()); - driver.getBottomButton() - .debounce(Settings.Driver.HOLD_TIME_FOR_AUTOMATED_SCORING) + driver.getRightButton() + .debounce(Settings.Driver.TIME_UNTIL_HOLD) .whileTrue(new SwerveDriveDriveToChain()); - driver.getRightButton().whileTrue(new SwerveDriveDriveToClimb()); - + driver.getBottomButton().whileTrue(new SwerveDriveDriveToChain()); driver.getLeftMenuButton().onTrue(new ArmToClimbing()); } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java new file mode 100644 index 00000000..408e153d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ArmSetShootHeightToHigh extends InstantCommand{ + + private final Arm arm; + + public ArmSetShootHeightToHigh() { + arm = Arm.getInstance(); + addRequirements(arm); + } + + @Override + public void initialize() { + arm.setShootHeightHigh(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java new file mode 100644 index 00000000..975ab487 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ArmSetShootHeightToLow extends InstantCommand{ + + private final Arm arm; + + public ArmSetShootHeightToLow() { + arm = Arm.getInstance(); + addRequirements(arm); + } + + @Override + public void initialize() { + arm.setShootHeightLow(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java index 009e4010..4b64cffb 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java @@ -1,33 +1,10 @@ package com.stuypulse.robot.commands.arm; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.stuylib.util.StopWatch; -import edu.wpi.first.wpilibj2.command.InstantCommand; +public class ArmToFerry extends ArmSetState{ -public class ArmToFerry extends InstantCommand{ - - private final Arm arm; - private final StopWatch stopWatch; - private double lastClick; - - public ArmToFerry() { - arm = Arm.getInstance(); - stopWatch = new StopWatch(); - lastClick = 0; - addRequirements(arm); - } - - @Override - public void initialize() { - if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) { - arm.setState(Arm.State.LOB_FERRY); - } - else { - arm.setState(Arm.State.LOW_FERRY); - } - lastClick = stopWatch.getTime(); + public ArmToFerry(){ + super(Arm.State.SPEAKER); } - -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java deleted file mode 100644 index d4ff5c97..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToLobFerry extends ArmSetState{ - - public ArmToLobFerry(){ - super(Arm.State.LOB_FERRY); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java deleted file mode 100644 index 902563b9..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToLowFerry extends ArmSetState{ - - public ArmToLowFerry(){ - super(Arm.State.LOW_FERRY); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java index 7282a898..2358b8e7 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java @@ -1,33 +1,10 @@ package com.stuypulse.robot.commands.arm; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.stuylib.util.StopWatch; -import edu.wpi.first.wpilibj2.command.InstantCommand; +public class ArmToSpeaker extends ArmSetState{ -public class ArmToSpeaker extends InstantCommand{ - - private final Arm arm; - private final StopWatch stopWatch; - private double lastClick; - - public ArmToSpeaker() { - arm = Arm.getInstance(); - stopWatch = new StopWatch(); - lastClick = 0; - addRequirements(arm); - } - - @Override - public void initialize() { - if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) { - arm.setState(Arm.State.SPEAKER_HIGH); - } - else { - arm.setState(Arm.State.SPEAKER_LOW); - } - lastClick = stopWatch.getTime(); + public ArmToSpeaker(){ + super(Arm.State.SPEAKER); } - -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java deleted file mode 100644 index bf5c1f2b..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToSpeakerHigh extends ArmSetState{ - - public ArmToSpeakerHigh(){ - super(Arm.State.SPEAKER_HIGH); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java deleted file mode 100644 index 6ce31e50..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToSpeakerLow extends ArmSetState{ - - public ArmToSpeakerLow(){ - super(Arm.State.SPEAKER_LOW); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java index 7d0dd506..8bed36ac 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java @@ -16,16 +16,10 @@ public void initialize() { case AMP: CommandScheduler.getInstance().schedule(new ShooterScoreAmp()); break; - case SPEAKER_HIGH: + case SPEAKER: CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); break; - case SPEAKER_LOW: - CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); - break; - case LOW_FERRY: - CommandScheduler.getInstance().schedule(new ShooterFerry()); - break; - case LOB_FERRY: + case FERRY: CommandScheduler.getInstance().schedule(new ShooterFerry()); break; default: diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java index 780720ed..b4ce4e79 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java @@ -18,7 +18,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveAutoAlignment.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveAutoAlignment.java deleted file mode 100644 index 01061d45..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveAutoAlignment.java +++ /dev/null @@ -1,46 +0,0 @@ -package com.stuypulse.robot.commands.swerve.driveAligned; - -import com.stuypulse.robot.commands.BuzzController; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.stuylib.input.Gamepad; - -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -/** - * automatically determines how to align depending on the state of the arm - * - *

buzzes controller if arm is not in one of the scoring/ferrying positions - */ -public class SwerveDriveAutoAlignment extends InstantCommand{ - - private final Gamepad driver; - - public SwerveDriveAutoAlignment(Gamepad driver) { - this.driver = driver; - } - - @Override - public void initialize() { - switch (Arm.getInstance().getState()) { - case AMP: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedAmp(driver)); - break; - case SPEAKER_HIGH: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedSpeakerHigh(driver)); - break; - case SPEAKER_LOW: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedSpeakerLow(driver)); - break; - case LOW_FERRY: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedLowFerry(driver)); - break; - case LOB_FERRY: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedLobFerry(driver)); - break; - default: - CommandScheduler.getInstance().schedule(new BuzzController(driver)); - break; - } - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java deleted file mode 100644 index bda4074a..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerryManual.java +++ /dev/null @@ -1,35 +0,0 @@ -package com.stuypulse.robot.commands.swerve.driveAndScore; - -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.input.Gamepad; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -public class SwerveDriveDriveAndLobFerryManual extends SwerveDriveDriveAndScore{ - - public SwerveDriveDriveAndLobFerryManual(Gamepad driver) { - super(driver, Arm.State.SPEAKER_HIGH); - } - - private Translation2d getAmpCornerPose() { - Translation2d targetPose = Robot.isBlue() - ? new Translation2d(0.0, Field.WIDTH - 1.5) - : new Translation2d(0.0, 1.5); - - return targetPose; - } - - @Override - protected Rotation2d getTargetAngle() { - return Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); - } - - @Override - protected double getDistanceToTarget() { - return Field.getManualFerryPosition().getDistance(getAmpCornerPose()); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java deleted file mode 100644 index 8142034d..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerry.java +++ /dev/null @@ -1,35 +0,0 @@ -package com.stuypulse.robot.commands.swerve.driveAndScore; - -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.input.Gamepad; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -public class SwerveDriveDriveAndLowFerry extends SwerveDriveDriveAndScore{ - - public SwerveDriveDriveAndLowFerry(Gamepad driver) { - super(driver, Arm.State.LOW_FERRY); - } - - private Translation2d getAmpCornerPose() { - Translation2d targetPose = Robot.isBlue() - ? new Translation2d(0.0, Field.WIDTH - 1.5) - : new Translation2d(0.0, 1.5); - - return targetPose; - } - - @Override - protected Rotation2d getTargetAngle() { - return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle(); - } - - @Override - protected double getDistanceToTarget() { - return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose()); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java deleted file mode 100644 index a29dfabf..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerLow.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.stuypulse.robot.commands.swerve.driveAndScore; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.input.Gamepad; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -public class SwerveDriveDriveAndScoreSpeakerLow extends SwerveDriveDriveAndScore{ - - public SwerveDriveDriveAndScoreSpeakerLow(Gamepad driver) { - super(driver, Arm.State.SPEAKER_LOW); - } - - @Override - protected Rotation2d getTargetAngle() { - Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); - Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); - return speakerPose.minus(currentPose).getAngle(); - } - - @Override - protected double getDistanceToTarget() { - Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); - Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); - return currentPose.getDistance(speakerPose); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java similarity index 59% rename from src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java index 4dfc29ed..5b3bb0ba 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLobFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve.driveAndScore; +package com.stuypulse.robot.commands.swerve.driveAndShoot; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; @@ -9,10 +9,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -public class SwerveDriveDriveAndLobFerry extends SwerveDriveDriveAndScore{ +public class SwerveDriveDriveAndFerry extends SwerveDriveDriveAndShoot{ - public SwerveDriveDriveAndLobFerry(Gamepad driver) { - super(driver, Arm.State.LOB_FERRY); + public SwerveDriveDriveAndFerry(Gamepad driver) { + super(driver, Arm.State.FERRY); } private Translation2d getAmpCornerPose() { @@ -25,7 +25,9 @@ private Translation2d getAmpCornerPose() { @Override protected Rotation2d getTargetAngle() { - return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); + return Arm.getInstance().getShootHeight() == Arm.ShootHeight.LOW + ? SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle() + : SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java similarity index 60% rename from src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerryManual.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java index b62efbb4..8c782992 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndLowFerryManual.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve.driveAndScore; +package com.stuypulse.robot.commands.swerve.driveAndShoot; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; @@ -9,10 +9,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -public class SwerveDriveDriveAndLowFerryManual extends SwerveDriveDriveAndScore{ +public class SwerveDriveDriveAndFerryManual extends SwerveDriveDriveAndShoot{ - public SwerveDriveDriveAndLowFerryManual(Gamepad driver) { - super(driver, Arm.State.SPEAKER_HIGH); + public SwerveDriveDriveAndFerryManual(Gamepad driver) { + super(driver, Arm.State.FERRY); } private Translation2d getAmpCornerPose() { @@ -25,7 +25,9 @@ private Translation2d getAmpCornerPose() { @Override protected Rotation2d getTargetAngle() { - return Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle(); + return Arm.getInstance().getShootHeight() == Arm.ShootHeight.LOW + ? Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle() + : Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java similarity index 64% rename from src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java index dc70c1df..b2ba95f0 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScoreSpeakerHigh.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve.driveAndScore; +package com.stuypulse.robot.commands.swerve.driveAndShoot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.arm.Arm; @@ -8,17 +8,19 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -public class SwerveDriveDriveAndScoreSpeakerHigh extends SwerveDriveDriveAndScore{ +public class SwerveDriveDriveAndScoreSpeaker extends SwerveDriveDriveAndShoot{ - public SwerveDriveDriveAndScoreSpeakerHigh(Gamepad driver) { - super(driver, Arm.State.SPEAKER_HIGH); + public SwerveDriveDriveAndScoreSpeaker(Gamepad driver) { + super(driver, Arm.State.SPEAKER); } @Override protected Rotation2d getTargetAngle() { Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); - return speakerPose.minus(currentPose).getAngle().plus(Rotation2d.fromDegrees(180)); + return Arm.getInstance().getShootHeight() == Arm.ShootHeight.LOW + ? speakerPose.minus(currentPose).getAngle() + : speakerPose.minus(currentPose).getAngle().plus(Rotation2d.fromDegrees(180)); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java similarity index 90% rename from src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java index 4ca9f8c9..6dab0288 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndScore/SwerveDriveDriveAndScore.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve.driveAndScore; +package com.stuypulse.robot.commands.swerve.driveAndShoot; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; @@ -30,7 +30,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -public abstract class SwerveDriveDriveAndScore extends Command { +public abstract class SwerveDriveDriveAndShoot extends Command { private final SwerveDrive swerve; private final Arm arm; @@ -48,7 +48,7 @@ public abstract class SwerveDriveDriveAndScore extends Command { private final AngleController controller; - public SwerveDriveDriveAndScore(Gamepad driver, Arm.State armState) { + public SwerveDriveDriveAndShoot(Gamepad driver, Arm.State armState) { swerve = SwerveDrive.getInstance(); shooter = Shooter.getInstance(); arm = Arm.getInstance(); @@ -98,14 +98,13 @@ private boolean isAligned() { @Override public void initialize() { arm.setState(armState); - if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { - shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); - } - else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { - shooter.setTargetSpeeds(shooter.getFerrySpeeds()); - } - else { - shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); + switch (armState) { + case SPEAKER: + shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); + case FERRY: + shooter.setTargetSpeeds(shooter.getFerrySpeeds()); + default: + shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); } } @@ -126,6 +125,10 @@ public void execute() { CommandScheduler.getInstance().schedule(new ShooterAutoShoot()); } + if (armState == Arm.State.FERRY) { + shooter.setTargetSpeeds(shooter.getFerrySpeeds()); + } + } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 44587d3e..26f58c7e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -315,8 +315,7 @@ public interface Rotation { } public interface Driver { - double DOUBLE_CLICK_TIME_BETWEEN_CLICKS = 0.5; - double HOLD_TIME_FOR_AUTOMATED_SCORING = 0.7; + double TIME_UNTIL_HOLD = 0.7; public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.015); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 9badf1e2..81b73014 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -18,10 +18,8 @@ public static Arm getInstance(){ public enum State { AMP, SUBWOOFER_SHOT, - SPEAKER_HIGH, - SPEAKER_LOW, - LOW_FERRY, - LOB_FERRY, + SPEAKER, + FERRY, FEED, STOW, PRE_CLIMB, @@ -29,10 +27,17 @@ public enum State { RESETTING } + public enum ShootHeight { + HIGH, + LOW + } + protected State state; + protected ShootHeight shootHeight; protected Arm() { state = State.RESETTING; + shootHeight = ShootHeight.LOW; } public void setState(State state) { @@ -43,6 +48,18 @@ public State getState() { return this.state; } + public void setShootHeightHigh() { + this.shootHeight = ShootHeight.HIGH; + } + + public void setShootHeightLow() { + this.shootHeight = ShootHeight.LOW; + } + + public ShootHeight getShootHeight() { + return this.shootHeight; + } + public abstract boolean atTarget(); @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index e1c8abc4..f957622a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -76,14 +76,12 @@ private double getTargetDegrees() { return Settings.Arm.AMP_ANGLE.getAsDouble(); case SUBWOOFER_SHOT: return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); - case SPEAKER_LOW: - return getSpeakerAngle(true); - case SPEAKER_HIGH: - return getSpeakerAngle(false); - case LOW_FERRY: - return Settings.Arm.LOW_FERRY_ANGLE.getAsDouble(); - case LOB_FERRY: - return Settings.Arm.LOB_FERRY_ANGLE.getAsDouble(); + case SPEAKER: + return getSpeakerAngle(shootHeight == ShootHeight.LOW); + case FERRY: + return shootHeight == ShootHeight.LOW + ? Settings.Arm.LOW_FERRY_ANGLE.getAsDouble() + : Settings.Arm.LOB_FERRY_ANGLE.getAsDouble(); case FEED: return Settings.Arm.FEED_ANGLE.getAsDouble(); case STOW: diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 354c6c6c..4e0af099 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -135,7 +135,7 @@ public ShooterSpeeds getFerrySpeeds() { double distanceToFerryInInches = Units.metersToInches(SwerveDrive.getInstance().getPose().getTranslation().getDistance(ferryZone)); - if (Arm.getInstance().getState() == Arm.State.LOB_FERRY) { + if (Arm.getInstance().getShootHeight() == Arm.ShootHeight.HIGH) { double targetRPM = ShooterLobFerryInterpolation.getRPM(distanceToFerryInInches); return new ShooterSpeeds(targetRPM, 500); } @@ -152,14 +152,13 @@ public void periodic () { Arm.State armState = Arm.getInstance().getState(); if (Settings.Shooter.ALWAYS_KEEP_AT_SPEED) { - if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { - setTargetSpeeds(Settings.Shooter.SPEAKER); - } - else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { - setTargetSpeeds(getFerrySpeeds()); - } - else { - setTargetSpeeds(Settings.Shooter.SPEAKER); + switch (armState) { + case SPEAKER: + setTargetSpeeds(Settings.Shooter.SPEAKER); + case FERRY: + setTargetSpeeds(getFerrySpeeds()); + default: + setTargetSpeeds(Settings.Shooter.SPEAKER); } } From fc0cff72db580fce4dbc10616a49778109cd8fb7 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Mon, 29 Jul 2024 21:33:15 -0400 Subject: [PATCH 10/11] fix drive and shoot alignment --- .../com/stuypulse/robot/RobotContainer.java | 8 +++- .../commands/shooter/ShooterAutoShoot.java | 30 ------------- .../commands/shooter/ShooterFerryManual.java | 16 +++++++ .../driveAligned/SwerveDriveDriveAligned.java | 4 +- .../SwerveDriveDriveAndFerry.java | 16 +++++++ .../SwerveDriveDriveAndFerryManual.java | 19 +++++++- .../SwerveDriveDriveAndScoreSpeaker.java | 18 ++++++++ .../SwerveDriveDriveAndShoot.java | 44 ++++++------------- .../stuypulse/robot/constants/Cameras.java | 4 +- .../stuypulse/robot/constants/Settings.java | 18 +++++--- .../robot/subsystems/shooter/Shooter.java | 4 +- .../robot/subsystems/shooter/ShooterImpl.java | 15 +------ 12 files changed, 107 insertions(+), 89 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 66c103fb..cab92f2c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -18,12 +18,12 @@ import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; -import com.stuypulse.robot.commands.shooter.ShooterAutoShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; import com.stuypulse.robot.commands.shooter.ShooterScoreAmp; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterSetRPM; +import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain; @@ -160,7 +160,11 @@ private void configureDriverBindings() { new ShooterScoreAmp(), new ArmToSubwooferShot() .andThen(new ShooterScoreSpeaker()), - () -> Arm.getInstance().getState() == Arm.State.AMP)); + () -> Arm.getInstance().getState() == Arm.State.AMP)) + .onFalse(new ConditionalCommand( + new ShooterFeederStop(), + new ShooterStop(), + () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); // manual ferry driver.getTopButton() diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java deleted file mode 100644 index 8bed36ac..00000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.subsystems.arm.Arm; - -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -/** - * automatically determines how to shoot depending on the state of the arm - */ -public class ShooterAutoShoot extends InstantCommand{ - - @Override - public void initialize() { - switch (Arm.getInstance().getState()) { - case AMP: - CommandScheduler.getInstance().schedule(new ShooterScoreAmp()); - break; - case SPEAKER: - CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); - break; - case FERRY: - CommandScheduler.getInstance().schedule(new ShooterFerry()); - break; - default: - CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); - break; - } - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java new file mode 100644 index 00000000..c7bd69b7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java @@ -0,0 +1,16 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class ShooterFerryManual extends SequentialCommandGroup { + + public ShooterFerryManual() { + addCommands( + new ShooterSetRPM(Settings.Shooter.MANUAL_FERRY), + new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)), + new ShooterFeederShoot() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index d663dfe5..7f340daa 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -50,7 +50,7 @@ public SwerveDriveDriveAligned(Gamepad driver) { .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kP) + controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); @@ -75,7 +75,7 @@ protected double getAngleError() { @Override public boolean isFinished() { - return Math.abs(driver.getRightX()) > Settings.Driver.Turn.DEADBAND.getAsDouble(); + return Math.abs(driver.getRightX()) > Settings.Driver.Turn.DISABLE_ALIGNMENT_DEADBAND.getAsDouble(); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java index 5b3bb0ba..b10c1dd0 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java @@ -1,13 +1,16 @@ package com.stuypulse.robot.commands.swerve.driveAndShoot; import com.stuypulse.robot.Robot; +import com.stuypulse.robot.commands.shooter.ShooterFerry; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; public class SwerveDriveDriveAndFerry extends SwerveDriveDriveAndShoot{ @@ -34,4 +37,17 @@ protected Rotation2d getTargetAngle() { protected double getDistanceToTarget() { return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose()); } + + @Override + protected ShooterSpeeds getTargetSpeeds() { + return shooter.getFerrySpeeds(); + } + + @Override + public void execute() { + super.execute(); + if (isAligned.get()) { + CommandScheduler.getInstance().schedule(new ShooterFerry()); + } + } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java index 8c782992..456148c8 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java @@ -1,13 +1,17 @@ package com.stuypulse.robot.commands.swerve.driveAndShoot; import com.stuypulse.robot.Robot; +import com.stuypulse.robot.commands.shooter.ShooterFerry; +import com.stuypulse.robot.commands.shooter.ShooterFerryManual; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; public class SwerveDriveDriveAndFerryManual extends SwerveDriveDriveAndShoot{ @@ -34,4 +38,17 @@ protected Rotation2d getTargetAngle() { protected double getDistanceToTarget() { return Field.getManualFerryPosition().getDistance(getAmpCornerPose()); } + + @Override + protected ShooterSpeeds getTargetSpeeds() { + return Settings.Shooter.MANUAL_FERRY; + } + + @Override + public void execute() { + super.execute(); + if (isAligned.get()) { + CommandScheduler.getInstance().schedule(new ShooterFerryManual()); + } + } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java index b2ba95f0..0b0c5493 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java @@ -1,12 +1,17 @@ package com.stuypulse.robot.commands.swerve.driveAndShoot; +import com.stuypulse.robot.commands.shooter.ShooterFerry; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; public class SwerveDriveDriveAndScoreSpeaker extends SwerveDriveDriveAndShoot{ @@ -29,4 +34,17 @@ protected double getDistanceToTarget() { Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); return currentPose.getDistance(speakerPose); } + + @Override + protected ShooterSpeeds getTargetSpeeds() { + return Settings.Shooter.SPEAKER; + } + + @Override + public void execute() { + super.execute(); + if (isAligned.get()) { + CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); + } + } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java index 6dab0288..4eb7600d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.stuypulse.robot.commands.shooter.ShooterAutoShoot; import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Alignment; @@ -32,21 +31,21 @@ public abstract class SwerveDriveDriveAndShoot extends Command { - private final SwerveDrive swerve; - private final Arm arm; - private final Shooter shooter; + protected final SwerveDrive swerve; + protected final Arm arm; + protected final Shooter shooter; - private final Arm.State armState; + protected final Arm.State armState; protected final Gamepad driver; - private final VStream velocity; - private final IStream angleVelocity; + protected final VStream velocity; + protected final IStream angleVelocity; - private final BStream isAligned; + protected final BStream isAligned; - private final SwerveRequest.FieldCentric drive; + protected final SwerveRequest.FieldCentric drive; - private final AngleController controller; + protected final AngleController controller; public SwerveDriveDriveAndShoot(Gamepad driver, Arm.State armState) { swerve = SwerveDrive.getInstance(); @@ -69,7 +68,7 @@ public SwerveDriveDriveAndShoot(Gamepad driver, Arm.State armState) { .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kP) + controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kD) .setOutputFilter(x -> -x); isAligned = BStream.create(this::isAligned) @@ -88,9 +87,10 @@ public SwerveDriveDriveAndShoot(Gamepad driver, Arm.State armState) { } protected abstract Rotation2d getTargetAngle(); - protected abstract double getDistanceToTarget(); + protected abstract ShooterSpeeds getTargetSpeeds(); + private boolean isAligned() { return controller.isDoneDegrees(Alignment.ANGLE_TOLERANCE.get()); } @@ -98,14 +98,7 @@ private boolean isAligned() { @Override public void initialize() { arm.setState(armState); - switch (armState) { - case SPEAKER: - shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); - case FERRY: - shooter.setTargetSpeeds(shooter.getFerrySpeeds()); - default: - shooter.setTargetSpeeds(Settings.Shooter.SPEAKER); - } + shooter.setTargetSpeeds(getTargetSpeeds()); } @Override @@ -120,19 +113,10 @@ public void execute() { Angle.fromRotation2d(swerve.getPose().getRotation())) ) ); - - if (isAligned.get()) { - CommandScheduler.getInstance().schedule(new ShooterAutoShoot()); - } - - if (armState == Arm.State.FERRY) { - shooter.setTargetSpeeds(shooter.getFerrySpeeds()); - } - } @Override public void end(boolean interrupted) { - CommandScheduler.getInstance().schedule(new ShooterStop()); + CommandScheduler.getInstance().schedule(new ShooterStop().onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index df90c72a..275446b2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -28,7 +28,7 @@ public interface Limelight { "samera0", //tower camera new Pose3d( new Translation3d(-11.25, -3.333797, 23.929362), - new Rotation3d(0, 15, 0) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(0)) ), "29", 3000 @@ -38,7 +38,7 @@ public interface Limelight { "samera1", //electronic plate camera new Pose3d( new Translation3d(0, 4.863591, 19.216471), - new Rotation3d(0, 80, 0) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0)) ), "96", 3001 diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 26f58c7e..cbb2a787 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -69,7 +69,7 @@ public interface Arm { SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 17); + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 16); SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -60); SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); @@ -97,7 +97,7 @@ public interface Encoder { } public interface Intake { - double INTAKE_ACQUIRE_SPEED = 0.8; + double INTAKE_ACQUIRE_SPEED = 0.75; double INTAKE_DEACQUIRE_SPEED = 1.0; double FUNNEL_ACQUIRE = 1.0; double FUNNEL_DEACQUIRE = 1.0; @@ -109,9 +109,9 @@ public interface Intake { } public interface Shooter { - double FEEDER_INTAKE_SPEED = 0.25; + double FEEDER_INTAKE_SPEED = 0.3; double FEEDER_DEAQUIRE_SPEED = 0.5; - double FEEDER_SHOOT_SPEED = 0.25; + double FEEDER_SHOOT_SPEED = 0.4; boolean ALWAYS_KEEP_AT_SPEED = false; @@ -123,6 +123,11 @@ public interface Shooter { new SmartNumber("Shooter/Speaker RPM differential", 500) ); + ShooterSpeeds MANUAL_FERRY = new ShooterSpeeds( + new SmartNumber("Shooter/Manual Ferry RPM", 4800), + new SmartNumber("Shooter/Manual Ferry RPM differential", 500) + ); + // Different falling debounce is used to detect note shooting; SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.01); SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.005); @@ -181,7 +186,7 @@ public interface Assist { double AMP_WALL_SCORE_DISTANCE = (Settings.LENGTH / 2) + Units.inchesToMeters(2.5); // angle PID - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); + SmartNumber kP = new SmartNumber("SwerveAssist/kP", 5.0); SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); @@ -328,7 +333,8 @@ public interface Drive { } public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.015); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.03); + SmartNumber DISABLE_ALIGNMENT_DEADBAND = new SmartNumber("Driver Settings/Turn/Disable Alignment Deadband", 0.08); SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index bf92c554..cb173b5a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -22,8 +22,8 @@ public static Shooter getInstance() { private final SmartNumber rightTargetRPM; public Shooter() { - leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", 0); - rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", 0); + leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getLeftRPM() : 0); + rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getRightRPM() : 0); } public void setTargetSpeeds(ShooterSpeeds speeds) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 4e0af099..70d8e88b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -77,7 +77,7 @@ protected ShooterImpl() { Motors.Shooter.LEFT_SHOOTER.configure(leftMotor); Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor); - Motors.Shooter.FEEDER_MOTOR.configure(feederMotor); + Motors.Shooter.FEEDER_MOTOR.configure(feederMotor); } private double getLeftShooterRPM() { @@ -149,19 +149,6 @@ public ShooterSpeeds getFerrySpeeds() { public void periodic () { super.periodic(); - Arm.State armState = Arm.getInstance().getState(); - - if (Settings.Shooter.ALWAYS_KEEP_AT_SPEED) { - switch (armState) { - case SPEAKER: - setTargetSpeeds(Settings.Shooter.SPEAKER); - case FERRY: - setTargetSpeeds(getFerrySpeeds()); - default: - setTargetSpeeds(Settings.Shooter.SPEAKER); - } - } - if (getLeftTargetRPM() == 0) { leftMotor.set(0); } From fcb95b2b0f5e7994e03f73506f7a4930d92f5390 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Mon, 29 Jul 2024 21:47:12 -0400 Subject: [PATCH 11/11] tune handoff --- src/main/java/com/stuypulse/robot/constants/Settings.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index cbb2a787..f0af98e9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -69,7 +69,7 @@ public interface Arm { SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 16); + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 17); SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -60); SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); @@ -97,7 +97,7 @@ public interface Encoder { } public interface Intake { - double INTAKE_ACQUIRE_SPEED = 0.75; + double INTAKE_ACQUIRE_SPEED = 0.6; double INTAKE_DEACQUIRE_SPEED = 1.0; double FUNNEL_ACQUIRE = 1.0; double FUNNEL_DEACQUIRE = 1.0; @@ -109,7 +109,7 @@ public interface Intake { } public interface Shooter { - double FEEDER_INTAKE_SPEED = 0.3; + double FEEDER_INTAKE_SPEED = 0.25; double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 0.4;