From 2c1f0d6c57daf8cfcba7702e5f8926a86f17a7ed Mon Sep 17 00:00:00 2001
From: carlos <carlos@riv.as>
Date: Tue, 5 Nov 2024 17:20:43 -0800
Subject: [PATCH] Commit "working" code?

---
 .../firstinspires/ftc/teamcode/DevTeleop.java   | 10 ++++++++++
 .../ftc/teamcode/configs/RobotConstants.java    |  4 ++--
 .../ftc/teamcode/subsystem/LiftSubsystem.java   | 17 +++++++++--------
 3 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
index 304ca6c..8288f4e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
@@ -113,6 +113,14 @@ public class DevTeleop extends OpMode {
             wrist.bucketWrist();
         }
     }
+
+    public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down){
+            lift.toFloor();
+            arm.bucketArm();
+            wrist.floorWrist();
+        }
+    }
     @Override
     public void loop() {
         previousGamepad1.copy(currentGamepad1);
@@ -126,6 +134,7 @@ public class DevTeleop extends OpMode {
         theLift(arm, wrist);
         theLowBucketScore(lift, wrist, arm);
         theHighBucketScore(lift, wrist, arm);
+        theTravel(lift, arm, wrist);
 
         double max;
 
@@ -164,6 +173,7 @@ public class DevTeleop extends OpMode {
         // Show the elapsed game time and wheel power.
         telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
         telemetry.addData("Back  left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+        telemetry.addData("Current Lift Position", lift.getPosition());
         telemetry.update();
 
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
index 755de44..d56cf6b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
@@ -13,8 +13,8 @@ public class RobotConstants {
 
     public static double wristFloor = 0.625;
     public static double wristBucket = 0.215;
-    public static int liftToFloorPos = 0;
-    public static int liftToPoolPos = 500;
+    public static int liftToFloorPos = 20;
+    public static int liftToFloatPos = 150;
     public static int liftToLowBucketPos = 2250;
     public static int liftToHighBucketPos = 3850;
     public static double liftPower = .45;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java
index ac895b5..ed54f00 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java
@@ -1,9 +1,10 @@
 package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
-import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToPoolPos;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -17,7 +18,7 @@ public class LiftSubsystem {
     public RunAction toFloor, toLowBucket, toHighBucket;
 
     public enum LiftState {
-        FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL
+        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT
     }
 
     private LiftState liftState;
@@ -36,8 +37,8 @@ public class LiftSubsystem {
 
     public void switchState() {
         if (this.liftState == LiftState.FLOOR) {
-            this.toPool();
-        } else if (this.liftState == LiftState.POOL) {
+            this.toFloor();
+        } else if (this.liftState == LiftState.FLOAT) {
             this.toLowBucket();
         } else if (this.liftState == LiftState.LOW_BUCKET) {
             this.toHighBucket();
@@ -47,13 +48,13 @@ public class LiftSubsystem {
     }
 
     public void toFloor() {
-        this.setTarget(liftToPoolPos);
+        this.setTarget(liftToFloorPos);
         this.setState(LiftState.FLOOR);
     }
 
-    public void toPool() {
-        this.setTarget(liftToPoolPos);
-        this.setState(LiftState.POOL);
+    public void toFloat() {
+        this.setTarget(liftToFloatPos);
+        this.setState(LiftState.FLOAT);
     }
 
     public void toLowBucket() {