[swerve] Play bad piggies while disabled

src/main/java/frc/robot/Robot.java

@@ -77,6 +77,9 @@ public Robot() {
     SignalLogger.start();
 
     m_drivetrain.registerTelemetry(logger::telemeterize);
+    m_drivetrain.addMusic("abba", "bad-piggies");
+    m_drivetrain.selectTrack("bad-piggies");
+    m_drivetrain.getOrchestra().play();
 
     CommandScheduler.getInstance().registerSubsystem(m_arm);
     CommandScheduler.getInstance().registerSubsystem(m_shooter);
@@ -104,13 +107,16 @@ public void robotPeriodic() {
   public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+    m_drivetrain.getOrchestra().play();
+  }
 
   @Override
   public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
+    m_drivetrain.getOrchestra().stop();
     m_autonomousCommand = getAutonomousCommand();
 
     if (m_autonomousCommand != null) {
@@ -126,6 +132,8 @@ public void autonomousExit() {}
 
   @Override
   public void teleopInit() {
+    m_drivetrain.getOrchestra().stop();
+
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -6,19 +6,24 @@
 
 import com.choreo.lib.Choreo;
 import com.choreo.lib.ChoreoControlFunction;
+import com.ctre.phoenix6.Orchestra;
 import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.AudioConfigs;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.function.Supplier;
 
 /**
@@ -35,6 +40,12 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
   private final PIDController m_rotationController = new PIDController(7, 0, 0.35);
   private final ChoreoControlFunction m_swerveController;
 
+  /* Orchestra classes */
+  private final Orchestra m_orchestra = new Orchestra();
+  private final ArrayList<String> m_songs = new ArrayList<String>();
+  private static final AudioConfigs m_audioConfig =
+      new AudioConfigs().withAllowMusicDurDisable(true);
+
   /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
   private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0);
   /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
@@ -96,6 +107,52 @@ private void startSimThread() {
     m_simNotifier.startPeriodic(kSimLoopPeriod);
   }
 
+  /**
+   * Add the given songs from a chirp file.
+   *
+   * @param songs The name of the chirp files for the songs to add. Doesn't include file extension.
+   */
+  public void addMusic(String... songs) {
+    m_songs.addAll(Arrays.asList(songs));
+  }
+
+  /**
+   * Selects the track to play.
+   *
+   * @param track The selected track. Must be a loaded song.
+   */
+  public void selectTrack(String track) {
+    if (!m_songs.contains(track)) {
+      DataLogManager.log("Track " + track + " not found.");
+    }
+
+    frc.robot.Utils.logStatusCode(m_orchestra.loadMusic(track + ".chrp"));
+
+    for (var i = 0; i < 4; i++) {
+      var module = super.getModule(i);
+      var driveMotor = module.getDriveMotor();
+      var steerMotor = module.getSteerMotor();
+
+      frc.robot.Utils.logStatusCode(driveMotor.getConfigurator().apply(m_audioConfig));
+      frc.robot.Utils.logStatusCode(m_orchestra.addInstrument(driveMotor, 0));
+      frc.robot.Utils.logStatusCode(steerMotor.getConfigurator().apply(m_audioConfig));
+      frc.robot.Utils.logStatusCode(m_orchestra.addInstrument(steerMotor, 1));
+    }
+  }
+
+  /**
+   * A list with the loaded songs.
+   *
+   * @return The list of loaded songs.
+   */
+  public ArrayList<String> getSongs() {
+    return m_songs;
+  }
+
+  public Orchestra getOrchestra() {
+    return m_orchestra;
+  }
+
   @Override
   public void periodic() {
     /*