added cameraserver and dislodge wrist pos

Author: Ihlathi

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

@@ -389,7 +389,7 @@ public static final class WristConstants{
       public static enum WristAngleRad {
         FREEHANG(Units.degreesToRadians(90)),
         FUNNEL_ANGLE(Units.degreesToRadians(80)),
-        DISLODGE_ANGLE(Units.degreesToRadians(215)),
+        DISLODGE_ANGLE(Units.degreesToRadians(245)),
         L1(Units.degreesToRadians(252)),  //  Needs to be changed
         L2L3(Units.degreesToRadians(252)),
         L4(Units.degreesToRadians(210));

src/main/java/frc/robot/Robot.java

@@ -13,6 +13,7 @@
 
 package frc.robot;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.util.datalog.StringLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -91,6 +92,9 @@ public Robot() {
     // Start AdvantageKit logger
     Logger.start();
 
+    // Start cameraserver for USB camera
+    CameraServer.startAutomaticCapture();
+
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();

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

@@ -156,6 +156,10 @@ private void competitionButtonBindings(){
         Trigger leftBumper = operatorController.leftBumper();
         leftBumper.whileTrue(new ManipulatorIntakeCoralCommand(manipulator)); // the lower one apparently doesn't do it
 
+        Trigger yButton = operatorController.y();
+        yButton.onTrue(new WristGoToPositionCommand(wrist, WristAngleRad.DISLODGE_ANGLE)); //dislodge algae position
+        yButton.onFalse(new WristGoToPositionCommand(wrist, WristAngleRad.L1));
+
         Trigger leftTrigger = operatorController.leftTrigger();
         leftTrigger.onTrue(robotGoToHeightCommandCreator(ElevatorHeight.DISLODGE_HIGH));