From 08d57fccff2966603bb7358b8b1316201315a66b Mon Sep 17 00:00:00 2001
From: Scott Barnes <scott@tearabite.com>
Date: Sat, 4 Nov 2023 12:39:16 -0500
Subject: [PATCH] Buttons assigned

---
 .../src/main/java/opmodes/MainTeleOp.java     | 37 +++++++++++--------
 .../ftc/teamcode/hardware/Gantry.java         |  8 ++--
 .../ftc/teamcode/hardware/RobotConstants.java |  1 +
 3 files changed, 27 insertions(+), 19 deletions(-)

diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java
index c676622..66c0654 100644
--- a/TeamCode/src/main/java/opmodes/MainTeleOp.java
+++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java
@@ -2,6 +2,7 @@ package opmodes;
 
 import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_DELTA;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -17,7 +18,6 @@ public class MainTeleOp extends OpMode {
     private boolean screwArmPos = false;
     private boolean previousScrewArmToggle = false;
     private boolean previousScrewDeposit = false;
-    private boolean previousScrewIntake = false;
     private boolean previousScrewReset = false;
     private boolean previousSlideUp = false;
     private boolean previousSlideDown = false;
@@ -38,11 +38,23 @@ public class MainTeleOp extends OpMode {
         double z = gamepad1.right_stick_x;
         this.robot.getDrive().setInput(x, y, z);
 
+        // Button Mappings
+        boolean openClaw = gamepad2.b; // B
+        boolean clawUp = gamepad2.y; // Y
+        boolean clawDown = gamepad2.a; // A
+
+        boolean raiseRobotLift = gamepad2.right_trigger > 0.25; // RT
+        boolean liftRobot = gamepad2.right_bumper; // RB
+
+        boolean screwArmToggle = gamepad2.x; // X
+        boolean screwDeposit = gamepad2.left_trigger > 0.25; // LT
+        boolean screwReset = gamepad2.left_bumper; // LB
+        boolean slideUp = gamepad2.left_stick_y < -0.25; // Left Y Axis Joystick
+        boolean slideDown = gamepad2.left_stick_y > 0.25; // Left Y Axis Joystick
+        boolean gantryLeft = gamepad2.dpad_left; // dpad left
+        boolean gantryRight = gamepad2.dpad_right; // dpad right
 
         // Claw
-        boolean openClaw = false;
-        boolean clawUp = false;
-        boolean clawDown = false;
         if (openClaw) {
             this.robot.getClaw().open();
         } else {
@@ -57,8 +69,6 @@ public class MainTeleOp extends OpMode {
         }
 
         // Robot Lift
-        boolean raiseRobotLift = false;
-        boolean liftRobot = false;
         if (raiseRobotLift) {
             this.robot.getLift().raise();
         } else if (liftRobot) {
@@ -66,12 +76,6 @@ public class MainTeleOp extends OpMode {
         }
 
         // Gantry
-        boolean screwArmToggle = false;
-        boolean screwDeposit = false;
-        boolean screwIntake = false;
-        boolean screwReset = false;
-        boolean slideUp = false;
-        boolean slideDown = false;
         if (!previousScrewArmToggle && screwArmToggle) {
             if (screwArmPos) {
                 this.robot.getGantry().armOut();
@@ -82,8 +86,6 @@ public class MainTeleOp extends OpMode {
         }
         if (!previousScrewDeposit && screwDeposit) {
             this.robot.getGantry().deposit();
-        } else if (!previousScrewIntake && screwIntake) {
-            this.robot.getGantry().intake();
         } else if (!previousScrewReset && screwReset) {
             this.robot.getGantry().resetScrew();
         }
@@ -91,11 +93,16 @@ public class MainTeleOp extends OpMode {
             int currentPosition  = this.robot.getGantry().getSlidePosition();
             this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA);
         }
+        if (gantryLeft) {
+            this.robot.getGantry().setX(this.robot.getGantry().getX() + GANTRY_X_DELTA);
+        } else if (gantryRight) {
+            this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA);
+        }
+
 
         this.previousSlideUp = slideUp;
         this.previousScrewArmToggle = screwArmToggle;
         this.previousScrewDeposit = screwDeposit;
-        this.previousScrewIntake = screwIntake;
         this.previousScrewReset = screwReset;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
index f94d4df..0192f3f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
@@ -28,6 +28,10 @@ public class Gantry extends Slide {
         this.xServo.setPosition(x);
     }
 
+    public double getX() {
+        return this.xServo.getPosition();
+    }
+
     public void armOut() {
         this.armServo.setPosition(GANTRY_ARM_MAX);
     }
@@ -41,10 +45,6 @@ public class Gantry extends Slide {
         this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[currentScrewIndex]);
     }
 
-    public void intake() {
-        this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex++]);
-    }
-
     public void deposit() {
         this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex--]);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java
index 0a6b3dd..2855961 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java
@@ -29,6 +29,7 @@ public class RobotConstants {
     public static double GANTRY_ARM_MAX = 0;
     public static double[] GANTRY_SCREW_POSITIONS = new double[] { 0, 0.1, 0.2, 1.0 };
     public static int GANTRY_LIFT_DELTA = 50;
+    public static double GANTRY_X_DELTA = 0.01;
     public static double GANTRY_CENTER = 0.5;
 
     // Robot Lift