elevator pid work, but crash

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

@@ -71,6 +71,7 @@ public class RobotContainer {
 
 		configureButtonBindings();
 		//elevatorSysIDBindings();
+		//elevatorOnlyBindings();
 
 		configureNamedCommands();
 
@@ -86,6 +87,13 @@ 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(
@@ -139,7 +147,7 @@ public class RobotContainer {
 			startingConfig()
 		);
 
-		driver.y().onTrue(drivetrain.zeroHeading());
+		driver.y().whileTrue(drivetrain.zeroHeading());
 		
 		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
 		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
@@ -177,19 +185,15 @@ public class RobotContainer {
 		);
 
 		operator.a().onTrue(
-			coralIntakeRoutine()
+			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
 		);
 
 		operator.x().onTrue(
-			algaeIntakeRoutine(true)
+			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
 		);
 
 		operator.b().onTrue(
-			algaeIntakeRoutine(false)
+			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
-		);
-
-		operator.y().whileTrue(
-			elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
 		);
 
 	}
@@ -398,8 +402,8 @@ public class RobotContainer {
 			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
 				 elevator.maintainPosition());*/
 		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
-		.andThen(elevator.goToSetpoint(() -> elevatorPosition))
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
-		.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
+		.raceWith(elevator.maintainPosition()));
 	}
 
 	@SuppressWarnings("unused")

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

@@ -43,14 +43,14 @@ public class ElevatorConstants {
     public static final double kL1Position = 0;
     public static final double kL2Position = 14.5;
     public static final double kL3Position = 29.0;
-    public static final double kL4Position = 52.0;
+    public static final double kL4Position = 49.0;
     public static final double kL4TransitionPosition = 40.0;
     public static final double kL2AlgaePosition = 22.0;
     public static final double kL3AlgaePosition = 39.0;
     public static final double kProcessorPosition = 4.0;
     /**The position of the top of the elevator brace */
     public static final double kBracePosition = 0;
-    public static final double kMaxHeight = 52.0; //actual is 52
+    public static final double kMaxHeight = 51.0; //actual is 51
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = .25;

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

@@ -34,7 +34,7 @@ public class ManipulatorPivotConstants {
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0.7815;
+    public static final double kEncoderOffset = 0.781;
 
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
@@ -46,7 +46,7 @@ public class ManipulatorPivotConstants {
     public static final double kProcesserPosition = Units.degreesToRadians(175.0);
     public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
     /**The forward rotation limit of the pivot */
     public static final double kRotationLimit = Units.degreesToRadians(175.0);
 

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

@@ -141,7 +141,7 @@ public class Elevator extends SubsystemBase {
     public Command maintainPosition() {
 
         return startRun(() -> {
-            
+            System.out.println("maintain position");
 
         }, 
         () -> {
@@ -184,10 +184,13 @@ public class Elevator extends SubsystemBase {
 
         return startRun(() -> {
         
-        pidControllerUp.setSetpoint(setpoint.getAsDouble());
-        pidControllerDown.setSetpoint(setpoint.getAsDouble());
         pidControllerUp.reset();
         pidControllerDown.reset();
+        pidControllerUp.setSetpoint(setpoint.getAsDouble());
+        pidControllerDown.setSetpoint(setpoint.getAsDouble());
+
+
+        System.out.println("go to setpoint");
         }, 
         () -> {
             double upOutput = pidControllerUp.calculate(getEncoderPosition());