From a7378ef38a40b14870f0006dd2b0d2145bce8f6c Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Wed, 16 Mar 2022 20:16:32 -0600
Subject: [PATCH] updated claws

---
 src/main/java/frc4388/robot/Constants.java    |  4 +--
 .../java/frc4388/robot/RobotContainer.java    | 30 +++++++++++--------
 src/main/java/frc4388/robot/RobotMap.java     |  2 +-
 .../java/frc4388/robot/subsystems/Claws.java  |  8 ++---
 4 files changed, 25 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 52c0845..aea43a0 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -200,9 +200,9 @@ public final class Constants {
     public static final int RIGHT_CLAW_ID = 45;
 
     public static final double OPEN_POSITION = 0;  // TODO: find actual position
-    public static final double CLOSE_POSITION = 0; // TODO: find actual position
+    public static final double CLOSE_POSITION = 1; // TODO: find actual position
 
-    public static final double THRESHOLD = 0; // TODO: find actual threshold
+    public static final double THRESHOLD = .1; // TODO: find actual threshold
 
     public static final double CALIBRATION_SPEED = 0;
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 13fa067..b94ae50 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -218,21 +218,27 @@ public class RobotContainer {
 
     /* Operator Buttons */
     // run claws
-    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-      .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2)))
-      .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
+    // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+    //   .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2)))
+    //   .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
     
-    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-      .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2)))
-      .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
+    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+    //   .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2)))
+    //   .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
+
+    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+    //   .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2)))
+    //   .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
+    
+    // new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+    //   .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2)))
+    //   .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+      .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(true)));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-      .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2)))
-      .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
-    
-    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
-      .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2)))
-      .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
+      .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(false)));
   }
 
     /*
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 63f0ac1..31f5296 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -175,7 +175,7 @@ public class RobotMap {
 //   public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
 //   public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless);
   public final Servo leftClaw = new Servo(1);  // TODO: find actual channel
-  public final Servo rightClaw = new Servo(1); // TODO: find actual channel
+  public final Servo rightClaw = new Servo(2); // TODO: find actual channel
 
   // Shooter Config
   /* Boom Boom Subsystem */
diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java
index c4f3f3f..3ea40a8 100644
--- a/src/main/java/frc4388/robot/subsystems/Claws.java
+++ b/src/main/java/frc4388/robot/subsystems/Claws.java
@@ -102,13 +102,13 @@ public class Claws extends SubsystemBase {
     if(open) {
       // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset);
       // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset);
-      m_leftClaw.set(0.1);
-      m_rightClaw.set(0.1);
+      m_leftClaw.set(-ClawConstants.OPEN_POSITION);
+      m_rightClaw.set(ClawConstants.OPEN_POSITION);
     } else {
       // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset);
       // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset);
-      m_leftClaw.set(-0.1);
-      m_rightClaw.set(-0.1);
+      m_leftClaw.set(-ClawConstants.CLOSE_POSITION);
+      m_rightClaw.set(ClawConstants.CLOSE_POSITION);
     }
   }