elevator and manipulator work invidiually, not together

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

@@ -103,18 +103,14 @@ public class RobotContainer {
 				() -> true
 			)
 		);
-		/*
+		
 		elevator.setDefaultCommand(
-			elevator.goToSetpoint(
+			elevator.maintainPosition()
-				() -> 0
-			)
 		);
-		*/
+		
 		
 		manipulatorPivot.setDefaultCommand(
-			manipulatorPivot.runManualPivot(
+			manipulatorPivot.maintainPosition()
-				() -> operator.getRightY() * 0.5
-			)
 		);
 		
 
@@ -125,7 +121,7 @@ public class RobotContainer {
 		);
 
 		//Driver inputs
-		/*
+		
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);
@@ -147,46 +143,31 @@ public class RobotContainer {
 
 		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
-		*/
-		
-		operator.povUp().whileTrue(
-			manipulatorPivot.goToSetpoint(() -> 0)
-		);
-
-		operator.povDown().whileTrue(
-			manipulatorPivot.goToSetpoint(() -> (Units.degreesToRadians(110))
-		));
-		
-		/*
-		operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
-
-		operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
-
 		
 		//Operator inputs
 		operator.povUp().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL4Position, 
 				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL3Position, 
 				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL2Position, 
 				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL1Position, 
 				ManipulatorPivotConstants.kL1Position
 			)
@@ -203,7 +184,11 @@ public class RobotContainer {
 		operator.b().onTrue(
 			algaeIntakeRoutine(false)
 		);
-		*/
+
+		operator.y().whileTrue(
+			elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
+		);
+	
 
 	}
 

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

@@ -49,7 +49,7 @@ public class ElevatorConstants {
     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 = 5.5;
+    public static final double kBracePosition = 0;
     public static final double kMaxHeight = 47.5; //actual is 53
 
     // 1, 7, 10 are the defaults for these, change as necessary

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

@@ -20,7 +20,7 @@ public class ManipulatorPivotConstants {
 
     public static final double kPivotMaxVelocity = 2 * Math.PI;
 
-    public static final double kPositionalP = 2.5;
+    public static final double kPositionalP = 4;
     public static final double kPositionalI = 0;
     public static final double kPositionalD = 0;
     public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
@@ -38,7 +38,7 @@ public class ManipulatorPivotConstants {
 
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
-    public static final double kL2Position = Units.degreesToRadians(25.0);
+    public static final double kL2Position = Units.degreesToRadians(60.0);
     public static final double kL3Position = Units.degreesToRadians(60.0);
     public static final double kL4Position = Units.degreesToRadians(45.0);
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.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(60.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.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

@@ -62,6 +62,7 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kDownControllerI,
             ElevatorConstants.kDownControllerD
         );
+        pidController.setSetpoint(0);
 
         pidController.setTolerance(ElevatorConstants.kAllowedError);
 
@@ -128,9 +129,56 @@ public class Elevator extends SubsystemBase {
      * @return Sets motor voltage based on feed forward calculation.
      */
     public Command maintainPosition() {
-        return run(() -> {
+
-            elevatorMotor1.setVoltage(feedForward.calculate(0));
+        return startRun(() -> {
+            
+        //pidController.reset();
+       // pidController.setSetpoint(encoder.getPosition());
+        }, 
+        () -> {
+            /*if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition()
+                    ) + feedForward.calculate(0)
+                );
+            } else {
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            }*/
+
+            elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
         });
+        
+            
+            
+            
+            /*
+            elevatorMotor1.setVoltage(
+                pidController.calculate(
+                    encoder.getPosition(),
+                    clampedSetpoint
+                ) + feedForward.calculate(0)
+            );
+*/
+            /* 
+            if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(0)
+                );
+            } else {
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            }
+             
+        });*/
     }
 
     /**
@@ -143,7 +191,6 @@ public class Elevator extends SubsystemBase {
     public Command goToSetpoint(DoubleSupplier setpoint) {
 
         return startRun(() -> {
-            
         
         pidController.setSetpoint(setpoint.getAsDouble());
         pidController.reset();
@@ -161,7 +208,7 @@ public class Elevator extends SubsystemBase {
                     feedForward.calculate(0)
                 );
             }
-        });
+        }).until(() -> pidController.atSetpoint());
         
             
             

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

@@ -45,6 +45,7 @@ public class ManipulatorPivot extends SubsystemBase {
             ManipulatorPivotConstants.kPositionalI, 
             ManipulatorPivotConstants.kPositionalD
         );
+        pidController.setSetpoint(0);
 
         pidController.disableContinuousInput();
         
@@ -98,11 +99,11 @@ public class ManipulatorPivot extends SubsystemBase {
     public Command goToSetpoint(DoubleSupplier setpoint) {
         return startRun(() -> {
             
-        
             pidController.setSetpoint(setpoint.getAsDouble());
             pidController.reset();
             }, 
             () -> {
+                /* 
                 if (!pidController.atSetpoint()) {
                     pivotMotor.setVoltage(
                         pidController.calculate(
@@ -115,6 +116,43 @@ public class ManipulatorPivot extends SubsystemBase {
                         -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
                     );
                 }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> pidController.atSetpoint());
+    }
+
+    public Command maintainPosition() {
+        return startRun(() -> {
+            
+        
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        pidController.getSetpoint()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
             });
     }