music lmao

README.md

@@ -48,7 +48,7 @@ Powershell
 
 ### Linux
 ```bash
-chmod +x gradlew 
+chmod +x gradlew
 ./gradlew build
 ```
 > Alternatively you can run `chmod +x gradlew && gradlew build`
@@ -59,14 +59,14 @@ Note: `chmod +x gradlew` is giving it permission. Read more [here](https://en.wi
 > All these commands are in the WPILIB (Icon) button (in the top right corner) options if you have WPILIB vscode.
 
 ### Deploying
-Deploying will build your code, and deploy it to the robot. 
+Deploying will build your code, and deploy it to the robot.
 
 ```bash
 ./gradlew deploy
 ```
 > (You have to be connected to the robot for this to work.)
 
-### Clean 
+### Clean
 This removes all cache (gradle default uses cache to rebuild) when gradle builds. This is to ensure your dependences/vendordeps work.
 
 ```bash
@@ -80,7 +80,7 @@ This outputs what is happening while its building and a good way to see what its
 ./gradlew --debug
 ```
 
-### Scan 
+### Scan
 This is preferbly not used, it will scan the project and send all the data into a website where it will give a link to email to you.
 
 ```bash

src/main/deploy/pathplanner/autos/Move Backward + Apriltag.auto

@@ -34,4 +34,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}

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

@@ -3,6 +3,8 @@
 
 package frc.robot;
 
+import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -18,6 +20,11 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
+    /* Talon FXs to play music through.  
+    More complex music MIDIs will contain several tracks, requiring multiple instruments.  */
+
+
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -69,6 +76,7 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
+
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }

src/main/java/frc/robot/commands/swerve/BrakeSwerve.java

@@ -23,7 +23,7 @@ public class BrakeSwerve extends Command {
   /** Creates a new BrakeSwerve. */
   public BrakeSwerve(SwerveDriveSubsystem swerveDrive, double timeout) {
     _swerveDrive = swerveDrive;
-    
+
     if (timeout != 0) {
       _timeout = timeout;
     }

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

@@ -3,8 +3,11 @@
 
 package frc.robot.subsystems;
 
+import java.util.ArrayList;
 import java.util.Optional;
 
+import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
@@ -52,6 +55,9 @@ public class SwerveDriveSubsystem extends SubsystemBase {
 
   private double _robotSpeed = 0;
 
+  Orchestra _orchestra;
+  String song = "output.chrp"; 
+
   // estimated pose
   private Pose2d _pose = new Pose2d();
 
@@ -94,6 +100,22 @@ public ChassisSpeeds getRobotRelativeSpeeds() {
   public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) {
     _visionSubsystem = visionSubsystem;
 
+    SwerveModule[] modules = new SwerveModule[4];
+    modules[0] = _frontLeft;
+    modules[1] = _frontRight;
+    modules[2] = _backRight;
+    modules[3] = _backLeft;
+
+    for (int i = 0; i < modules.length; i++) {
+        for (int j = 0; j < 2; j++){
+            _orchestra.addInstrument(modules[i].returnTalons()[j]);
+        }
+    } 
+
+    _orchestra.loadMusic(song);
+
+    _orchestra.play();
+
     // pathplannerlib setup
     AutoBuilder.configureHolonomic(
       this::getPose,
@@ -146,8 +168,8 @@ public void periodic() {
     _field.setRobotPose(_pose);
     SmartDashboard.putData("FIELD", _field);
 
-    _robotSpeed = Math.sqrt(Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2)  + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2)); 
-    SmartDashboard.putNumber("DRIVE SPEED (m/s)", _robotSpeed); 
+    _robotSpeed = Math.sqrt(Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2)  + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2));
+    SmartDashboard.putNumber("DRIVE SPEED (m/s)", _robotSpeed);
   }
 
   /**
@@ -225,6 +247,6 @@ public Rotation2d getHeadingRaw() {
     return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360));
       }
 
-  
+
 
 }

src/main/java/frc/robot/utils/SwerveModule.java

@@ -29,6 +29,8 @@ public class SwerveModule {
 
     private final CANcoder _encoder;
 
+
+
     /**
      * Represents a single swerve module built with talons for rotation and drive control, and a cancoder for angle.
      * @param driveMotorId CAN ID of drive motor.
@@ -121,4 +123,11 @@ public void setState(SwerveModuleState state) {
     public SwerveModuleState getState() {
         return new SwerveModuleState(getDriveVelocity(), Rotation2d.fromDegrees(getAngle()));
     }
+
+    public TalonFX[] returnTalons(){
+        TalonFX[] fxes = new TalonFX[2];
+        fxes[0] = _driveMotor;
+        fxes[1] = _rotationMotor;
+        return fxes;
+    }
 }