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..b41d3163 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; @@ -19,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; @@ -35,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; @@ -96,6 +103,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() @@ -111,26 +119,37 @@ 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 ConditionalCommand( - new ArmToSpeakerHigh(), - new ArmToSpeakerLow(), - () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW)); - - driver.getLeftButton().onTrue(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() - .onTrue(new ConditionalCommand( - new ArmToLobFerry(), - new ArmToLowFerry(), - () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY)); + .whileTrue(new ArmToFerry()); + + driver.getRightButton() + .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) + .onTrue(new ArmEnableOverride()) + .onFalse(new ArmDisableOverride()); driver.getBottomButton().onTrue(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/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 33700542..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,7 +1,6 @@ package com.stuypulse.robot.commands.arm; import com.stuypulse.robot.subsystems.arm.Arm; - import edu.wpi.first.wpilibj2.command.InstantCommand; public class ArmSetState extends InstantCommand{ @@ -17,6 +16,6 @@ public ArmSetState(Arm.State state) { @Override public void initialize() { - arm.setState(state); + arm.setRequestedState(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 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/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/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/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..88658606 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -114,8 +114,10 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 0.25; + boolean ALWAYS_KEEP_AT_SPEED = false; + double TARGET_RPM_THRESHOLD = 250; - double MAX_WAIT_TO_REACH_TARGET = 1.5; + double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0; ShooterSpeeds SPEAKER = new ShooterSpeeds( new SmartNumber("Shooter/Speaker RPM", 4875), @@ -325,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/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..5d342af6 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 { @@ -75,8 +76,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 +164,30 @@ 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()) { + 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()); @@ -179,7 +198,7 @@ public void periodic() { 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/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 be2bdac6..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,14 +132,15 @@ 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); 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,20 +154,33 @@ private ShooterSpeeds getFerrySpeeds() { public void periodic () { super.periodic(); - Arm.State armState = Arm.getInstance().getState(); - - if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { - setTargetSpeeds(Settings.Shooter.SPEAKER); + Arm.State armState = Arm.getInstance().getActualState(); + + 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()); 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 {