diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java index 5a351b01..9c9d3afc 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java @@ -10,6 +10,7 @@ import org.trigon.hardware.phoenix6.talonfx.io.RealTalonFXIO; import org.trigon.hardware.phoenix6.talonfx.io.SimulationTalonFXIO; import org.trigon.hardware.simulation.MotorPhysicsSimulation; +import org.trigon.utilities.Orchestra; /** * A class that represents a TalonFX motor controller. @@ -46,6 +47,9 @@ public TalonFXMotor(int id, String motorName, String canbus) { this.motorInputs = new Phoenix6Inputs(motorName, !canbus.isEmpty()); this.id = id; motorIO.optimizeBusUsage(); + + if (!RobotHardwareStats.isReplay()) + Orchestra.addMotor(motorIO.getTalonFX(), id); } /** diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/io/RealTalonFXIO.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/io/RealTalonFXIO.java index b465e437..385f76d2 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/io/RealTalonFXIO.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/io/RealTalonFXIO.java @@ -25,6 +25,10 @@ protected void setPosition(double positionRotations) { @Override public void applyConfiguration(TalonFXConfiguration configuration) { + configuration.Audio.BeepOnBoot = false; + configuration.Audio.BeepOnConfig = false; + configuration.Audio.AllowMusicDurDisable = true; + talonFX.getConfigurator().apply(configuration); } diff --git a/src/main/java/org/trigon/utilities/Orchestra.java b/src/main/java/org/trigon/utilities/Orchestra.java new file mode 100644 index 00000000..cb29dca4 --- /dev/null +++ b/src/main/java/org/trigon/utilities/Orchestra.java @@ -0,0 +1,234 @@ +package org.trigon.utilities; + +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; + +/** + * A class that uses the {@link com.ctre.phoenix6.Orchestra} library in Phoenix 6 to play a .chrp file. + */ +public class Orchestra extends SubsystemBase { + private static final Orchestra INSTANCE = new Orchestra(); + private static final com.ctre.phoenix6.Orchestra ORCHESTRA = new com.ctre.phoenix6.Orchestra(); + private static final HashMap MOTORS = new HashMap<>(); + private static ArrayList USED_MOTOR_IDS = new ArrayList<>(); + private static boolean SHOULD_UPDATE_USED_MOTORS = false; + + /** + * Adds a motor to the Orchestra. + * The Orchestra can then play the music from those motors. + * + * @param motor the motor to be added to the Orchestra + */ + public static void addMotor(TalonFX motor, int id) { + MOTORS.put(id, motor); + } + + public static Command getPlayFileCommand(String filePath, int totalTracks) { + return new FunctionalCommand( + () -> playFile(filePath, totalTracks), + () -> updateMotors(totalTracks), + (interrupted) -> stop(), + () -> false, + INSTANCE + ).ignoringDisable(true); + } + + public static Command getPlayFileCommand(String filePath, int[] motorsPerTrack) { + return new FunctionalCommand( + () -> playFile(filePath, motorsPerTrack), + () -> updateMotors(motorsPerTrack), + (interrupted) -> stop(), + () -> false, + INSTANCE + ).ignoringDisable(true); + } + + public static Command getStopCommand() { + return new InstantCommand( + Orchestra::stop, + INSTANCE + ).ignoringDisable(true); + } + + public static Command getPauseCommand() { + return new InstantCommand( + Orchestra::pause, + INSTANCE + ).ignoringDisable(true); + } + + public static Command getPlayCommand() { + return new InstantCommand( + Orchestra::play, + INSTANCE + ).ignoringDisable(true); + } + + /** + * @return whether orchestra is currently playing a file + */ + public static boolean isOrchestraCurrentlyPlaying() { + return ORCHESTRA.isPlaying(); + } + + /** + * @return the play time of the current file in seconds + */ + public static double getPlayTimeSeconds() { + return ORCHESTRA.getCurrentTime(); + } + + /** + * Adds IDs of motors that should be used when playing a file. + * + * @param newIDs the IDs of the motors that should be used + */ + public static void addUsedMotorIDs(Integer... newIDs) { + if (USED_MOTOR_IDS.containsAll(Arrays.asList(newIDs))) + return; + + USED_MOTOR_IDS.addAll(Arrays.asList(newIDs)); + setUsedMotorIDs(USED_MOTOR_IDS); + } + + /** + * Removes IDs of motors that should no longer be used when playing a file. + * + * @param idsToRemove the IDs of motors that should no longer be used + */ + public static void removeUsedMotorIDs(Integer... idsToRemove) { + if (!USED_MOTOR_IDS.containsAll(Arrays.asList(idsToRemove))) + return; + + USED_MOTOR_IDS.removeAll(Arrays.asList(idsToRemove)); + setUsedMotorIDs(USED_MOTOR_IDS); + } + + /** + * Clears all used motor IDs. + */ + public static void clearUsedMotorIDs() { + setUsedMotorIDs(new ArrayList<>()); + } + + /** + * Sets the used motor IDs to a specific list. + * + * @param usedMotorIDs the list of motor IDs to be used + */ + public static void setUsedMotorIDs(ArrayList usedMotorIDs) { + USED_MOTOR_IDS = usedMotorIDs; + SHOULD_UPDATE_USED_MOTORS = true; + } + + /** + * Plays a .chrp file and assigns a track to each motor. + * A .chrp file stores music as tracks that can be played separately. + * The tracks are assigned linearly and loop back if there are extra motors. + * + * @param filePath the path of the .chrp file to be played by the Orchestra + * @param totalTracks the number of tracks in the .chrp file + */ + private static void playFile(String filePath, int totalTracks) { + applyTracks(totalTracks); + addAndPlayFile(filePath); + } + + /** + * plays a .chrp file and assigns a track to each motor. + * A .chrp file stores music as tracks that can be played separately. + * The tracks are assigned by the {@code motorsPerTrack}. + * Each slot represents a track. + * Each value in the slot represents the number of motors that will be assigned that track. + * + * @param filePath the path of the .chrp file to be added to the Orchestra + * @param motorsPerTrack number of motors that should be assigned to each track + */ + private static void playFile(String filePath, int[] motorsPerTrack) { + applyTracks(motorsPerTrack); + addAndPlayFile(filePath); + } + + private static void updateMotors(int totalTracks) { + if (!SHOULD_UPDATE_USED_MOTORS) + return; + ORCHESTRA.clearInstruments(); + + applyTracks(totalTracks); + SHOULD_UPDATE_USED_MOTORS = false; + } + + private static void updateMotors(int[] motorsPerTrack) { + if (!SHOULD_UPDATE_USED_MOTORS) + return; + ORCHESTRA.clearInstruments(); + + applyTracks(motorsPerTrack); + SHOULD_UPDATE_USED_MOTORS = false; + } + + private static void applyTracks(int totalTracks) { + for (int i = 1; i < MOTORS.size() + 1; i++) + if (shouldUseMotor(i) && MOTORS.containsKey(i)) + ORCHESTRA.addInstrument(MOTORS.get(i), i % totalTracks); + } + + private static void applyTracks(int[] motorsPerTrack) { + int motorIndex = 1; + + for (int trackIndex = 0; trackIndex < motorsPerTrack.length; trackIndex++) { + for (int motorsInCurrentTrack = 0; motorsInCurrentTrack < motorsPerTrack[trackIndex]; motorsInCurrentTrack++) { + if (motorIndex > MOTORS.size()) { + System.out.println("Orchestra: Not enough motors"); + return; + } + if (shouldUseMotor(motorIndex) && MOTORS.containsKey(motorIndex)) + ORCHESTRA.addInstrument(MOTORS.get(motorIndex), trackIndex); + motorIndex++; + } + } + } + + /** + * Stops the music and removes all tracks assigned to motors. + */ + private static void stop() { + ORCHESTRA.stop(); + ORCHESTRA.clearInstruments(); + } + + /** + * Pauses the music. + */ + private static void pause() { + ORCHESTRA.pause(); + } + + /** + * Plays the stored .chrp file. + */ + private static void play() { + ORCHESTRA.play(); + } + + /** + * Adds a .chrp file to the Orchestra and plays it. + * + * @param filePath the path of the .chrp file to be added to the Orchestra + */ + private static void addAndPlayFile(String filePath) { + ORCHESTRA.loadMusic(filePath); + play(); + } + + private static boolean shouldUseMotor(Integer id) { + return USED_MOTOR_IDS.contains(id); + } +}