Something to try to fix field oriented control with

src/main/deploy/pathplanner/autos/Field Oriented Test Auto.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Field Oriented Test Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/One Coral Left.auto

@@ -15,6 +15,12 @@
           "data": {
             "name": "Shoot Coral L4"
           }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J Backup"
+          }
         }
       ]
     }

src/main/deploy/pathplanner/paths/Field Oriented Test Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.495389344262295,
+        "y": 6.788165983606557
+      },
+      "prevControl": {
+        "x": 3.4164959016393444,
+        "y": 6.80015368852459
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/J Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.227054794520548
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.1321837955756875,
+        "y": 5.49468698033176
+      },
+      "isLocked": false,
+      "linkedName": "J"
+    },
+    {
+      "anchor": {
+        "x": 5.442044107776481,
+        "y": 6.005045141603656
+      },
+      "prevControl": {
+        "x": 5.268886874487802,
+        "y": 5.749866060967707
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -118.30075576600632
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -121.60750224624898
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Start to 30 Right.path

@@ -25,7 +25,7 @@
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "J"
     }
   ],
   "rotationTargets": [],

src/main/java/frc/robot/RobotContainer.java

@@ -97,28 +97,21 @@ public class RobotContainer {
 		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
 	}*/
 
-	private void elevatorOnlyBindings(){
-		elevator.setDefaultCommand(elevator.maintainPosition());
-		manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition());
-
-		driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position));
-	}
-
 	private void configureButtonBindings() {
 		//Default commands
 		climberPivot.setDefaultCommand(
-			climberPivot.runPivot(0)
+			climberPivot.runPivot(() -> 0)
 		);
 
 		climberRollers.setDefaultCommand(
-			climberRollers.runRoller(0)
+			climberRollers.runRoller(() -> 0)
 		);
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
 				() -> Math.pow(driver.getLeftY(), 3),
 				() -> Math.pow(driver.getLeftX(), 3),
-				driver::getRightX, //Math.pow(driver.getRightX(), 3)
+				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
 				() -> true
 			)
 		);
@@ -150,7 +143,7 @@ public class RobotContainer {
 		);
 
 		driver.leftTrigger().whileTrue( 
-			manipulator.runUntilCollected(() -> 0.75)
+			manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
 		);
 
 		driver.start().and(driver.back()).onTrue(
@@ -159,12 +152,7 @@ public class RobotContainer {
 
 		driver.y().whileTrue(drivetrain.zeroHeading());
 		
-		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
-		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
-
-		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
-		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
-		driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false));
+		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
 		
 		/* 
 		driver.rightBumper().whileTrue(
@@ -213,6 +201,8 @@ public class RobotContainer {
 			)
 		);
 
+		operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
+
 		operator.a().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
 		);
@@ -220,24 +210,26 @@ public class RobotContainer {
 		operator.x().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
 				.alongWith(manipulator.runManipulator(() -> 0.5, false))
-				.until(driver.rightTrigger())
+				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.b().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
 				.alongWith(manipulator.runManipulator(() -> 0.5, false))
-				.until(driver.rightTrigger())
+				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
 			.alongWith(manipulator.runManipulator(() -> 0.5, false))
-			.until(driver.rightTrigger())
+			.until(() -> driver.a().getAsBoolean())
 		);
 
 	}
 
 	private void configureNamedCommands() {
 		new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+
 		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
 		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
 		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -29,6 +29,8 @@ public class ElevatorConstants {
     public static final double kDownControllerP = 5.6;//7; //
     public static final double kDownControllerI = 0;
     public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
+
+    public static final double kMaintainP = 3;
      
     public static final double kAllowedError = 1;
 

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -2,6 +2,7 @@ package frc.robot.constants;
 
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.ctre.phoenix6.configs.AudioConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -57,6 +58,7 @@ public class ModuleConstants {
     public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
     public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
     public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
+    public static final AudioConfigs kAudioConfig = new AudioConfigs();
     public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
 
     static {
@@ -70,6 +72,8 @@ public class ModuleConstants {
         kDriveMotorConfig.Inverted = kDriveInversionState;
         kDriveMotorConfig.NeutralMode = kDriveIdleMode;
 
+        kAudioConfig.AllowMusicDurDisable = true;
+
         kDriveSlot0Config.kP = kDriveP;
         kDriveSlot0Config.kI = kDriveI;
         kDriveSlot0Config.kD = kDriveD;

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -30,9 +32,9 @@ public class ClimberPivot extends SubsystemBase {
         neoEncoder = pivotMotor.getEncoder();
     }
 
-    public Command runPivot(double speed) {
+    public Command runPivot(DoubleSupplier speed) {
         return run(() -> {
-            pivotMotor.set(speed);
+            pivotMotor.set(speed.getAsDouble());
         });
     }
 

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -32,9 +34,9 @@ public class ClimberRollers extends SubsystemBase {
      * @param speed The speed in which the roller runs
      * @return Runs the rollers at a set speed
      */
-    public Command runRoller(double speed) {
+    public Command runRoller(DoubleSupplier speed) {
         return run(() -> {
-            rollerMotor.set(speed);
+            rollerMotor.set(speed.getAsDouble());
         });
     }
 }

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

@@ -24,7 +24,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -47,8 +46,6 @@ public class Drivetrain extends SubsystemBase {
     // Odometry class for tracking robot pose
     private SwerveDrivePoseEstimator m_estimator;
 
-    private Vision vision;
-
     public Orchestra m_orchestra = new Orchestra();
     private Timer musicTimer = new Timer();
 
@@ -164,7 +161,7 @@ public class Drivetrain extends SubsystemBase {
         }
         */
         
-        if(musicTimer.get()>4){
+        if(musicTimer.get()>20){
             if (m_orchestra.isPlaying()) {
                 m_orchestra.stop();
             }
@@ -206,7 +203,14 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
-        m_estimator.resetPose(
+        m_estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()), 
+            new SwerveModulePosition[] {
+                m_frontLeft.getPosition(),
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+            }, 
             pose
         );
     }

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

@@ -27,6 +27,8 @@ public class Elevator extends SubsystemBase {
     private PIDController pidControllerUp;
     private PIDController pidControllerDown;
 
+    private PIDController maintainPID;
+
     private ElevatorFeedforward feedForward;
     
     public Elevator() {
@@ -76,7 +78,9 @@ public class Elevator extends SubsystemBase {
 
         pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
 
-        
+        maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
+
+        maintainPID.setTolerance(ElevatorConstants.kAllowedError);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -143,31 +147,28 @@ public class Elevator extends SubsystemBase {
     public Command maintainPosition() {
 
         return startRun(() -> {
-/* 
-            pidControllerUp.reset();
-            pidControllerDown.reset();
-            */
+            maintainPID.reset();
+            maintainPID.setSetpoint(pidControllerUp.getSetpoint());
         }, 
         () -> {
-/* 
-            double upOutput = pidControllerUp.calculate(getEncoderPosition());
-            double downOutput = pidControllerDown.calculate(getEncoderPosition());
 
-            if(pidControllerUp.getSetpoint()>encoder.getPosition()) 
+            double maintainOutput = maintainPID.calculate(getEncoderPosition());
+
+            if(!maintainPID.atSetpoint()) 
                 elevatorMotor1.setVoltage( MathUtil.clamp(
-                    upOutput + feedForward.calculate(0), -1, 1)
+                    maintainOutput + feedForward.calculate(0), -2, 2)
                 );
             else{
                 elevatorMotor1.setVoltage(
-                    MathUtil.clamp(
-                    downOutput + feedForward.calculate(0), -1, 1)
+                    feedForward.calculate(0)
                 );
             } 
-            */
-
+            
+/* 
             elevatorMotor1.setVoltage(
                     feedForward.calculate(0)
                 );
+                */
 
         });
         

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

@@ -56,6 +56,7 @@ public class MAXSwerveModule {
         m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
         m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
         m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+        m_drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
         m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
 
         m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,

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

@@ -74,9 +74,9 @@ public class Manipulator extends SubsystemBase {
         }).until(() -> !coralBeamBreak.get());
     }
 
-    public Command retractCommand(DoubleSupplier speed){
+    public Command retractCommand(DoubleSupplier retractSpeed){
         return run(() -> {
-            manipulatorMotor.set(-speed.getAsDouble());
+            manipulatorMotor.set(-retractSpeed.getAsDouble());
         }
         ).until(() -> coralBeamBreak.get());
     }