Commit

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java

@@ -45,7 +45,7 @@ public class DevTeleop extends OpMode {
     public DcMotor frontRightMotor;
     public DcMotor backRightMotor;
 
-    private double MAX_POWER = .6;
+    private double MAX_POWER = .45;
     @Override
     public void init() {
         claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
@@ -74,22 +74,24 @@ public class DevTeleop extends OpMode {
     }
 
     public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
-
-        if (currentGamepad1.a && !previousGamepad1.a) {
+//pick up
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
             wrist.floorWrist();
             arm.engageArm();
         }
 
+
+
     }
     public void thePickup(ClawSubsystem claw) {
-
-        if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+//claw open close
+        if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
             claw.switchState();
         }
 
     }
 
-    public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
+    /* public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
 
         if (currentGamepad1.b && !previousGamepad1.b) {
             arm.parkArm();
@@ -97,22 +99,44 @@ public class DevTeleop extends OpMode {
         }
 
     }
-
+*/
     public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
-        if (currentGamepad2.y && !previousGamepad2.y) {
+//low bucket
+        if (currentGamepad2.a && !previousGamepad2.a) {
             lift.toLowBucket();
             arm.bucketArm();
             wrist.bucketWrist();
         }
     }
-    public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
 
-        if (currentGamepad2.a && !previousGamepad2.a) {
+    public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
+//high basket
+        if (currentGamepad2.b && !previousGamepad2.b) {
             lift.toHighBucket();
             arm.bucketArm();
             wrist.bucketWrist();
         }
     }
+
+    public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){
+//
+        if (currentGamepad2.dpad_right && !previousGamepad2.dpad_right){
+            lift.toFloor();
+            arm.bucketArm();
+            wrist.floorWrist();
+        }
+    }
+
+    public void hoverState(ArmSubsystem arm, WristSubsystem wrist, LiftSubsystem lift){
+        if (currentGamepad1.dpad_left && !previousGamepad2.dpad_left){
+            lift.toHover();
+            wrist.floorWrist();
+            arm.engageArm();
+        }
+
+
+    }
+
     @Override
     public void loop() {
         previousGamepad1.copy(currentGamepad1);
@@ -123,9 +147,10 @@ public class DevTeleop extends OpMode {
 
         theDrop(arm, wrist);
         thePickup(claw);
-        theLift(arm, wrist);
+       // theLift(arm, wrist);
         theLowBucketScore(lift, wrist, arm);
         theHighBucketScore(lift, wrist, arm);
+        theTravel(lift, arm, wrist);
 
         double max;
 
@@ -164,6 +189,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();
 
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class RobotConstants {
     public static double clawClose = 1.00;
-    public static double clawOpen = 0.25;
+    public static double clawOpen = 0.05;
 
     public static double armEngage = 0.5;
     public static double armPark = 0.125;
@@ -13,9 +13,10 @@ public class RobotConstants {
 
     public static double wristFloor = 0.625;
     public static double wristBucket = 0.215;
-    public static int liftToFloorPos = 0;
-    public static int liftToPoolPos = 650;
+    public static int liftToFloorPos = 20;
+    public static int liftToFloatPos = 150;
     public static int liftToLowBucketPos = 2250;
-    public static int liftToHighBucketPos = 3850;
+    public static int liftToHighBucketPos = 3900;
     public static double liftPower = .45;
+    public static int liftToHoverState = 60;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java

@@ -1,9 +1,11 @@
 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 static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHoverState;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -17,7 +19,7 @@ public class LiftSubsystem {
     public RunAction toFloor, toLowBucket, toHighBucket;
 
     public enum LiftState {
-        FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL
+        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, HOVER
     }
 
     private LiftState liftState;
@@ -36,8 +38,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();
@@ -46,14 +48,19 @@ public class LiftSubsystem {
         }
     }
 
+     public void toHover() {
+        this.setTarget(liftToHoverState);
+        this.setState(LiftState.HOVER);
+    }
+
     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() {