pid maintain position for elevator

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/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

@@ -228,6 +228,8 @@ public class RobotContainer {
 
 	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/subsystems/Drivetrain.java

@@ -161,7 +161,7 @@ public class Drivetrain extends SubsystemBase {
         }
         */
         
-        if(musicTimer.get()>4){
+        if(musicTimer.get()>20){
             if (m_orchestra.isPlaying()) {
                 m_orchestra.stop();
             }

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,7 @@ public class Elevator extends SubsystemBase {
 
         pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
 
-        
+        maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -143,27 +145,23 @@ public class Elevator extends SubsystemBase {
     public Command maintainPosition() {
 
         return startRun(() -> {
-/* 
+            
-            pidControllerUp.reset();
-            pidControllerDown.reset();
-            */
         }, 
         () -> {
-/* 
+
-            double upOutput = pidControllerUp.calculate(getEncoderPosition());
+            double maintainOutput = maintainPID.calculate(getEncoderPosition());
-            double downOutput = pidControllerDown.calculate(getEncoderPosition());
 
             if(pidControllerUp.getSetpoint()>encoder.getPosition()) 
                 elevatorMotor1.setVoltage( MathUtil.clamp(
-                    upOutput + feedForward.calculate(0), -1, 1)
+                    maintainOutput + feedForward.calculate(0), -1, 1)
                 );
             else{
                 elevatorMotor1.setVoltage(
                     MathUtil.clamp(
-                    downOutput + feedForward.calculate(0), -1, 1)
+                    maintainOutput + feedForward.calculate(0), -1, 1)
                 );
             } 
-            */
+            
 
             elevatorMotor1.setVoltage(
                     feedForward.calculate(0)