Refactored the Lift Update method.

edinabucketbrigade · Oct 23, 2024 · e486aee · e486aee
1 parent a69451d
commit e486aee
Showing 1 changed file with 50 additions and 22 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
@@ -6,9 +6,13 @@
 import org.firstinspires.ftc.teamcode.enums.LiftPosition;
 
 public class Lift extends SubSystem {
-    LiftPosition liftPosition;
-    LiftPosition liftState;
-    RobotHardware robot;
+    private LiftPosition liftPosition;
+    private LiftPosition liftState;
+    private RobotHardware robot;
+    private boolean aPressed = false;
+    private boolean xPressed = false;
+    private boolean yPressed = false;
+
     // Encoder positions for the lift.
     //TODO Test to find the proper values.
     private final int LIFT_DOWN = 0;
@@ -25,6 +29,9 @@ public Lift(RobotHardware robot) {
     public void init() {
         liftPosition = LiftPosition.Down;
         liftState = liftPosition;
+        // Make sure encoder is 0 at start.
+        robot.liftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.liftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
     }
 
     @Override
@@ -33,28 +40,49 @@ public void start() {
 
     @Override
     public void update() {
-        if (robot.liftMotor.isBusy() && (liftState != liftPosition)) {
-            return;
-        } else {
-            liftState = liftPosition;
-        }
+        switch (liftState) {
+            case Down:
+                if (robot.liftMotor.getCurrentPosition() < 5) {
+                    if (xPressed) {
+                        robot.liftMotor.setTargetPosition(LIFT_DOWN);
+                        liftState = LiftPosition.LowBasket;
+                    }
 
-        // Only try to move if we are finished with any moves.
-        if (!robot.liftMotor.isBusy()) {
-            switch (liftPosition) {
-                case Down:
-                    robot.liftMotor.setTargetPosition(LIFT_DOWN);
-                case LowBasket:
-                    robot.liftMotor.setTargetPosition(LIFT_LOW);
-                case HighBasket:
-                    robot.liftMotor.setTargetPosition(LIFT_HIGH);
-            }
+                    if (yPressed) {
+                        robot.liftMotor.setTargetPosition(LIFT_HIGH);
+                        liftState = LiftPosition.HighBasket;
+                    }
+                }
+                break;
+            case LowBasket:
+                if (robot.liftMotor.getCurrentPosition() < 5) {
+                    if (aPressed) {
+                        robot.liftMotor.setTargetPosition(LIFT_DOWN);
+                        liftState = LiftPosition.Down;
+                    }
 
-            if (liftState != liftPosition) {
-                robot.liftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-                robot.liftMotor.setPower(LIFT_MAX_POWER);
-            }
+                    if (yPressed) {
+                        robot.liftMotor.setTargetPosition(LIFT_HIGH);
+                        liftState = LiftPosition.HighBasket;
+                    }
+                }
+                break;
+            case HighBasket:
+                if (robot.liftMotor.getCurrentPosition() < 5) {
+                    if (aPressed) {
+                        robot.liftMotor.setTargetPosition(LIFT_DOWN);
+                        liftState = LiftPosition.Down;
+                    }
+                }
+                break;
+            default:
+                // If we get here we have problem, stop the lift.
+                robot.liftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                robot.liftMotor.setPower(0);
+                liftState = LiftPosition.Down;
         }
+
+        robot.liftMotor.setPower(LIFT_MAX_POWER);
     }
 
     // Respond to gamepad inputs.