From 504818b1758d3e6129d03f3196de6f8967dc3a2a Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 23 Jul 2024 19:59:46 -0400 Subject: [PATCH 1/3] hold to override arm states --- src/main/java/com/stuypulse/robot/Robot.java | 2 +- .../com/stuypulse/robot/RobotContainer.java | 17 +++++---- .../robot/commands/arm/ArmSetState.java | 24 ++++++++++-- .../commands/shooter/ShooterAutoShoot.java | 2 +- .../swerve/SwerveDriveAutoAlignment.java | 2 +- .../stuypulse/robot/constants/Settings.java | 2 + .../stuypulse/robot/subsystems/arm/Arm.java | 38 ++++++++++++++----- .../robot/subsystems/arm/ArmImpl.java | 33 ++++++++++++---- .../robot/subsystems/shooter/ShooterImpl.java | 4 +- 9 files changed, 92 insertions(+), 32 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 0971a6e4..1f1b6116 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().getState() == Arm.State.FEED + if (Arm.getInstance().getActualState() == 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 6949b4e9..79462059 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -96,6 +96,7 @@ private void configureDefaultCommands() { private void configureButtonBindings() { driver.getRightBumper().whileTrue(new SwerveDriveXMode()); + driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative()); driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver)); driver.getLeftTriggerButton() @@ -114,23 +115,23 @@ private void configureButtonBindings() { .onFalse(new ShooterFeederStop()); driver.getTopButton() - .onTrue(new ConditionalCommand( + .whileTrue(new ConditionalCommand( new ArmToSpeakerHigh(), new ArmToSpeakerLow(), - () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW)); + () -> Arm.getInstance().getActualState() == Arm.State.SPEAKER_LOW)); - driver.getLeftButton().onTrue(new ArmToAmp()); + driver.getLeftButton().whileTrue(new ArmToAmp()); driver.getRightButton() - .onTrue(new ConditionalCommand( + .whileTrue(new ConditionalCommand( new ArmToLobFerry(), new ArmToLowFerry(), - () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); + () -> Arm.getInstance().getActualState() == Arm.State.LOW_FERRY)); - driver.getBottomButton().onTrue(new ArmToFeed()); + driver.getBottomButton().whileTrue(new ArmToFeed()); - driver.getDPadUp().onTrue(new ArmToPreClimb()); - driver.getDPadDown().onTrue(new ArmToStow()); + driver.getDPadUp().whileTrue(new ArmToPreClimb()); + driver.getDPadDown().whileTrue(new ArmToStow()); } 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 33700542..da57a07d 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java @@ -1,22 +1,40 @@ 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; +import edu.wpi.first.wpilibj2.command.Command; -public class ArmSetState extends InstantCommand{ +public class ArmSetState extends Command{ private final Arm arm; private final Arm.State state; + private final StopWatch stopWatch; public ArmSetState(Arm.State state) { arm = Arm.getInstance(); this.state = state; + this.stopWatch = new StopWatch(); addRequirements(arm); } @Override public void initialize() { - arm.setState(state); + arm.setRequestedState(state); + stopWatch.reset(); + } + + @Override + public void execute() { + arm.setRequestedState(state); + if (stopWatch.getTime() > Settings.Arm.HOLD_TO_OVERRIDE_TIME) { + arm.setOverriding(true); + } + } + + @Override + public void end(boolean interrupted) { + arm.setOverriding(false); } } 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..1377464c 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().getState()) { + switch (Arm.getInstance().getActualState()) { case AMP: CommandScheduler.getInstance().schedule(new ShooterScoreAmp()); break; 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 218f440c..c2feab30 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().getState()) { + switch (Arm.getInstance().getActualState()) { 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 74872822..4fb0fe06 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -77,6 +77,8 @@ public interface Arm { double MAX_WAIT_TO_REACH_TARGET = 2.0; + double HOLD_TO_OVERRIDE_TIME = 0.45; + // characterize and manually tune public interface PID { SmartNumber kP = new SmartNumber("Arm/kP", 0.20000); 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 75aeb73b..842ee3a1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -1,5 +1,7 @@ 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; @@ -28,26 +30,44 @@ public enum State { RESETTING } - protected State state; + protected State requestedState; + protected State actualState; + protected boolean overriding; protected Arm() { - state = State.RESETTING; + 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; } - public void setState(State state) { - if (this.state != State.RESETTING) { - this.state = state; - } + public void setOverriding(boolean overriding) { + this.overriding = overriding; } - public State getState() { - return this.state; + public boolean isOverriding() { + return this.overriding; } public abstract boolean atTarget(); + public abstract boolean shouldReturnToFeed(); + @Override public void periodic() { - SmartDashboard.putString("Arm/State", getState().toString()); + SmartDashboard.putBoolean("Arm/Is Overriding", overriding); + SmartDashboard.putString("Arm/Requested State", getRequestedState().toString()); + SmartDashboard.putString("Arm/Actual State", getActualState().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 a0f46554..c52391f2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -26,6 +26,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandScheduler; public class ArmImpl extends Arm { @@ -68,6 +69,8 @@ protected ArmImpl() { shouldGoBackToFeed = BStream.create(() -> !Shooter.getInstance().hasNote()) .filtered(new BDebounce.Rising(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME)); + + overriding = false; } @Override @@ -75,8 +78,13 @@ 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 (state) { + switch (actualState) { case AMP: return Settings.Arm.AMP_ANGLE.getAsDouble(); case SPEAKER_LOW: @@ -158,17 +166,28 @@ public void periodic() { if (bumpSwitchTriggered.get()) { armEncoder.setPosition(Settings.Arm.MIN_ANGLE.get()/360); - if (state == State.RESETTING) { - state = State.FEED; + if (actualState == State.RESETTING) { + actualState = State.FEED; + requestedState = State.FEED; } } - if (state == State.RESETTING) { - setVoltage(-2); + if (actualState == State.RESETTING) { + setVoltage(-1.5); } else { - if (state != State.PRE_CLIMB && state != State.STOW && shouldGoBackToFeed.get()) { - setState(State.FEED); + 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()) { + requestedState = State.FEED; + actualState = State.FEED; } controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); 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 be2bdac6..e64ec59e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -139,7 +139,7 @@ private ShooterSpeeds getFerrySpeeds() { double distanceToFerryInInches = Units.metersToInches(SwerveDrive.getInstance().getPose().getTranslation().getDistance(ferryZone)); - if (Arm.getInstance().getState() == Arm.State.LOB_FERRY) { + if (Arm.getInstance().getActualState() == Arm.State.LOB_FERRY) { double targetRPM = ShooterLobFerryInterpolation.getRPM(distanceToFerryInInches); return new ShooterSpeeds(targetRPM, 500); } @@ -153,7 +153,7 @@ private ShooterSpeeds getFerrySpeeds() { public void periodic () { super.periodic(); - Arm.State armState = Arm.getInstance().getState(); + Arm.State armState = Arm.getInstance().getActualState(); if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { setTargetSpeeds(Settings.Shooter.SPEAKER); From 3b466408aed435347bae5c14659fec5fa619acbd Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 25 Jul 2024 21:25:49 -0400 Subject: [PATCH 2/3] added overrides for single and double click controls --- .../com/stuypulse/robot/RobotContainer.java | 37 +++++++++++++------ .../commands/arm/ArmDisableOverride.java | 19 ++++++++++ .../robot/commands/arm/ArmEnableOverride.java | 19 ++++++++++ .../robot/commands/arm/ArmSetState.java | 23 +----------- .../robot/commands/arm/ArmToFerry.java | 33 +++++++++++++++++ .../robot/commands/arm/ArmToSpeaker.java | 33 +++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 7 +++- .../robot/subsystems/arm/ArmImpl.java | 10 ++--- .../util/ShooterLobFerryInterpolation.java | 5 ++- 9 files changed, 146 insertions(+), 40 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 79462059..1ce6980e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -2,12 +2,17 @@ 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; 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.ArmWaitUntilAtTarget; @@ -115,20 +120,30 @@ private void configureButtonBindings() { .onFalse(new ShooterFeederStop()); driver.getTopButton() - .whileTrue(new ConditionalCommand( - new ArmToSpeakerHigh(), - new ArmToSpeakerLow(), - () -> Arm.getInstance().getActualState() == Arm.State.SPEAKER_LOW)); - - driver.getLeftButton().whileTrue(new ArmToAmp()); + .onTrue(new ArmToSpeaker()); + + driver.getTopButton() + .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) + .onTrue(new ArmEnableOverride()) + .onFalse(new ArmDisableOverride()); + + driver.getLeftButton() + .onTrue(new ArmToAmp()); + + driver.getLeftButton() + .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) + .onTrue(new ArmEnableOverride()) + .onFalse(new ArmDisableOverride()); driver.getRightButton() - .whileTrue(new ConditionalCommand( - new ArmToLobFerry(), - new ArmToLowFerry(), - () -> Arm.getInstance().getActualState() == Arm.State.LOW_FERRY)); + .whileTrue(new ArmToFerry()); + + driver.getRightButton() + .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) + .onTrue(new ArmEnableOverride()) + .onFalse(new ArmDisableOverride()); - driver.getBottomButton().whileTrue(new ArmToFeed()); + driver.getBottomButton().onTrue(new ArmToFeed()); driver.getDPadUp().whileTrue(new ArmToPreClimb()); driver.getDPadDown().whileTrue(new ArmToStow()); diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java new file mode 100644 index 00000000..0babc58f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.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 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 new file mode 100644 index 00000000..9c89f6d4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.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 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 da57a07d..3f94c534 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java @@ -1,40 +1,21 @@ 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; -import edu.wpi.first.wpilibj2.command.Command; - -public class ArmSetState extends Command{ +public class ArmSetState extends InstantCommand{ private final Arm arm; private final Arm.State state; - private final StopWatch stopWatch; public ArmSetState(Arm.State state) { arm = Arm.getInstance(); this.state = state; - this.stopWatch = new StopWatch(); addRequirements(arm); } @Override public void initialize() { arm.setRequestedState(state); - stopWatch.reset(); - } - - @Override - public void execute() { - arm.setRequestedState(state); - if (stopWatch.getTime() > Settings.Arm.HOLD_TO_OVERRIDE_TIME) { - arm.setOverriding(true); - } - } - - @Override - public void end(boolean interrupted) { - arm.setOverriding(false); } } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java new file mode 100644 index 00000000..d39a34f3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java @@ -0,0 +1,33 @@ +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 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.setRequestedState(Arm.State.LOB_FERRY); + } + else { + arm.setRequestedState(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 new file mode 100644 index 00000000..eafa55aa --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java @@ -0,0 +1,33 @@ +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 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.setRequestedState(Arm.State.SPEAKER_HIGH); + } + else { + arm.setRequestedState(Arm.State.SPEAKER_LOW); + } + lastClick = stopWatch.getTime(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4fb0fe06..b5d3ec0d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -77,8 +77,6 @@ public interface Arm { double MAX_WAIT_TO_REACH_TARGET = 2.0; - double HOLD_TO_OVERRIDE_TIME = 0.45; - // characterize and manually tune public interface PID { SmartNumber kP = new SmartNumber("Arm/kP", 0.20000); @@ -118,6 +116,8 @@ public interface Shooter { double TARGET_RPM_THRESHOLD = 250; double MAX_WAIT_TO_REACH_TARGET = 1.5; + + boolean ALWAYS_KEEP_AT_SPEED = true; ShooterSpeeds SPEAKER = new ShooterSpeeds( new SmartNumber("Shooter/Speaker RPM", 4875), @@ -327,6 +327,9 @@ public interface Rotation { } public interface Driver { + double HOLD_TO_OVERRIDE_TIME = 0.55; + double DOUBLE_CLICK_TIME_BETWEEN_CLICKS = 0.5; + public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); 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 c52391f2..5d342af6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -69,8 +69,6 @@ protected ArmImpl() { shouldGoBackToFeed = BStream.create(() -> !Shooter.getInstance().hasNote()) .filtered(new BDebounce.Rising(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME)); - - overriding = false; } @Override @@ -186,8 +184,10 @@ public void periodic() { this.actualState = this.requestedState; } else if (shouldGoBackToFeed.get()) { - requestedState = State.FEED; - actualState = State.FEED; + 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()); @@ -198,7 +198,7 @@ else if (shouldGoBackToFeed.get()) { SmartDashboard.putNumber("Arm/Error (deg)", controller.getError()); SmartDashboard.putNumber("Arm/Output (V)", controller.getOutput()); - SmartDashboard.putBoolean("Arm/Bump Switch Triggered?", bumpSwitch.get()); + SmartDashboard.putBoolean("Arm/Bump Switch Triggered?", !bumpSwitch.get()); SmartDashboard.putNumber("Arm/Encoder Angle (deg))", getDegrees()); SmartDashboard.putNumber("Arm/Raw Encoder Angle (rot)", armEncoder.getPosition()); diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java index f3d8972c..c2a2250a 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java @@ -13,7 +13,10 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - {, }, + // fake data im just putting here so it doesnt crash lol + {1000, 55.5}, + {1000, 42.5}, + {1000, 57}, }; static { From 1257c636deb43692c2a6802b1b1c813f3bcbcde7 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 25 Jul 2024 22:11:53 -0400 Subject: [PATCH 3/3] settings to always hold shooter speed --- .../com/stuypulse/robot/RobotContainer.java | 5 +++- .../robot/commands/shooter/ShooterFerry.java | 3 +- .../commands/shooter/ShooterScoreSpeaker.java | 1 + .../robot/commands/shooter/ShooterSetRPM.java | 14 +++++---- .../stuypulse/robot/constants/Settings.java | 6 ++-- .../robot/subsystems/shooter/Shooter.java | 6 ++-- .../robot/subsystems/shooter/ShooterImpl.java | 30 ++++++++++++++----- 7 files changed, 45 insertions(+), 20 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1ce6980e..b41d3163 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,7 @@ 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.ShooterSetRPM; import com.stuypulse.robot.commands.swerve.SwerveDriveAutoAlignment; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry; @@ -40,6 +41,7 @@ import com.stuypulse.robot.subsystems.swerve.Telemetry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.Intake; @@ -117,7 +119,8 @@ private void configureButtonBindings() { .whileTrue(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .andThen(new ShooterAutoShoot()) ) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ShooterSetRPM(new ShooterSpeeds()).onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); driver.getTopButton() .onTrue(new ArmToSpeaker()); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java index e2aeee4b..ecd37b11 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java @@ -1,13 +1,14 @@ 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; public class ShooterFerry extends SequentialCommandGroup { public ShooterFerry() { addCommands( + new ShooterSetRPM(Shooter.getInstance()::getFerrySpeeds), new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)), new ShooterFeederShoot() ); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java index 61931553..76b2d342 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java @@ -8,6 +8,7 @@ public class ShooterScoreSpeaker extends SequentialCommandGroup { public ShooterScoreSpeaker() { addCommands( + new ShooterSetRPM(Settings.Shooter.SPEAKER), new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET), new ShooterFeederShoot() ); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java index bfeab4fb..31928d8e 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java @@ -1,5 +1,7 @@ package com.stuypulse.robot.commands.shooter; +import java.util.function.Supplier; + import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.util.ShooterSpeeds; @@ -8,19 +10,21 @@ public class ShooterSetRPM extends InstantCommand { private final Shooter shooter; - private final ShooterSpeeds speeds; + private final Supplier speeds; - public ShooterSetRPM(ShooterSpeeds speeds) { + public ShooterSetRPM(Supplier speeds) { shooter = Shooter.getInstance(); - this.speeds = speeds; - addRequirements(shooter); } + public ShooterSetRPM(ShooterSpeeds speeds) { + this(() -> speeds); + } + @Override public void initialize() { - shooter.setTargetSpeeds(speeds); + shooter.setTargetSpeeds(speeds.get()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b5d3ec0d..88658606 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -114,10 +114,10 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 0.25; - double TARGET_RPM_THRESHOLD = 250; - double MAX_WAIT_TO_REACH_TARGET = 1.5; + boolean ALWAYS_KEEP_AT_SPEED = false; - boolean ALWAYS_KEEP_AT_SPEED = true; + double TARGET_RPM_THRESHOLD = 250; + double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0; ShooterSpeeds SPEAKER = new ShooterSpeeds( new SmartNumber("Shooter/Speaker RPM", 4875), 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 b324e3d3..41f6b74d 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", Settings.Shooter.SPEAKER.getLeftRPM()); - rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.SPEAKER.getRightRPM()); + leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", 0); + rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", 0); } public void setTargetSpeeds(ShooterSpeeds speeds) { @@ -47,6 +47,8 @@ public void stop() { public abstract boolean atTargetSpeeds(); + public abstract ShooterSpeeds getFerrySpeeds(); + public abstract void feederIntake(); public abstract void feederDeacquire(); public abstract void feederShoot(); 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 e64ec59e..59854b2e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -132,7 +132,8 @@ public boolean noteShot() { return getLeftTargetRPM() > 0 && getRightTargetRPM() > 0 && hasNote.get() == false; } - private ShooterSpeeds getFerrySpeeds() { + @Override + public ShooterSpeeds getFerrySpeeds() { Translation2d ferryZone = Robot.isBlue() ? new Translation2d(0, Field.WIDTH - 1.5) : new Translation2d(0, 1.5); @@ -155,18 +156,31 @@ public void periodic () { Arm.State armState = Arm.getInstance().getActualState(); - if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { - setTargetSpeeds(Settings.Shooter.SPEAKER); + 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); + } } - else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { - setTargetSpeeds(getFerrySpeeds()); + + if (getLeftTargetRPM() == 0) { + leftMotor.set(0); } else { - setTargetSpeeds(Settings.Shooter.SPEAKER); + setLeftShooterRPM(getLeftTargetRPM()); } - setLeftShooterRPM(getLeftTargetRPM()); - setRightShooterRPM(getRightTargetRPM()); + if (getRightTargetRPM() == 0) { + rightMotor.set(0); + } + else { + setRightShooterRPM(getRightTargetRPM()); + } SmartDashboard.putNumber("Shooter/Feeder Speed", feederMotor.get());