diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java index 30b4dc3..282320d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java @@ -76,7 +76,7 @@ public class LiftTest extends LinearOpMode { currentGamepad1.copy(gamepad1); if (currentGamepad1.square && !previousGamepad1.square) { - lift.toZero(); + lift.toFloor(); } if (currentGamepad1.triangle && !previousGamepad1.triangle) { 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 aa62e1c..cf66dce 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,7 +13,8 @@ public class RobotConstants { public static double wristFloor = 0.625; public static double wristBucket = 0.215; - public static int liftZeroPos = 0; + public static int liftToFloorPos = 0; + public static int liftToPoolPos = 500; public static int liftToLowBucketPos = 2250; public static int liftToHighBucketPos = 3700; 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 33bd585..e523389 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 @@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower; 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.liftZeroPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToPoolPos; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -14,16 +14,17 @@ import org.firstinspires.ftc.teamcode.util.action.RunAction; public class LiftSubsystem { public DcMotor lift; - public RunAction toZero, toLowBucket, toHighBucket; + public RunAction toFloor, toLowBucket, toHighBucket; + public enum LiftState { - FLOOR, LOW_BUCKET, HIGH_BUCKET + FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL } + private LiftState liftState; public LiftSubsystem(HardwareMap hardwareMap) { lift = hardwareMap.get(DcMotor.class, "lift-motor"); - - toZero = new RunAction(this::toZero); + toFloor = new RunAction(this::toFloor); toLowBucket = new RunAction(this::toLowBucket); toHighBucket = new RunAction(this::toHighBucket); } @@ -33,29 +34,36 @@ public class LiftSubsystem { lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); } - public void toZero() { - lift.setTargetPosition(liftZeroPos); - lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - public void switchState() { - if(this.liftState == LiftState.FLOOR) { + if (this.liftState == LiftState.FLOOR) { + this.toPool(); + } else if (this.liftState == LiftState.POOL) { this.toLowBucket(); } else if (this.liftState == LiftState.LOW_BUCKET) { this.toHighBucket(); } else if (this.liftState == LiftState.HIGH_BUCKET) { - this.toZero(); + this.toFloor(); } } + public void toFloor() { + this.setTarget(liftToPoolPos); + this.setState(LiftState.FLOOR); + } + + public void toPool() { + this.setTarget(liftToPoolPos); + this.setState(LiftState.POOL); + } + public void toLowBucket() { - lift.setTargetPosition(liftToLowBucketPos); - lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.setTarget(liftToLowBucketPos); + this.setState(LiftState.LOW_BUCKET); } public void toHighBucket() { - lift.setTargetPosition(liftToHighBucketPos); - lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.setTarget(liftToHighBucketPos); + this.setState(LiftState.HIGH_BUCKET); } public void init() {