diff --git a/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java b/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java index dfdc68d9..329965bd 100644 --- a/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java +++ b/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java @@ -6,6 +6,8 @@ import java.util.function.DoubleSupplier; +import com.ctre.phoenix6.Orchestra; +import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; @@ -53,7 +55,6 @@ public class AmpArmIOTalonFX implements AmpArmIO { private final double wristKI = 0.0; private final double wristKD = 0.0; private final double wristKF = 0.0; - private DigitalInput zeroButton; private DigitalInput brakeButton; private DigitalInput sensor = new DigitalInput(Constants.AMP_INTAKE_SENSOR_PORT); @@ -61,6 +62,7 @@ public class AmpArmIOTalonFX implements AmpArmIO { private boolean zeroPrev = false; private boolean brakePrev = false; + private Orchestra updateSound; private NeutralModeValue neutralMode = NeutralModeValue.Brake; /** Creates a new AmpArmIOTalonFX. */ @@ -70,12 +72,21 @@ public AmpArmIOTalonFX(DigitalInput zeroButton, DigitalInput brakeButton) { armMotor.getConfigurator().apply(new TalonFXConfiguration()); wristMotor.getConfigurator().apply(new TalonFXConfiguration()); + + updateSound = new Orchestra(); + updateSound.addInstrument(armMotor); + updateSound.addInstrument(wristMotor); + StatusCode status = updateSound.loadMusic("TetrisTheme.chrp"); + + if(status != StatusCode.OK){ + System.out.println("Error loading sound"); + } TalonFXConfiguration armConfig = new TalonFXConfiguration(); armConfig.SoftwareLimitSwitch .withForwardSoftLimitThreshold(ARM_FORWARD_LIMIT) - .withReverseSoftLimitThreshold(ARM_REVERSE_LIMIT) + .withReverseSoftLimitThreshold(ARM_REVERSE_LIMIT) .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true); @@ -131,10 +142,11 @@ public void disableBrakeMode() { @Override public void resetArmWristPos() { - armMotor.setPosition(-78.0); wristMotor.setPosition(70.0); - + + updateSound.stop(); + updateSound.play(); } @Override @@ -152,6 +164,10 @@ public void setArm(double angle) { public void setWrist(double angle) { PositionVoltage positionVoltage = new PositionVoltage(angle); wristMotor.setControl(positionVoltage); + if(angle == 0.0){ + updateSound.stop(); + updateSound.play(); + } } @Override @@ -219,4 +235,9 @@ public boolean getBrakeButton(){ public boolean isBrakeMode(){ return neutralMode == NeutralModeValue.Brake; } + public void stopSound(){ + if(updateSound.isPlaying()){ + updateSound.stop(); + } + } } diff --git a/src/main/java/frc/robot/io/AmpArmIO.java b/src/main/java/frc/robot/io/AmpArmIO.java index 5c740f70..149123ee 100644 --- a/src/main/java/frc/robot/io/AmpArmIO.java +++ b/src/main/java/frc/robot/io/AmpArmIO.java @@ -63,4 +63,6 @@ public default void updateInputs(AmpArmIOInputs inputs) {} public boolean isBrakeMode(); public boolean getZeroButton(); + + public void stopSound(); }