Merge branch 'Development' into rhs

edinabucketbrigade · Oct 23, 2024 · a69451d · a69451d
2 parents 8e4a217 + d97d158
commit a69451d
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 6 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotHardware.java
@@ -2,6 +2,7 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 
 public class RobotHardware {
     /* Declare OpMode members. */
@@ -30,10 +31,10 @@ public void init() {
         liftMotor.setDirection(DcMotor.Direction.REVERSE);
         liftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
-        leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
-        leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
-        rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
-        rightBackDrive.setDirection(DcMotor.Direction.FORWARD);
+        leftFrontDrive.setDirection(DcMotor.Direction.FORWARD);
+        leftBackDrive.setDirection(DcMotor.Direction.FORWARD);
+        rightFrontDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightBackDrive.setDirection(DcMotor.Direction.REVERSE);
     }
 
     /**

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
@@ -7,6 +7,7 @@
 
 public class Lift extends SubSystem {
     LiftPosition liftPosition;
+    LiftPosition liftState;
     RobotHardware robot;
     // Encoder positions for the lift.
     //TODO Test to find the proper values.
@@ -23,6 +24,7 @@ public Lift(RobotHardware robot) {
     @Override
     public void init() {
         liftPosition = LiftPosition.Down;
+        liftState = liftPosition;
     }
 
     @Override
@@ -31,6 +33,12 @@ public void start() {
 
     @Override
     public void update() {
+        if (robot.liftMotor.isBusy() && (liftState != liftPosition)) {
+            return;
+        } else {
+            liftState = liftPosition;
+        }
+
         // Only try to move if we are finished with any moves.
         if (!robot.liftMotor.isBusy()) {
             switch (liftPosition) {
@@ -42,8 +50,10 @@ public void update() {
                     robot.liftMotor.setTargetPosition(LIFT_HIGH);
             }
 
-            robot.liftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            robot.liftMotor.setPower(LIFT_MAX_POWER);
+            if (liftState != liftPosition) {
+                robot.liftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+                robot.liftMotor.setPower(LIFT_MAX_POWER);
+            }
         }
     }