Some Lift refactors

Changes to get the run to position working.
edinabucketbrigade · Oct 25, 2024 · 30d6680 · 30d6680
1 parent c5b695b
commit 30d6680
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
@@ -20,6 +20,7 @@ public class Lift extends SubSystem {
     private final int LIFT_HIGH = 20;
 
     private final double LIFT_MAX_POWER = .7;
+    private final int LIFT_POSITION_TOLERANCE = 10;
 
     public Lift(RobotHardware robot) {
         this.robot = robot;
@@ -32,6 +33,7 @@ public void init() {
         // Make sure encoder is 0 at start
         robot.liftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.liftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        robot.liftMotor.setTargetPositionTolerance(LIFT_POSITION_TOLERANCE);
     }
 
     @Override
@@ -42,7 +44,7 @@ public void start() {
     public void update() {
         switch (liftState) {
             case Down:
-                if (robot.liftMotor.getCurrentPosition() < 5) {
+                if (Math.abs(robot.liftMotor.getCurrentPosition() - LIFT_DOWN) < LIFT_POSITION_TOLERANCE) {
                     if (xPressed) {
                         robot.liftMotor.setTargetPosition(LIFT_DOWN);
                         liftState = LiftPosition.LowBasket;
@@ -55,7 +57,7 @@ public void update() {
                 }
                 break;
             case LowBasket:
-                if (robot.liftMotor.getCurrentPosition() < 5) {
+                if (Math.abs(robot.liftMotor.getCurrentPosition() - LIFT_LOW) < LIFT_POSITION_TOLERANCE) {
                     if (aPressed) {
                         robot.liftMotor.setTargetPosition(LIFT_DOWN);
                         liftState = LiftPosition.Down;
@@ -68,18 +70,19 @@ public void update() {
                 }
                 break;
             case HighBasket:
-                if (robot.liftMotor.getCurrentPosition() < 5) {
+                if (Math.abs(robot.liftMotor.getCurrentPosition() - LIFT_HIGH) < LIFT_POSITION_TOLERANCE) {
                     if (aPressed) {
                         robot.liftMotor.setTargetPosition(LIFT_DOWN);
                         liftState = LiftPosition.Down;
                     }
                 }
-                    break;
+                break;
             default:
                 // if get here, there is a problem
                 robot.liftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
                 robot.liftMotor.setPower(0);
                 liftState = LiftPosition.Down;
+                return;
         }
 
         robot.liftMotor.setPower(LIFT_MAX_POWER);