Slight changes to dislodger

Author: EvanWoodfin

src/main/java/frc/robot/Constants.java

@@ -263,7 +263,8 @@ public enum ElevatorHeight{
       //  THESE ARE ALL STILL ARBITRARY
         HOME(minHeightMeters),
         FUNNEL(minHeightMeters + Units.inchesToMeters(3.0)), //  Used to be 8 inches
-        DISLODGE(minHeightMeters + Units.inchesToMeters(3 + 13)),
+        DISLODGE_LOW(minHeightMeters),
+        DISLODGE_HIGH(minHeightMeters + Units.inchesToMeters(3 + 13)),
         L1(minHeightMeters + Units.inchesToMeters(2)),
         L2(minHeightMeters + Units.inchesToMeters(3)),
         L3(minHeightMeters + Units.inchesToMeters(3 + 16)),

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

@@ -139,6 +139,7 @@ private void competitionButtonBindings(){
 
         //  ELEVATOR & WRIST
 
+
         Trigger dPadDown = operatorController.povDown();
         dPadDown.onTrue(robotGoToHeightCommandCreator(ElevatorHeight.L1));
 
@@ -155,14 +156,14 @@ private void competitionButtonBindings(){
         Trigger rightBumper = operatorController.rightBumper();
         rightBumper.whileTrue(new OuttakeCoralCommand(manipulator));
 
-        Trigger dislodgeAlgaeTrigger = operatorController.rightTrigger();
+        Trigger dislodgeAlgaeTrigger = operatorController.leftBumper();
         dislodgeAlgaeTrigger.whileTrue(new DislodgeAlgaeCommand(manipulator, false));
 
         Trigger leftTrigger = operatorController.leftTrigger();
-        leftTrigger.onTrue(robotGoToHeightCommandCreator(ElevatorHeight.DISLODGE));
+        leftTrigger.onTrue(robotGoToHeightCommandCreator(ElevatorHeight.DISLODGE_HIGH));
 
-        Trigger yPressedTrigger = operatorController.y();
-        yPressedTrigger.onTrue(new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.DISLODGE_ANGLE));
+        Trigger yButton = operatorController.y();
+        yButton.onTrue(robotGoToHeightCommandCreator(ElevatorHeight.DISLODGE_LOW));
 
 
         //  FUNNEL INTAKING
@@ -266,7 +267,9 @@ private WristAngleRad getEndWristAngleForGivenElevatorHeight(ElevatorHeight heig
                 return WristAngleRad.L2L3;
             case FUNNEL:
                 return WristAngleRad.FUNNEL_ANGLE;
-            case DISLODGE:
+            case DISLODGE_LOW:
+                return WristAngleRad.DISLODGE_ANGLE;
+            case DISLODGE_HIGH:
                 return WristAngleRad.DISLODGE_ANGLE;
             case L1:
                 return WristAngleRad.L1;