Update Lift.java

trying to fix the lift motor and state machine
edinabucketbrigade · Oct 26, 2024 · d21da73 · d21da73
1 parent 69533f6
commit d21da73
Showing 1 changed file with 5 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;
@@ -40,9 +41,9 @@ public void start() {
 
     @Override
     public void update() {
-        switch (liftPosition) {
+        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 +56,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,7 +69,7 @@ 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;