From d2ceff589f856cece31dd1e6a9d63d4f5fa1adba Mon Sep 17 00:00:00 2001
From: Abigail <79606542+abigailprowse@users.noreply.github.com>
Date: Fri, 20 Jan 2023 11:46:15 -0500
Subject: [PATCH] separate manual and levels

Both manual and levels are working, but while the arm is moving, the manual controls cannot be used.  Pay attention to alreadyMoving variable which will tell you when the arm is moving during the finite state machine.  God sorry i messed this up the first time, my brain has literally been going in circles for like the last 12 hours.
---
 .../teamcode/TeleOp/LinearSlidesDrive.java    | 63 +++++++++++--------
 1 file changed, 38 insertions(+), 25 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/LinearSlidesDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/LinearSlidesDrive.java
index bdb55f9..d186dae 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/LinearSlidesDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/LinearSlidesDrive.java
@@ -11,12 +11,9 @@
 import com.qualcomm.robotcore.hardware.TouchSensor;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import com.qualcomm.robotcore.hardware.DistanceSensor;
-
 import java.util.List;
-
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 @TeleOp (name = "LinearSlidesDrive" , group = "Iterative Opmode")
 public class LinearSlidesDrive extends OpMode {
     // wheels
@@ -77,6 +74,8 @@ public enum ArmState {
     // bottom is the default
 
     private boolean found = false; // tells whether we have reached target height once
+    
+    private boolean alreadyMoving = false; // tells whether the arm is already moving.
 
     @Override
     public void init() {
@@ -200,17 +199,21 @@ public void loop() {
             case BOTTOM:
                 arm.setPower(0);
                 arm2.setPower(0);
+                alreadyMoving = false;
 
                 found = false;
 
                 if (a2) {
                     levels = ArmState.LOWER;
+                    alreadyMoving = true;
                 }
                 if (b2) {
                     levels = ArmState.MIDDLE;
+                    alreadyMoving = true;
                 }
                 if (y2) {
                     levels = ArmState.UPPER;
+                    alreadyMoving = true;
                 }
                 break;
             // at low  continue to low or respond to button push
@@ -222,10 +225,12 @@ public void loop() {
                     arm.setPower(0);
                     arm2.setPower(0);
                     found = true;
+                    alreadyMoving = false;
                 }
                 
                 if (rb2) {
                     levels = ArmState.RESET;
+                    alreadyMoving = true;
                 }
                 break;
             // at middle  continue to middle or respond to button push
@@ -239,15 +244,16 @@ public void loop() {
                     arm.setPower(0);
                     arm2.setPower(0);
                     found = true;
+                    alreadyMoving = false;
                 }
                 
                 if (rb2) {
                     levels = ArmState.RESET;
+                    alreadyMoving = true;
                 }
                 break;
             case UPPER:
                 
-
                 if (armHeight.getDistance(DistanceUnit.INCH) < tall  &&  !found) {
                     arm.setPower(.6);
                     arm2.setPower(.6);
@@ -255,10 +261,12 @@ public void loop() {
                     arm.setPower(0);
                     arm2.setPower(0);
                     found = true;
+                    alreadyMoving = false;
                 }
                 
                 if (rb2) {
                     levels = ArmState.RESET;
+                    alreadyMoving = true;
                 }
                 break;
             // at reset  continue to reset or respond to button push
@@ -269,11 +277,13 @@ public void loop() {
                     arm2.setPower(-.3);
                 } else {
                     levels = ArmState.BOTTOM;
+                    alreadyMoving = false;
                 }
 
                 break;
             default:
                 levels = ArmState.BOTTOM;
+                alreadyMoving = false;
         }
 
 
@@ -366,32 +376,35 @@ public void loop() {
 
         pow = 0.8;
 
-        if (Math.abs(lefty2) >= DEAD_ZONE && arm.getPower() == 0) {
-            if (lefty2 < 0) {
-                if (armTouch.isPressed()) {
-                    // if button is pressed, stop moving
-                    arm.setPower(0);
-                    arm2.setPower(0);
-                } else {
+        if (!alreadyMoving) {
+            // basically we are waiting to use it manually
+            if (Math.abs(lefty2) >= DEAD_ZONE) {
+                if (lefty2 < 0) {
+                    if (armTouch.isPressed()) {
+                        // if button is pressed, stop moving
+                        arm.setPower(0);
+                        arm2.setPower(0);
+                    } else {
+                        arm.setPower(lefty2 * pow);
+                        arm2.setPower(lefty2 * pow);
+                    }
+                }
+                if (lefty2 > 0) {
                     arm.setPower(lefty2 * pow);
                     arm2.setPower(lefty2 * pow);
                 }
+            } else if (buttonDown2) {
+                arm.setPower(-pow / 2);
+                arm2.setPower(-pow / 2);
+            } else if (buttonUp2) {
+                arm.setPower(pow / 2);
+                arm2.setPower(pow / 2);
+            } else {
+                arm.setPower(0);
+                arm2.setPower(0);
             }
-            if (lefty2 > 0) {
-                arm.setPower(lefty2 * pow);
-                arm2.setPower(lefty2 * pow);
-            }
-        } else if (buttonDown2) {
-            arm.setPower(-pow / 2);
-            arm2.setPower(-pow / 2);
-        } else if (buttonUp2) {
-            arm.setPower(pow / 2);
-            arm2.setPower(pow / 2);
-        } else {
-            arm.setPower(0);
-            arm2.setPower(0);
         }
-
+        
         // add information on arm powers
         telemetry.addData("arm", arm.getPower());
         telemetry.addData("arm2", arm2.getPower());