diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7cbf07..e1879ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -148,7 +148,7 @@ public class RobotContainer { ); driver.leftTrigger().whileTrue( - manipulator.runUntilCollected(() -> 0.35).andThen(manipulator.retractCommand(() -> 0.35)) + manipulator.runUntilCollected(() -> 0.35) ); driver.start().and(driver.back()).onTrue( @@ -217,13 +217,15 @@ public class RobotContainer { operator.x().onTrue( safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) - ); + + ).and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false))); operator.b().onTrue( safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) - ); + ).and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false))); - operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)); + operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)) + .and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false))); } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 01ea613..aeb9173 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -41,8 +41,8 @@ public class ElevatorConstants { public static final double kCoralIntakePosition = 0; public static final double kL1Position = 0; - public static final double kL2Position = 8; - public static final double kL3Position = 25.0; + public static final double kL2Position = 9; + public static final double kL3Position = 23.0; public static final double kL4Position = 50.5; public static final double kL4TransitionPosition = 40.0; public static final double kL2AlgaePosition = 21.0; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index e4b9ea2..722e49c 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -23,7 +23,7 @@ public class ManipulatorPivotConstants { 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); + public static final double kPositionalTolerance = Units.degreesToRadians(1.5); public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19 public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41