Algae dislodger code

Author: EvanWoodfin

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

@@ -263,7 +263,7 @@ 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)),
         L1(minHeightMeters + Units.inchesToMeters(2)),
         L2(minHeightMeters + Units.inchesToMeters(3)),
         L3(minHeightMeters + Units.inchesToMeters(3 + 16)),
@@ -394,6 +394,7 @@ public static final class WristConstants{
       public static enum WristAngleRad {
         FREEHANG(Units.degreesToRadians(90)),
         FUNNEL_ANGLE(Units.degreesToRadians(66)),
+        DISLODGE_ANGLE(Units.degreesToRadians(215)),
         L1(Units.degreesToRadians(252)),  //  Needs to be changed
         L2L3(Units.degreesToRadians(252)),
         L4(Units.degreesToRadians(200));

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

@@ -21,7 +21,6 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -153,13 +152,18 @@ private void competitionButtonBindings(){
         dPadUp.onTrue(robotGoToHeightCommandCreator(ElevatorHeight.L4));
 
 
-        //  OUTTAKING
-        // Trigger leftBumper = operatorController.leftBumper();
-        // leftBumper.whileTrue(new IntakeCoralCommand(manipulator, passthrough));
-
         Trigger rightBumper = operatorController.rightBumper();
         rightBumper.whileTrue(new OuttakeCoralCommand(manipulator));
 
+        Trigger dislodgeAlgaeTrigger = operatorController.rightTrigger();
+        dislodgeAlgaeTrigger.whileTrue(new DislodgeAlgaeCommand(manipulator, false));
+
+        Trigger leftTrigger = operatorController.leftTrigger();
+        leftTrigger.onTrue(robotGoToHeightCommandCreator(ElevatorHeight.DISLODGE));
+
+        Trigger yPressedTrigger = operatorController.y();
+        yPressedTrigger.onTrue(new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.DISLODGE_ANGLE));
+
 
         //  FUNNEL INTAKING
 
@@ -262,6 +266,8 @@ private WristAngleRad getEndWristAngleForGivenElevatorHeight(ElevatorHeight heig
                 return WristAngleRad.L2L3;
             case FUNNEL:
                 return WristAngleRad.FUNNEL_ANGLE;
+            case DISLODGE:
+                return WristAngleRad.DISLODGE_ANGLE;
             case L1:
                 return WristAngleRad.L1;
             case L2:
@@ -402,8 +408,7 @@ private void configureManipulatorBindings(){
         //     new WaitCommand(0.04),
         //     Commands.runOnce(() -> manipulator.stopOuttake(), manipulator)));
 
-        // Trigger dislodgeAlgaeTrigger = operatorController.rightTrigger();
-        // dislodgeAlgaeTrigger.whileTrue(new DislodgeAlgaeCommand(manipulator, false));
+
 
     }
 
@@ -429,9 +434,9 @@ private void configureElevatorBindings(){
             new ElevatorGoToPositionPositionCommand(elevator, ElevatorHeight.L3)
         );
 
-        Trigger dPadUp = operatorController.povUp();
-        dPadUp.whileTrue(
-            new ElevatorGoToPositionPositionCommand(elevator, ElevatorHeight.L4)
-        );
+        // Trigger dPadUp = operatorController.povUp();
+        // dPadUp.whileTrue(
+        //     new ElevatorGoToPositionPositionCommand(elevator, ElevatorHeight.L4)
+        // );
     }
 }

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

@@ -91,6 +91,7 @@ public void startDislodger(boolean invert){
   }
   public void stopDislodger(){
     currentDislodgerGoal = new State(0, 0);
+    manipulatorIO.setDislodgerVoltage(0);
   }