From 7aa9511131e753d02f1cc54917d8f9c17f92c163 Mon Sep 17 00:00:00 2001
From: Aarush <36015585+AarushX@users.noreply.github.com>
Date: Thu, 25 Jan 2024 12:17:10 -0500
Subject: [PATCH 1/2] Fixed IMU

---
 TeamCode/build.gradle | 4 ++--
 gradle.properties     | 5 +----
 2 files changed, 3 insertions(+), 6 deletions(-)

diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 756eae793e4b..14467fae4fa7 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -34,7 +34,7 @@ dependencies {
 }
 
 repositories {
-    flatDir {
-        dirs 'libs', navxPath
+    flatDir{
+        dirs '../libs'
     }
 }
diff --git a/gradle.properties b/gradle.properties
index 5d2b2f798759..703a88f1985e 100644
--- a/gradle.properties
+++ b/gradle.properties
@@ -7,7 +7,4 @@ android.useAndroidX=true
 android.enableJetifier=true
 
 # Allow Gradle to use up to 1 GB of RAM
-org.gradle.jvmargs=-Xmx1024M
-
-# Adds the navxPath for users -- Currently trying to make it so that it doesnt have to be changed per person
-navxPath=C\:\\Users\\micha\\navx-micro\\android\\libs
+org.gradle.jvmargs=-Xmx1024M
\ No newline at end of file

From 4c2fdf11172764adc470c28852f5230bfae2a90d Mon Sep 17 00:00:00 2001
From: Aarush <36015585+AarushX@users.noreply.github.com>
Date: Sat, 10 Feb 2024 18:41:33 -0500
Subject: [PATCH 2/2] j

cioik
---
 .gitignore                                    |   3 +-
 .run/TeamCode.run.xml                         |   4 +-
 TeamCode/build.gradle                         |   1 +
 .../ftc/teamcode/auto/BlueClose.java          | 508 ++++++++--------
 .../ftc/teamcode/auto/BlueClose2plus2.java    | 540 +++++++++---------
 .../ftc/teamcode/auto/BlueFar.java            | 506 ++++++++--------
 .../ftc/teamcode/auto/BlueFar2plus2.java      | 534 ++++++++---------
 .../ftc/teamcode/auto/RedClose.java           | 492 ++++++++--------
 .../ftc/teamcode/auto/RedClose2plus2.java     | 540 +++++++++---------
 .../ftc/teamcode/auto/RedFar.java             | 506 ++++++++--------
 .../ftc/teamcode/auto/RedFar2plus2.java       | 530 ++++++++---------
 .../auto/TrajectorySequenceCommand.java       |  64 +--
 .../commands/AarushsIntakeCommandEx.java      |  97 ++++
 .../commands/SlidesManualCommand.java         |  36 --
 .../teamcode/commands/SlidesResetCommand.java |  39 --
 .../liftcommands/LiftPositionCommand.java     |  80 +++
 .../liftcommands/ManualLiftCommand.java       |  76 +++
 .../liftcommands/ManualLiftResetCommand.java  |  42 ++
 .../liftcommands/ProfiledLiftCommand.java     | 137 +++++
 .../commands/presets/MoveToScoringCommand.kt  |  48 ++
 .../presets/RetractOuttakeCommand.java        |  31 +
 .../ftc/teamcode/subsystems/Drivebase.java    | 130 +++++
 .../ftc/teamcode/subsystems/Impasta.java      | 242 --------
 .../ftc/teamcode/subsystems/Intake.java       |  24 +-
 .../ftc/teamcode/subsystems/Launcher.java     |  28 +-
 .../ftc/teamcode/subsystems/Mechanisms.java   |  86 +--
 .../ftc/teamcode/subsystems/Outtake.java      |  71 ++-
 .../ftc/teamcode/subsystems/PID.java          |  19 -
 .../ftc/teamcode/subsystems/SlidePID.java     |  68 +--
 .../ftc/teamcode/subsystems/Slides.java       | 122 ++--
 .../ftc/teamcode/subsystems/V4B.java          |  83 ++-
 .../ftc/teamcode/subsystems/Winch.java        |  34 --
 .../teamcode/teleop/ImpastaCommandTeleOp.java | 145 -----
 .../teleop/ImpastaRobotCentricTeleOp.java     | 248 ++++----
 .../ftc/teamcode/teleop/ImpastaTeleOp.java    | 284 +++++----
 .../ftc/teamcode/teleop/LiftTester.java       |  99 ++++
 .../ftc/teamcode/teleop/MainTeleOp.java       | 187 ++++++
 .../ftc/teamcode/teleop/OuttakeTester.java    |  92 +--
 .../ftc/teamcode/teleop/SlidePIDTest.java     | 102 ++--
 .../ftc/teamcode/testers/ClawScanner.java     |  49 ++
 .../ftc/teamcode/util/CrossBindings.java      |  15 +
 .../ftc/teamcode/util/ServoModule.java        |  95 +++
 build.gradle                                  |   4 +
 43 files changed, 3817 insertions(+), 3224 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AarushsIntakeCommandEx.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesManualCommand.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesResetCommand.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/LiftPositionCommand.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftCommand.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftResetCommand.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ProfiledLiftCommand.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/MoveToScoringCommand.kt
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/RetractOuttakeCommand.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivebase.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Impasta.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PID.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Winch.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaCommandTeleOp.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/LiftTester.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleOp.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testers/ClawScanner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CrossBindings.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ServoModule.java

diff --git a/.gitignore b/.gitignore
index b85aa2fac629..91e150fd23fa 100644
--- a/.gitignore
+++ b/.gitignore
@@ -78,4 +78,5 @@ lint/intermediates/
 lint/generated/
 lint/outputs/
 lint/tmp/
-# lint/reports/
\ No newline at end of file
+# lint/reports/
+.run/TeamCode.run.xml
diff --git a/.run/TeamCode.run.xml b/.run/TeamCode.run.xml
index e0a71e8ae146..5e0a77a6a898 100644
--- a/.run/TeamCode.run.xml
+++ b/.run/TeamCode.run.xml
@@ -1,6 +1,6 @@
 <component name="ProjectRunConfigurationManager">
   <configuration default="false" name="TeamCode" type="AndroidRunConfigurationType" factoryName="Android App">
-    <module name="FtcRobotController.TeamCode.main" />
+    <module name="akshaj_FtcRobotController.TeamCode.main" />
     <option name="DEPLOY" value="true" />
     <option name="DEPLOY_APK_FROM_BUNDLE" value="false" />
     <option name="DEPLOY_AS_INSTANT" value="false" />
@@ -16,6 +16,8 @@
     <option name="SHOW_LOGCAT_AUTOMATICALLY" value="false" />
     <option name="INSPECTION_WITHOUT_ACTIVITY_RESTART" value="false" />
     <option name="TARGET_SELECTION_MODE" value="DEVICE_AND_SNAPSHOT_COMBO_BOX" />
+    <option name="SELECTED_CLOUD_MATRIX_CONFIGURATION_ID" value="-1" />
+    <option name="SELECTED_CLOUD_MATRIX_PROJECT_ID" value="" />
     <option name="DEBUGGER_TYPE" value="Auto" />
     <Auto>
       <option name="USE_JAVA_AWARE_DEBUGGER" value="false" />
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 701f1898fb90..c0ae3c7183c8 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -16,6 +16,7 @@ apply from: '../build.common.gradle'
 apply from: '../build.dependencies.gradle'
 
 apply plugin: 'org.team11260.fast-load-plugin'
+apply plugin: 'kotlin-android'
 
 android {
     namespace = 'org.firstinspires.ftc.teamcode'
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose.java
index 2650cd211cb7..287ddad33788 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose.java
@@ -1,255 +1,255 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-
-import java.util.concurrent.TimeUnit;
-
-
-@Autonomous(name="Blue Close", group="Close")
-public class BlueClose extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    Gamepad gamepad;
-
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseCloseBlue = new Pose2d(12,62, Math.toRadians(-90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-
-        drive.setPoseEstimate(startPoseCloseBlue);
-
-        TrajectorySequence propCloseLeftBlue = drive.trajectorySequenceBuilder(startPoseCloseBlue)
-                .forward(25)
-                .turn(Math.toRadians(90))
-                .forward(3)
-                .back(3)
-                .strafeLeft(25)
-                .build();
-
-        TrajectorySequence scoreCloseLeftBlue = drive.trajectorySequenceBuilder(propCloseLeftBlue.end())
-                .forward(34)
-                .strafeRight(12)
-                .build();
-
-        TrajectorySequence parkCloseLeftBlue = drive.trajectorySequenceBuilder(scoreCloseLeftBlue.end())
-                .strafeLeft(20)
-                .forward(20)
-                .build();
-
-        TrajectorySequence propCloseMidBlue = drive.trajectorySequenceBuilder(startPoseCloseBlue)
-                .forward(28)
-                .lineToLinearHeading(new Pose2d(12, 62, 0))
-                .build();
-
-        TrajectorySequence scoreCloseMidBlue = drive.trajectorySequenceBuilder(propCloseMidBlue.end())
-                .forward(34)
-                .strafeRight(25)
-                .build();
-
-        TrajectorySequence parkCloseMidBlue = drive.trajectorySequenceBuilder(scoreCloseMidBlue.end())
-                .strafeLeft(25)
-                .forward(12)
-                .build();
-
-        TrajectorySequence propCloseRightBlue = drive.trajectorySequenceBuilder(startPoseCloseBlue)
-                .forward(25)
-                .turn(Math.toRadians(-90))
-                .forward(4)
-                .back(4)
-                .lineToLinearHeading(new Pose2d(12, 62, 0))
-                .build();
-
-        TrajectorySequence scoreCloseRightBlue = drive.trajectorySequenceBuilder(propCloseRightBlue.end())
-                .forward(34)
-                .strafeRight(30)
-                .build();
-
-        TrajectorySequence parkCloseRightBlue = drive.trajectorySequenceBuilder(scoreCloseRightBlue.end())
-                .strafeLeft(30)
-                .forward(12)
-                .build();
-
-        //Spline trajectories
-
-        TrajectorySequence LeftSplines = drive.trajectorySequenceBuilder(startPoseCloseBlue)
-//                Prop
-                .lineToLinearHeading(new Pose2d(13, 30, Math.toRadians(0)))
-//                Score
-                .back(3)
-                .strafeLeft(13)
-                .splineToSplineHeading(new Pose2d(46, 42, Math.toRadians(0)), 0)
-//                Park
-                .strafeLeft(10)
-                .splineToLinearHeading(new Pose2d(60, 62, 0), 0)
-                .build();
-
-        TrajectorySequence RightSplines = drive.trajectorySequenceBuilder(startPoseCloseBlue)
-//                Prop
-                .splineToLinearHeading(new Pose2d(9, 37, Math.toRadians(180)), Math.toRadians(180))
-//                Score
-                .back(3)
-                .strafeRight(10)
-                .splineToSplineHeading(new Pose2d(46, 32, Math.toRadians(0)), 0)
-//                Park
-                .strafeLeft(10)
-                .splineToLinearHeading(new Pose2d(60, 62, 0), 0)
-                .build();
-
-        TrajectorySequence MidSplines = drive.trajectorySequenceBuilder(startPoseCloseBlue)
-//                Prop
-                .forward(28)
-//                Score
-                .back(3)
-                .splineToSplineHeading(new Pose2d(46, 37, Math.toRadians(0)), 0)
-//                Park
-                .strafeLeft(15)
-                .splineToLinearHeading(new Pose2d(60, 62, 0), 0)
-                .build();
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 1) { //If blue, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            } else {
-                section = "MIDDLE";
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propCloseLeftBlue;
-                score = scoreCloseLeftBlue;
-                park = parkCloseLeftBlue;
-                break;
-            case "MIDDLE":
-                prop = propCloseMidBlue;
-                score = scoreCloseMidBlue;
-                park = parkCloseMidBlue;
-                break;
-            default:
-                prop = propCloseRightBlue;
-                score = scoreCloseRightBlue;
-                park = parkCloseRightBlue;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propCloseMidBlue),
-//                    new TrajectorySequenceCommand(drive, scoreCloseMidBlue),
-//                    new TrajectorySequenceCommand(drive, parkCloseMidBlue)
-//                ),
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//
+//@Autonomous(name="Blue Close", group="Close")
+//public class BlueClose extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    Gamepad gamepad;
+//
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseCloseBlue = new Pose2d(12,62, Math.toRadians(-90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//
+//        drive.setPoseEstimate(startPoseCloseBlue);
+//
+//        TrajectorySequence propCloseLeftBlue = drive.trajectorySequenceBuilder(startPoseCloseBlue)
+//                .forward(25)
+//                .turn(Math.toRadians(90))
+//                .forward(3)
+//                .back(3)
+//                .strafeLeft(25)
+//                .build();
+//
+//        TrajectorySequence scoreCloseLeftBlue = drive.trajectorySequenceBuilder(propCloseLeftBlue.end())
+//                .forward(34)
+//                .strafeRight(12)
+//                .build();
+//
+//        TrajectorySequence parkCloseLeftBlue = drive.trajectorySequenceBuilder(scoreCloseLeftBlue.end())
+//                .strafeLeft(20)
+//                .forward(20)
+//                .build();
+//
+//        TrajectorySequence propCloseMidBlue = drive.trajectorySequenceBuilder(startPoseCloseBlue)
+//                .forward(28)
+//                .lineToLinearHeading(new Pose2d(12, 62, 0))
+//                .build();
+//
+//        TrajectorySequence scoreCloseMidBlue = drive.trajectorySequenceBuilder(propCloseMidBlue.end())
+//                .forward(34)
+//                .strafeRight(25)
+//                .build();
+//
+//        TrajectorySequence parkCloseMidBlue = drive.trajectorySequenceBuilder(scoreCloseMidBlue.end())
+//                .strafeLeft(25)
+//                .forward(12)
+//                .build();
+//
+//        TrajectorySequence propCloseRightBlue = drive.trajectorySequenceBuilder(startPoseCloseBlue)
+//                .forward(25)
+//                .turn(Math.toRadians(-90))
+//                .forward(4)
+//                .back(4)
+//                .lineToLinearHeading(new Pose2d(12, 62, 0))
+//                .build();
+//
+//        TrajectorySequence scoreCloseRightBlue = drive.trajectorySequenceBuilder(propCloseRightBlue.end())
+//                .forward(34)
+//                .strafeRight(30)
+//                .build();
+//
+//        TrajectorySequence parkCloseRightBlue = drive.trajectorySequenceBuilder(scoreCloseRightBlue.end())
+//                .strafeLeft(30)
+//                .forward(12)
+//                .build();
+//
+//        //Spline trajectories
+//
+//        TrajectorySequence LeftSplines = drive.trajectorySequenceBuilder(startPoseCloseBlue)
+////                Prop
+//                .lineToLinearHeading(new Pose2d(13, 30, Math.toRadians(0)))
+////                Score
+//                .back(3)
+//                .strafeLeft(13)
+//                .splineToSplineHeading(new Pose2d(46, 42, Math.toRadians(0)), 0)
+////                Park
+//                .strafeLeft(10)
+//                .splineToLinearHeading(new Pose2d(60, 62, 0), 0)
+//                .build();
+//
+//        TrajectorySequence RightSplines = drive.trajectorySequenceBuilder(startPoseCloseBlue)
+////                Prop
+//                .splineToLinearHeading(new Pose2d(9, 37, Math.toRadians(180)), Math.toRadians(180))
+////                Score
+//                .back(3)
+//                .strafeRight(10)
+//                .splineToSplineHeading(new Pose2d(46, 32, Math.toRadians(0)), 0)
+////                Park
+//                .strafeLeft(10)
+//                .splineToLinearHeading(new Pose2d(60, 62, 0), 0)
+//                .build();
+//
+//        TrajectorySequence MidSplines = drive.trajectorySequenceBuilder(startPoseCloseBlue)
+////                Prop
+//                .forward(28)
+////                Score
+//                .back(3)
+//                .splineToSplineHeading(new Pose2d(46, 37, Math.toRadians(0)), 0)
+////                Park
+//                .strafeLeft(15)
+//                .splineToLinearHeading(new Pose2d(60, 62, 0), 0)
+//                .build();
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 1) { //If blue, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            } else {
+//                section = "MIDDLE";
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propCloseLeftBlue;
+//                score = scoreCloseLeftBlue;
+//                park = parkCloseLeftBlue;
+//                break;
+//            case "MIDDLE":
+//                prop = propCloseMidBlue;
+//                score = scoreCloseMidBlue;
+//                park = parkCloseMidBlue;
+//                break;
+//            default:
+//                prop = propCloseRightBlue;
+//                score = scoreCloseRightBlue;
+//                park = parkCloseRightBlue;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propCloseMidBlue),
+////                    new TrajectorySequenceCommand(drive, scoreCloseMidBlue),
+////                    new TrajectorySequenceCommand(drive, parkCloseMidBlue)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+////                    s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//                new TrajectorySequenceCommand(drive, prop),
+//                new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-//                    s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
-//        ));
-
-        schedule(new SequentialCommandGroup(
-                new TrajectorySequenceCommand(drive, prop),
-                new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, park)
-                )
-
-        );
-    }
-}
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, park)
+//                )
+//
+//        );
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose2plus2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose2plus2.java
index f3615a97ebfa..1f12877c6f41 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose2plus2.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueClose2plus2.java
@@ -1,272 +1,272 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Intake;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-
-import java.util.concurrent.TimeUnit;
-
-
-@Autonomous(name="Blue Close 2plus2", group="Close")
-public class BlueClose2plus2 extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    Intake i;
-    Gamepad gamepad;
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence parkAfter2;
-    TrajectorySequence scorePlus;
-    TrajectorySequence intakeIt;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        i = new Intake(gamepad1, hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-
-        drive.setPoseEstimate(startPoseCloseRed);
-
-        TrajectorySequence propCloseLeftRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .turn(Math.toRadians(90))
-                .forward(11)
-                .back(11)
-                .lineToLinearHeading(new Pose2d(12, -62, 0))
-                .build();
-        TrajectorySequence scoreCloseLeftRed = drive.trajectorySequenceBuilder(propCloseLeftRed.end())
-                .forward(34)
-                .strafeRight(24)
-                .build();
-
-        TrajectorySequence parkCloseLeftRed = drive.trajectorySequenceBuilder(scoreCloseLeftRed.end())
-                .strafeLeft(24)
-
-                .build();;
-
-        TrajectorySequence propCloseMidRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(12, -62, 0))
-                .build();
-
-        TrajectorySequence scoreCloseMidRed = drive.trajectorySequenceBuilder(propCloseMidRed.end())
-                .forward(34)
-                .strafeRight(25)
-                .build();
-        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
-                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(180)))
-                .lineToLinearHeading(new Pose2d(-60, 10.9, Math.toRadians(180)))
-                .build();
-                //intake here(code this late)
-
-        TrajectorySequence andScore = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
-                .lineToLinearHeading(new Pose2d(46.5, -10.9, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(45.8, 10.9, Math.toRadians(0)))
-                .build();
-        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
-                .lineToLinearHeading(new Pose2d(46.5, 61.9, 0))
-                .forward(10)
-                .build();
-
-        TrajectorySequence parkCloseMidRed = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
-                .strafeRight(25)
-                .forward(12)
-                .build();
-//
-        TrajectorySequence propCloseRightRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .turn(Math.toRadians(-90))
-                .forward(11)
-                .back(11)
-                .strafeRight(25)
-                .build();
-
-        TrajectorySequence scoreCloseRightRed = drive.trajectorySequenceBuilder(propCloseRightRed.end())
-                .forward(34)
-                .strafeLeft(12)
-                .build();
-
-        TrajectorySequence parkCloseRightRed = drive.trajectorySequenceBuilder(getPixel.end())
-                .strafeRight(12)
-                .forward(12)
-                .build();
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 2) { //If red, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propCloseLeftRed;
-                score = scoreCloseLeftRed;
-//                park = parkCloseLeftRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-            case "MIDDLE":
-                prop = propCloseMidRed;
-                score = scoreCloseMidRed;
-//                park = parkCloseMidRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-
-                break;
-            default:
-                prop = propCloseRightRed;
-                score = scoreCloseRightRed;
-//                park = parkCloseRightRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propCloseMidRed),
-//                    new TrajectorySequenceCommand(drive, scoreCloseMidRed),
-//                    new TrajectorySequenceCommand(drive, parkCloseMidRed)
-//                ),
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Intake;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//
+//@Autonomous(name="Blue Close 2plus2", group="Close")
+//public class BlueClose2plus2 extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    Intake i;
+//    Gamepad gamepad;
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence parkAfter2;
+//    TrajectorySequence scorePlus;
+//    TrajectorySequence intakeIt;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        i = new Intake(gamepad1, hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//
+//        drive.setPoseEstimate(startPoseCloseRed);
+//
+//        TrajectorySequence propCloseLeftRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .turn(Math.toRadians(90))
+//                .forward(11)
+//                .back(11)
+//                .lineToLinearHeading(new Pose2d(12, -62, 0))
+//                .build();
+//        TrajectorySequence scoreCloseLeftRed = drive.trajectorySequenceBuilder(propCloseLeftRed.end())
+//                .forward(34)
+//                .strafeRight(24)
+//                .build();
+//
+//        TrajectorySequence parkCloseLeftRed = drive.trajectorySequenceBuilder(scoreCloseLeftRed.end())
+//                .strafeLeft(24)
+//
+//                .build();;
+//
+//        TrajectorySequence propCloseMidRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(12, -62, 0))
+//                .build();
+//
+//        TrajectorySequence scoreCloseMidRed = drive.trajectorySequenceBuilder(propCloseMidRed.end())
+//                .forward(34)
+//                .strafeRight(25)
+//                .build();
+//        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(180)))
+//                .lineToLinearHeading(new Pose2d(-60, 10.9, Math.toRadians(180)))
+//                .build();
+//                //intake here(code this late)
+//
+//        TrajectorySequence andScore = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
+//                .lineToLinearHeading(new Pose2d(46.5, -10.9, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(45.8, 10.9, Math.toRadians(0)))
+//                .build();
+//        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 61.9, 0))
+//                .forward(10)
+//                .build();
+//
+//        TrajectorySequence parkCloseMidRed = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
+//                .strafeRight(25)
+//                .forward(12)
+//                .build();
+////
+//        TrajectorySequence propCloseRightRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .turn(Math.toRadians(-90))
+//                .forward(11)
+//                .back(11)
+//                .strafeRight(25)
+//                .build();
+//
+//        TrajectorySequence scoreCloseRightRed = drive.trajectorySequenceBuilder(propCloseRightRed.end())
+//                .forward(34)
+//                .strafeLeft(12)
+//                .build();
+//
+//        TrajectorySequence parkCloseRightRed = drive.trajectorySequenceBuilder(getPixel.end())
+//                .strafeRight(12)
+//                .forward(12)
+//                .build();
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 2) { //If red, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propCloseLeftRed;
+//                score = scoreCloseLeftRed;
+////                park = parkCloseLeftRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//            case "MIDDLE":
+//                prop = propCloseMidRed;
+//                score = scoreCloseMidRed;
+////                park = parkCloseMidRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//
+//                break;
+//            default:
+//                prop = propCloseRightRed;
+//                score = scoreCloseRightRed;
+////                park = parkCloseRightRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propCloseMidRed),
+////                    new TrajectorySequenceCommand(drive, scoreCloseMidRed),
+////                    new TrajectorySequenceCommand(drive, parkCloseMidRed)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+//// /                   s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//                new TrajectorySequenceCommand(drive, prop),
+////                new InstantCommand(() -> {
+////                    s.runToPos(Slides.SlidePos.LOW.position);
+////                })
+//                new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-// /                   s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
-//        ));
-
-        schedule(new SequentialCommandGroup(
-                new TrajectorySequenceCommand(drive, prop),
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, park),
+//                new TrajectorySequenceCommand(drive, intakeIt),
 //                new InstantCommand(() -> {
-//                    s.runToPos(Slides.SlidePos.LOW.position);
-//                })
-                new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, park),
-                new TrajectorySequenceCommand(drive, intakeIt),
-                new InstantCommand(() -> {
-                    i.autoIntake(30);//get better number
-                    new WaitCommand(3000);
-                    i.forceStop();
-                    i.autoIntake(-30);
-                    new WaitCommand(1000);
-                    i.forceStop();
-
-                }),
-                new TrajectorySequenceCommand(drive, andScore),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, parkAfter2)
-
-
-        ));
-    }
-}
+//                    i.autoIntake(30);//get better number
+//                    new WaitCommand(3000);
+//                    i.forceStop();
+//                    i.autoIntake(-30);
+//                    new WaitCommand(1000);
+//                    i.forceStop();
+//
+//                }),
+//                new TrajectorySequenceCommand(drive, andScore),
+//                new InstantCommand(() -> {
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, parkAfter2)
+//
+//
+//        ));
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar.java
index 3556ae4df0f2..2ec9e2786023 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar.java
@@ -1,255 +1,255 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-
-import java.util.concurrent.TimeUnit;
-
-@Autonomous(name="Blue Far",group="Far")
-public class BlueFar extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    int backBoard = 12;
-    Gamepad gamepad;
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseFarBlue = new Pose2d(-35,62, Math.toRadians(-90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-        drive.setPoseEstimate(startPoseFarBlue);
-
-
-        TrajectorySequence propFarMidBlue = drive.trajectorySequenceBuilder(startPoseFarBlue)
-                .lineToLinearHeading(new Pose2d(-35.6, 32, Math.toRadians(-90)))
-                .back(6)
-                .strafeRight(15)
-                .lineToLinearHeading(new Pose2d(-50.6, 14, Math.toRadians(0)))
-                .build();
-
-        TrajectorySequence propFarRightBlue =  drive.trajectorySequenceBuilder(startPoseFarBlue)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(-36.6, 30.5, Math.toRadians(180)))
-                .build();
-
-        TrajectorySequence propFarLeftBlue = drive.trajectorySequenceBuilder(startPoseFarBlue)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(-32.6, 30.5, Math.toRadians(0)))
-                .back(5)
-                .lineToLinearHeading(new Pose2d(-32.6, 30.5, Math.toRadians(0)))
-                .build();
-
-
-        TrajectorySequence scoreFarLeftBlue = drive.trajectorySequenceBuilder(propFarMidBlue.end())
-                .back(3)
-                .lineToLinearHeading(new Pose2d(-34, 14, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(46, 14, Math.toRadians(0)))
-                .strafeLeft(25)
-               .build();
-        TrajectorySequence scoreFarMidBlue = drive.trajectorySequenceBuilder(propFarMidBlue.end())
-                .lineToLinearHeading(new Pose2d(46, 14, Math.toRadians(0)))
-                .strafeLeft(20)
-                .build();
-         TrajectorySequence scoreFarRightBlue = drive.trajectorySequenceBuilder(propFarRightBlue.end())
-                 .back(3)
-                 .lineToLinearHeading(new Pose2d(-34, 14, Math.toRadians(0)))
-                 .lineToLinearHeading(new Pose2d(46, 14, Math.toRadians(0)))
-                 .strafeLeft(14)
-                .build();
-
-        TrajectorySequence parkFarLeftBlue = drive.trajectorySequenceBuilder(scoreFarLeftBlue.end())
-                .strafeRight(25)
-                .forward(14)
-                .build();
-
-        TrajectorySequence parkFarMidBlue = drive.trajectorySequenceBuilder(scoreFarMidBlue.end())
-                .strafeRight(20)
-                .forward(14)
-                .build();
-        TrajectorySequence parkFarRightBlue = drive.trajectorySequenceBuilder(scoreFarMidBlue.end())
-                .strafeRight(14)
-                .forward(14)
-                .build();
-
-//        Splines
-
-        TrajectorySequence leftSplines = drive.trajectorySequenceBuilder(startPoseFarBlue)
-//                Prop
-                .lineToLinearHeading(new Pose2d(-37, 30, Math.toRadians(180)))
-//                Score
-                .lineToLinearHeading(new Pose2d(-34, 14, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(35, 14, Math.toRadians(0)))
-                .splineToConstantHeading(new Vector2d(46, 28), Math.toRadians(0))
-//                Park
-                .strafeRight(5)
-                .splineToConstantHeading(new Vector2d(60, 11), Math.toRadians(0))
-                .build();
-
-        TrajectorySequence rightSplines = drive.trajectorySequenceBuilder(startPoseFarBlue)
-//                Prop
-                .splineToLinearHeading(new Pose2d(-33, -30, Math.toRadians(0)), 0)
-//                Score
-                .back(5)
-                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
-                .splineToConstantHeading(new Vector2d(46, -39), Math.toRadians(0))
-//                Park
-                .strafeLeft(10)
-                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
-                .build();
-
-        TrajectorySequence midSplines = drive.trajectorySequenceBuilder(startPoseFarBlue)
-//                Prop
-                .splineToLinearHeading(new Pose2d(-52, 48, Math.toRadians(-90)), Math.toRadians(-90))
-                .splineToLinearHeading(new Pose2d(-39, 22, Math.toRadians(0)), 0)
-//                Score
-                .back(5)
-                .splineToLinearHeading(new Pose2d(-39, 14, Math.toRadians(0)), 0)
-                .lineToLinearHeading(new Pose2d(35, 14, Math.toRadians(0)))
-                .splineToConstantHeading(new Vector2d(46, 34), Math.toRadians(0))
-//                Park
-                .strafeRight(10)
-                .splineToConstantHeading(new Vector2d(60, 11), Math.toRadians(0))
-                .build();
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 1) { //If blue, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propFarLeftBlue;
-                score = scoreFarLeftBlue;
-                park = parkFarLeftBlue;
-                break;
-            case "MIDDLE":
-                prop = propFarMidBlue;
-                score = scoreFarMidBlue;
-                park = parkFarMidBlue;
-                break;
-            default:
-                prop = propFarRightBlue;
-                score = scoreFarRightBlue;
-                park = parkFarRightBlue;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propFarMidBlue),
-//                    new TrajectorySequenceCommand(drive, scoreFarMidBlue),
-//                    new TrajectorySequenceCommand(drive, parkFarMidBlue)
-//                ),
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.acmerobotics.roadrunner.geometry.Vector2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//@Autonomous(name="Blue Far",group="Far")
+//public class BlueFar extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    int backBoard = 12;
+//    Gamepad gamepad;
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseFarBlue = new Pose2d(-35,62, Math.toRadians(-90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//        drive.setPoseEstimate(startPoseFarBlue);
+//
+//
+//        TrajectorySequence propFarMidBlue = drive.trajectorySequenceBuilder(startPoseFarBlue)
+//                .lineToLinearHeading(new Pose2d(-35.6, 32, Math.toRadians(-90)))
+//                .back(6)
+//                .strafeRight(15)
+//                .lineToLinearHeading(new Pose2d(-50.6, 14, Math.toRadians(0)))
+//                .build();
+//
+//        TrajectorySequence propFarRightBlue =  drive.trajectorySequenceBuilder(startPoseFarBlue)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(-36.6, 30.5, Math.toRadians(180)))
+//                .build();
+//
+//        TrajectorySequence propFarLeftBlue = drive.trajectorySequenceBuilder(startPoseFarBlue)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(-32.6, 30.5, Math.toRadians(0)))
+//                .back(5)
+//                .lineToLinearHeading(new Pose2d(-32.6, 30.5, Math.toRadians(0)))
+//                .build();
+//
+//
+//        TrajectorySequence scoreFarLeftBlue = drive.trajectorySequenceBuilder(propFarMidBlue.end())
+//                .back(3)
+//                .lineToLinearHeading(new Pose2d(-34, 14, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(46, 14, Math.toRadians(0)))
+//                .strafeLeft(25)
+//               .build();
+//        TrajectorySequence scoreFarMidBlue = drive.trajectorySequenceBuilder(propFarMidBlue.end())
+//                .lineToLinearHeading(new Pose2d(46, 14, Math.toRadians(0)))
+//                .strafeLeft(20)
+//                .build();
+//         TrajectorySequence scoreFarRightBlue = drive.trajectorySequenceBuilder(propFarRightBlue.end())
+//                 .back(3)
+//                 .lineToLinearHeading(new Pose2d(-34, 14, Math.toRadians(0)))
+//                 .lineToLinearHeading(new Pose2d(46, 14, Math.toRadians(0)))
+//                 .strafeLeft(14)
+//                .build();
+//
+//        TrajectorySequence parkFarLeftBlue = drive.trajectorySequenceBuilder(scoreFarLeftBlue.end())
+//                .strafeRight(25)
+//                .forward(14)
+//                .build();
+//
+//        TrajectorySequence parkFarMidBlue = drive.trajectorySequenceBuilder(scoreFarMidBlue.end())
+//                .strafeRight(20)
+//                .forward(14)
+//                .build();
+//        TrajectorySequence parkFarRightBlue = drive.trajectorySequenceBuilder(scoreFarMidBlue.end())
+//                .strafeRight(14)
+//                .forward(14)
+//                .build();
+//
+////        Splines
+//
+//        TrajectorySequence leftSplines = drive.trajectorySequenceBuilder(startPoseFarBlue)
+////                Prop
+//                .lineToLinearHeading(new Pose2d(-37, 30, Math.toRadians(180)))
+////                Score
+//                .lineToLinearHeading(new Pose2d(-34, 14, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(35, 14, Math.toRadians(0)))
+//                .splineToConstantHeading(new Vector2d(46, 28), Math.toRadians(0))
+////                Park
+//                .strafeRight(5)
+//                .splineToConstantHeading(new Vector2d(60, 11), Math.toRadians(0))
+//                .build();
+//
+//        TrajectorySequence rightSplines = drive.trajectorySequenceBuilder(startPoseFarBlue)
+////                Prop
+//                .splineToLinearHeading(new Pose2d(-33, -30, Math.toRadians(0)), 0)
+////                Score
+//                .back(5)
+//                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
+//                .splineToConstantHeading(new Vector2d(46, -39), Math.toRadians(0))
+////                Park
+//                .strafeLeft(10)
+//                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
+//                .build();
+//
+//        TrajectorySequence midSplines = drive.trajectorySequenceBuilder(startPoseFarBlue)
+////                Prop
+//                .splineToLinearHeading(new Pose2d(-52, 48, Math.toRadians(-90)), Math.toRadians(-90))
+//                .splineToLinearHeading(new Pose2d(-39, 22, Math.toRadians(0)), 0)
+////                Score
+//                .back(5)
+//                .splineToLinearHeading(new Pose2d(-39, 14, Math.toRadians(0)), 0)
+//                .lineToLinearHeading(new Pose2d(35, 14, Math.toRadians(0)))
+//                .splineToConstantHeading(new Vector2d(46, 34), Math.toRadians(0))
+////                Park
+//                .strafeRight(10)
+//                .splineToConstantHeading(new Vector2d(60, 11), Math.toRadians(0))
+//                .build();
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 1) { //If blue, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propFarLeftBlue;
+//                score = scoreFarLeftBlue;
+//                park = parkFarLeftBlue;
+//                break;
+//            case "MIDDLE":
+//                prop = propFarMidBlue;
+//                score = scoreFarMidBlue;
+//                park = parkFarMidBlue;
+//                break;
+//            default:
+//                prop = propFarRightBlue;
+//                score = scoreFarRightBlue;
+//                park = parkFarRightBlue;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propFarMidBlue),
+////                    new TrajectorySequenceCommand(drive, scoreFarMidBlue),
+////                    new TrajectorySequenceCommand(drive, parkFarMidBlue)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+////                    s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.Close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//              new TrajectorySequenceCommand(drive, prop),
+//              new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-//                    s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.Close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//               new TrajectorySequenceCommand(drive, park)
 //        ));
-
-        schedule(new SequentialCommandGroup(
-              new TrajectorySequenceCommand(drive, prop),
-              new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-               new TrajectorySequenceCommand(drive, park)
-        ));
-    }
-}
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar2plus2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar2plus2.java
index 4e15b629ae54..c5c4814a8c3b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar2plus2.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/BlueFar2plus2.java
@@ -1,275 +1,275 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Intake;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-
-import java.util.concurrent.TimeUnit;
-
-
-@Autonomous(name="Blue Far 2plus2", group="Far")
-public class BlueFar2plus2 extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    Intake i;
-    Gamepad gamepad;
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-    public static Pose2d startPoseFarRed = new Pose2d(-35,-62, Math.toRadians(90));
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence parkAfter2;
-    TrajectorySequence scorePlus;
-    TrajectorySequence intakeIt;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-
-        drive.setPoseEstimate(startPoseFarRed);
-
-        TrajectorySequence propFarMidRed = drive.trajectorySequenceBuilder(startPoseFarRed)
-
-                .lineToLinearHeading(new Pose2d(-35.6, 32, Math.toRadians(-90)))
-                .back(6)
-                .strafeRight(15)
-                .lineToLinearHeading(new Pose2d(-51.6, 7.4, Math.toRadians(0)))
-                .build();
-
-        TrajectorySequence propFarLeftRed =  drive.trajectorySequenceBuilder(startPoseFarRed)
-                .forward(3)
-                .back(3)
-                .lineToLinearHeading(new Pose2d(-36.6, 30.5, Math.toRadians(180)))
-                .build();
-
-        TrajectorySequence propFarRightRed = drive.trajectorySequenceBuilder(startPoseFarRed)
-                .forward(25)
-                .back(3)
-                .lineToLinearHeading(new Pose2d(-32.6, 30.5, Math.toRadians(0)))
-                .build();
-
-
-        TrajectorySequence scoreFarLeftRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
-                .lineToLinearHeading(new Pose2d(47.4, 7.4, Math.toRadians(0)))
-                .strafeLeft(25-3)
-                .build();
-        TrajectorySequence scoreFarMidRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
-                .lineToLinearHeading(new Pose2d(47.4, 7.4, Math.toRadians(0)))
-                .strafeLeft(25)
-                .build();
-        TrajectorySequence scoreFarRightRed = drive.trajectorySequenceBuilder(propFarRightRed.end())
-                .lineToLinearHeading(new Pose2d(47.4, 7.4, Math.toRadians(0)))
-                .strafeLeft(25+3)
-                .build();
-
-//        TrajectorySequence parkFarLeftRed = drive.trajectorySequenceBuilder(scoreFarLeftRed.end())
-//                .back(4)
-//                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
-//                .forward(10)
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Intake;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//
+//@Autonomous(name="Blue Far 2plus2", group="Far")
+//public class BlueFar2plus2 extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    Intake i;
+//    Gamepad gamepad;
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//    public static Pose2d startPoseFarRed = new Pose2d(-35,-62, Math.toRadians(90));
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence parkAfter2;
+//    TrajectorySequence scorePlus;
+//    TrajectorySequence intakeIt;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//
+//        drive.setPoseEstimate(startPoseFarRed);
+//
+//        TrajectorySequence propFarMidRed = drive.trajectorySequenceBuilder(startPoseFarRed)
+//
+//                .lineToLinearHeading(new Pose2d(-35.6, 32, Math.toRadians(-90)))
+//                .back(6)
+//                .strafeRight(15)
+//                .lineToLinearHeading(new Pose2d(-51.6, 7.4, Math.toRadians(0)))
 //                .build();
 //
-//        TrajectorySequence parkFarMidRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-//                .back(4)
-//                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
-//                .forward(10)
+//        TrajectorySequence propFarLeftRed =  drive.trajectorySequenceBuilder(startPoseFarRed)
+//                .forward(3)
+//                .back(3)
+//                .lineToLinearHeading(new Pose2d(-36.6, 30.5, Math.toRadians(180)))
+//                .build();
+//
+//        TrajectorySequence propFarRightRed = drive.trajectorySequenceBuilder(startPoseFarRed)
+//                .forward(25)
+//                .back(3)
+//                .lineToLinearHeading(new Pose2d(-32.6, 30.5, Math.toRadians(0)))
+//                .build();
+//
+//
+//        TrajectorySequence scoreFarLeftRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
+//                .lineToLinearHeading(new Pose2d(47.4, 7.4, Math.toRadians(0)))
+//                .strafeLeft(25-3)
+//                .build();
+//        TrajectorySequence scoreFarMidRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
+//                .lineToLinearHeading(new Pose2d(47.4, 7.4, Math.toRadians(0)))
+//                .strafeLeft(25)
+//                .build();
+//        TrajectorySequence scoreFarRightRed = drive.trajectorySequenceBuilder(propFarRightRed.end())
+//                .lineToLinearHeading(new Pose2d(47.4, 7.4, Math.toRadians(0)))
+//                .strafeLeft(25+3)
 //                .build();
 //
-//        TrajectorySequence parkFarRightRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-//                .back(4)
-//                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////        TrajectorySequence parkFarLeftRed = drive.trajectorySequenceBuilder(scoreFarLeftRed.end())
+////                .back(4)
+////                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////                .forward(10)
+////                .build();
+////
+////        TrajectorySequence parkFarMidRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+////                .back(4)
+////                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////                .forward(10)
+////                .build();
+////
+////        TrajectorySequence parkFarRightRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+////                .back(4)
+////                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////                .forward(10)
+////                .build();
+//        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(180)))
+//                .lineToLinearHeading(new Pose2d(-60, 10.9, Math.toRadians(180)))
+//                .build();
+//        //intake here(code this late)
+//
+//        TrajectorySequence andScore = drive.trajectorySequenceBuilder(getPixel.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(47.2, 29.3, Math.toRadians(0)))
+//                .build();
+//        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 10.9, 0))
 //                .forward(10)
 //                .build();
-        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(180)))
-                .lineToLinearHeading(new Pose2d(-60, 10.9, Math.toRadians(180)))
-                .build();
-        //intake here(code this late)
-
-        TrajectorySequence andScore = drive.trajectorySequenceBuilder(getPixel.end())
-                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(47.2, 29.3, Math.toRadians(0)))
-                .build();
-        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
-                .lineToLinearHeading(new Pose2d(46.5, 10.9, 0))
-                .forward(10)
-                .build();
-
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 2) { //If red, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propFarLeftRed;
-                score = scoreFarLeftRed;
-//                park = parkFarLeftRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-            case "MIDDLE":
-                prop = propFarMidRed;
-                score = scoreFarMidRed;
-//                park = parkFarMidRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-
-                break;
-            default:
-                prop = propFarRightRed;
-                score = scoreFarRightRed;
-//                park = parkFarRightRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propFarMidRed),
-//                    new TrajectorySequenceCommand(drive, scoreFarMidRed),
-//                    new TrajectorySequenceCommand(drive, parkFarMidRed)
-//                ),
+//
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 2) { //If red, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propFarLeftRed;
+//                score = scoreFarLeftRed;
+////                park = parkFarLeftRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//            case "MIDDLE":
+//                prop = propFarMidRed;
+//                score = scoreFarMidRed;
+////                park = parkFarMidRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//
+//                break;
+//            default:
+//                prop = propFarRightRed;
+//                score = scoreFarRightRed;
+////                park = parkFarRightRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propFarMidRed),
+////                    new TrajectorySequenceCommand(drive, scoreFarMidRed),
+////                    new TrajectorySequenceCommand(drive, parkFarMidRed)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+//// /                   s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//                new TrajectorySequenceCommand(drive, prop),
+////                new InstantCommand(() -> {
+////                    s.runToPos(Slides.SlidePos.LOW.position);
+////                })
+//                new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-// /                   s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
-//        ));
-
-        schedule(new SequentialCommandGroup(
-                new TrajectorySequenceCommand(drive, prop),
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, park),
+//                new TrajectorySequenceCommand(drive, intakeIt),
 //                new InstantCommand(() -> {
-//                    s.runToPos(Slides.SlidePos.LOW.position);
-//                })
-                new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, park),
-                new TrajectorySequenceCommand(drive, intakeIt),
-                new InstantCommand(() -> {
-                    i.autoIntake(30);//get better number
-                    new WaitCommand(3000);
-                    i.forceStop();
-                    i.autoIntake(-30);
-                    new WaitCommand(1000);
-                    i.forceStop();
-
-                }),
-                new TrajectorySequenceCommand(drive, andScore),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, parkAfter2)
-
-
-        ));
-    }
-}
+//                    i.autoIntake(30);//get better number
+//                    new WaitCommand(3000);
+//                    i.forceStop();
+//                    i.autoIntake(-30);
+//                    new WaitCommand(1000);
+//                    i.forceStop();
+//
+//                }),
+//                new TrajectorySequenceCommand(drive, andScore),
+//                new InstantCommand(() -> {
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, parkAfter2)
+//
+//
+//        ));
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose.java
index 875d786d9287..02350a6ae47b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose.java
@@ -1,248 +1,248 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-
-import java.util.concurrent.TimeUnit;
-
-
-@Autonomous(name="Red Close", group="Close")
-public class RedClose extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    Gamepad gamepad;
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseCloseRed = new Pose2d(12,-62, Math.toRadians(90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-
-        drive.setPoseEstimate(startPoseCloseRed);
-
-        TrajectorySequence propCloseLeftRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .turn(Math.toRadians(90))
-                .forward(3)
-                .back(3)
-                .lineToLinearHeading(new Pose2d(12, -62, 0))
-                .build();
-        TrajectorySequence scoreCloseLeftRed = drive.trajectorySequenceBuilder(propCloseLeftRed.end())
-                .forward(34)
-                .strafeLeft(30)
-                .build();
-
-        TrajectorySequence parkCloseLeftRed = drive.trajectorySequenceBuilder(scoreCloseLeftRed.end())
-                .strafeRight(30)
-                .forward(12)
-                .build();;
-
-        TrajectorySequence propCloseMidRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(28)
-                .lineToLinearHeading(new Pose2d(12, -62, 0))
-                .build();
-
-        TrajectorySequence scoreCloseMidRed = drive.trajectorySequenceBuilder(propCloseMidRed.end())
-                .forward(34)
-                .strafeLeft(25)
-                .build();
-
-        TrajectorySequence parkCloseMidRed = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
-                .strafeRight(25)
-                .forward(12)
-                .build();
-
-        TrajectorySequence propCloseRightRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .turn(Math.toRadians(-90))
-                .forward(3)
-                .back(3)
-                .strafeRight(25)
-                .build();
-
-        TrajectorySequence scoreCloseRightRed = drive.trajectorySequenceBuilder(propCloseRightRed.end())
-                .forward(34)
-                .strafeLeft(20)
-                .build();
-
-        TrajectorySequence parkCloseRightRed = drive.trajectorySequenceBuilder(scoreCloseRightRed.end())
-                .strafeRight(20)
-                .forward(12)
-                .build();
-
-        // Spline trajectories
-
-        TrajectorySequence LeftSplines = drive.trajectorySequenceBuilder(startPoseCloseRed)
-//                Prop
-                .splineToLinearHeading(new Pose2d(9, -30, Math.toRadians(180)), Math.toRadians(180))
-//                Score
-                .back(5)
-                .splineToSplineHeading(new Pose2d(46, -32, Math.toRadians(0)), 0)
-//                Park
-                .strafeRight(20)
-                .splineToLinearHeading(new Pose2d(60, -62, 0), 0)
-                .build();
-
-        TrajectorySequence RightSplines = drive.trajectorySequenceBuilder(startPoseCloseRed)
-//                Prop
-                .lineToLinearHeading(new Pose2d(13, -37, Math.toRadians(0)))
-//                Score
-                .back(3)
-                .strafeRight(10)
-                .splineToSplineHeading(new Pose2d(46, -42, Math.toRadians(0)), 0)
-//                Park
-                .strafeRight(10)
-                .splineToLinearHeading(new Pose2d(60, -62, 0), 0)
-                .build();
-
-        TrajectorySequence MidSplines = drive.trajectorySequenceBuilder(startPoseCloseRed)
-//                Prop
-                .forward(28)
-//                Score
-                .back(3)
-                .splineToSplineHeading(new Pose2d(46, -37, Math.toRadians(0)), 0)
-//                Park
-                .strafeRight(15)
-                .splineToLinearHeading(new Pose2d(60, -62, 0), 0)
-                .build();
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 2) { //If red, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propCloseLeftRed;
-                score = scoreCloseLeftRed;
-                park = parkCloseLeftRed;
-                break;
-            case "MIDDLE":
-                prop = propCloseMidRed;
-                score = scoreCloseMidRed;
-                park = parkCloseMidRed;
-                break;
-            default:
-                prop = propCloseRightRed;
-                score = scoreCloseRightRed;
-                park = parkCloseRightRed;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propCloseMidRed),
-//                    new TrajectorySequenceCommand(drive, scoreCloseMidRed),
-//                    new TrajectorySequenceCommand(drive, parkCloseMidRed)
-//                ),
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//
+//@Autonomous(name="Red Close", group="Close")
+//public class RedClose extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    Gamepad gamepad;
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseCloseRed = new Pose2d(12,-62, Math.toRadians(90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//
+//        drive.setPoseEstimate(startPoseCloseRed);
+//
+//        TrajectorySequence propCloseLeftRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .turn(Math.toRadians(90))
+//                .forward(3)
+//                .back(3)
+//                .lineToLinearHeading(new Pose2d(12, -62, 0))
+//                .build();
+//        TrajectorySequence scoreCloseLeftRed = drive.trajectorySequenceBuilder(propCloseLeftRed.end())
+//                .forward(34)
+//                .strafeLeft(30)
+//                .build();
+//
+//        TrajectorySequence parkCloseLeftRed = drive.trajectorySequenceBuilder(scoreCloseLeftRed.end())
+//                .strafeRight(30)
+//                .forward(12)
+//                .build();;
+//
+//        TrajectorySequence propCloseMidRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(28)
+//                .lineToLinearHeading(new Pose2d(12, -62, 0))
+//                .build();
+//
+//        TrajectorySequence scoreCloseMidRed = drive.trajectorySequenceBuilder(propCloseMidRed.end())
+//                .forward(34)
+//                .strafeLeft(25)
+//                .build();
+//
+//        TrajectorySequence parkCloseMidRed = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
+//                .strafeRight(25)
+//                .forward(12)
+//                .build();
+//
+//        TrajectorySequence propCloseRightRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .turn(Math.toRadians(-90))
+//                .forward(3)
+//                .back(3)
+//                .strafeRight(25)
+//                .build();
+//
+//        TrajectorySequence scoreCloseRightRed = drive.trajectorySequenceBuilder(propCloseRightRed.end())
+//                .forward(34)
+//                .strafeLeft(20)
+//                .build();
+//
+//        TrajectorySequence parkCloseRightRed = drive.trajectorySequenceBuilder(scoreCloseRightRed.end())
+//                .strafeRight(20)
+//                .forward(12)
+//                .build();
+//
+//        // Spline trajectories
+//
+//        TrajectorySequence LeftSplines = drive.trajectorySequenceBuilder(startPoseCloseRed)
+////                Prop
+//                .splineToLinearHeading(new Pose2d(9, -30, Math.toRadians(180)), Math.toRadians(180))
+////                Score
+//                .back(5)
+//                .splineToSplineHeading(new Pose2d(46, -32, Math.toRadians(0)), 0)
+////                Park
+//                .strafeRight(20)
+//                .splineToLinearHeading(new Pose2d(60, -62, 0), 0)
+//                .build();
+//
+//        TrajectorySequence RightSplines = drive.trajectorySequenceBuilder(startPoseCloseRed)
+////                Prop
+//                .lineToLinearHeading(new Pose2d(13, -37, Math.toRadians(0)))
+////                Score
+//                .back(3)
+//                .strafeRight(10)
+//                .splineToSplineHeading(new Pose2d(46, -42, Math.toRadians(0)), 0)
+////                Park
+//                .strafeRight(10)
+//                .splineToLinearHeading(new Pose2d(60, -62, 0), 0)
+//                .build();
+//
+//        TrajectorySequence MidSplines = drive.trajectorySequenceBuilder(startPoseCloseRed)
+////                Prop
+//                .forward(28)
+////                Score
+//                .back(3)
+//                .splineToSplineHeading(new Pose2d(46, -37, Math.toRadians(0)), 0)
+////                Park
+//                .strafeRight(15)
+//                .splineToLinearHeading(new Pose2d(60, -62, 0), 0)
+//                .build();
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 2) { //If red, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propCloseLeftRed;
+//                score = scoreCloseLeftRed;
+//                park = parkCloseLeftRed;
+//                break;
+//            case "MIDDLE":
+//                prop = propCloseMidRed;
+//                score = scoreCloseMidRed;
+//                park = parkCloseMidRed;
+//                break;
+//            default:
+//                prop = propCloseRightRed;
+//                score = scoreCloseRightRed;
+//                park = parkCloseRightRed;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propCloseMidRed),
+////                    new TrajectorySequenceCommand(drive, scoreCloseMidRed),
+////                    new TrajectorySequenceCommand(drive, parkCloseMidRed)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+////                    s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//                new TrajectorySequenceCommand(drive, prop),
+//                new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-//                    s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, park)
 //        ));
-
-        schedule(new SequentialCommandGroup(
-                new TrajectorySequenceCommand(drive, prop),
-                new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, park)
-        ));
-    }
-}
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose2plus2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose2plus2.java
index 4e85947b25a8..a44d24423858 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose2plus2.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedClose2plus2.java
@@ -1,272 +1,272 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Intake;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-
-import java.util.concurrent.TimeUnit;
-
-
-@Autonomous(name="Red Close 2plus2", group="Close")
-public class RedClose2plus2 extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    Intake i;
-    Gamepad gamepad;
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence parkAfter2;
-    TrajectorySequence scorePlus;
-    TrajectorySequence intakeIt;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        i = new Intake(gamepad1, hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-
-        drive.setPoseEstimate(startPoseCloseRed);
-
-        TrajectorySequence propCloseLeftRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .turn(Math.toRadians(90))
-                .forward(3)
-                .back(3)
-                .lineToLinearHeading(new Pose2d(12, 62, 0))
-                .build();
-        TrajectorySequence scoreCloseLeftRed = drive.trajectorySequenceBuilder(propCloseLeftRed.end())
-                .forward(34)
-                .strafeRight(24)
-                .build();
-
-        TrajectorySequence parkCloseLeftRed = drive.trajectorySequenceBuilder(scoreCloseLeftRed.end())
-                .strafeRight(24)
-                .forward(12)
-                .build();;
-
-        TrajectorySequence propCloseMidRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(12, -62, 0))
-                .build();
-
-        TrajectorySequence scoreCloseMidRed = drive.trajectorySequenceBuilder(propCloseMidRed.end())
-                .forward(34)
-                .strafeRight(25)
-                .build();
-        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
-                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(180)))
-                .lineToLinearHeading(new Pose2d(-60, 10.9, Math.toRadians(180)))
-                .build();
-                //intake here(code this late)
-
-        TrajectorySequence andScore = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
-                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(46.5, 29.3, Math.toRadians(0)))
-                .build();
-        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
-                .lineToLinearHeading(new Pose2d(46.5, 61.9, 0))
-                .forward(10)
-                .build();
-
-        TrajectorySequence parkCloseMidRed = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
-                .strafeRight(25)
-                .forward(12)
-                .build();
-//
-        TrajectorySequence propCloseRightRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
-                .forward(25)
-                .turn(Math.toRadians(-90))
-                .forward(11)
-                .back(11)
-                .strafeRight(25)
-                .build();
-
-        TrajectorySequence scoreCloseRightRed = drive.trajectorySequenceBuilder(propCloseRightRed.end())
-                .forward(34)
-                .strafeLeft(12)
-                .build();
-
-        TrajectorySequence parkCloseRightRed = drive.trajectorySequenceBuilder(scoreCloseRightRed.end())
-                .strafeRight(12)
-                .forward(12)
-                .build();
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 2) { //If red, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propCloseLeftRed;
-                score = scoreCloseLeftRed;
-//                park = parkCloseLeftRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-            case "MIDDLE":
-                prop = propCloseMidRed;
-                score = scoreCloseMidRed;
-//                park = parkCloseMidRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-
-                break;
-            default:
-                prop = propCloseRightRed;
-                score = scoreCloseRightRed;
-//                park = parkCloseRightRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propCloseMidRed),
-//                    new TrajectorySequenceCommand(drive, scoreCloseMidRed),
-//                    new TrajectorySequenceCommand(drive, parkCloseMidRed)
-//                ),
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Intake;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//
+//@Autonomous(name="Red Close 2plus2", group="Close")
+//public class RedClose2plus2 extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    Intake i;
+//    Gamepad gamepad;
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence parkAfter2;
+//    TrajectorySequence scorePlus;
+//    TrajectorySequence intakeIt;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        i = new Intake(gamepad1, hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//
+//        drive.setPoseEstimate(startPoseCloseRed);
+//
+//        TrajectorySequence propCloseLeftRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .turn(Math.toRadians(90))
+//                .forward(3)
+//                .back(3)
+//                .lineToLinearHeading(new Pose2d(12, 62, 0))
+//                .build();
+//        TrajectorySequence scoreCloseLeftRed = drive.trajectorySequenceBuilder(propCloseLeftRed.end())
+//                .forward(34)
+//                .strafeRight(24)
+//                .build();
+//
+//        TrajectorySequence parkCloseLeftRed = drive.trajectorySequenceBuilder(scoreCloseLeftRed.end())
+//                .strafeRight(24)
+//                .forward(12)
+//                .build();;
+//
+//        TrajectorySequence propCloseMidRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(12, -62, 0))
+//                .build();
+//
+//        TrajectorySequence scoreCloseMidRed = drive.trajectorySequenceBuilder(propCloseMidRed.end())
+//                .forward(34)
+//                .strafeRight(25)
+//                .build();
+//        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(180)))
+//                .lineToLinearHeading(new Pose2d(-60, 10.9, Math.toRadians(180)))
+//                .build();
+//                //intake here(code this late)
+//
+//        TrajectorySequence andScore = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 10.9, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(46.5, 29.3, Math.toRadians(0)))
+//                .build();
+//        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
+//                .lineToLinearHeading(new Pose2d(46.5, 61.9, 0))
+//                .forward(10)
+//                .build();
+//
+//        TrajectorySequence parkCloseMidRed = drive.trajectorySequenceBuilder(scoreCloseMidRed.end())
+//                .strafeRight(25)
+//                .forward(12)
+//                .build();
+////
+//        TrajectorySequence propCloseRightRed = drive.trajectorySequenceBuilder(startPoseCloseRed)
+//                .forward(25)
+//                .turn(Math.toRadians(-90))
+//                .forward(11)
+//                .back(11)
+//                .strafeRight(25)
+//                .build();
+//
+//        TrajectorySequence scoreCloseRightRed = drive.trajectorySequenceBuilder(propCloseRightRed.end())
+//                .forward(34)
+//                .strafeLeft(12)
+//                .build();
+//
+//        TrajectorySequence parkCloseRightRed = drive.trajectorySequenceBuilder(scoreCloseRightRed.end())
+//                .strafeRight(12)
+//                .forward(12)
+//                .build();
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 2) { //If red, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propCloseLeftRed;
+//                score = scoreCloseLeftRed;
+////                park = parkCloseLeftRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//            case "MIDDLE":
+//                prop = propCloseMidRed;
+//                score = scoreCloseMidRed;
+////                park = parkCloseMidRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//
+//                break;
+//            default:
+//                prop = propCloseRightRed;
+//                score = scoreCloseRightRed;
+////                park = parkCloseRightRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propCloseMidRed),
+////                    new TrajectorySequenceCommand(drive, scoreCloseMidRed),
+////                    new TrajectorySequenceCommand(drive, parkCloseMidRed)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+//// /                   s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//                new TrajectorySequenceCommand(drive, prop),
+////                new InstantCommand(() -> {
+////                    s.runToPos(Slides.SlidePos.LOW.position);
+////                })
+//                new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-// /                   s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
-//        ));
-
-        schedule(new SequentialCommandGroup(
-                new TrajectorySequenceCommand(drive, prop),
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, park),
+//                new TrajectorySequenceCommand(drive, intakeIt),
 //                new InstantCommand(() -> {
-//                    s.runToPos(Slides.SlidePos.LOW.position);
-//                })
-                new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, park),
-                new TrajectorySequenceCommand(drive, intakeIt),
-                new InstantCommand(() -> {
-                    i.autoIntake(30);//get better number
-                    new WaitCommand(3000);
-                    i.forceStop();
-                    i.autoIntake(-30);
-                    new WaitCommand(1000);
-                    i.forceStop();
-
-                }),
-                new TrajectorySequenceCommand(drive, andScore),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, parkAfter2)
-
-
-        ));
-    }
-}
+//                    i.autoIntake(30);//get better number
+//                    new WaitCommand(3000);
+//                    i.forceStop();
+//                    i.autoIntake(-30);
+//                    new WaitCommand(1000);
+//                    i.forceStop();
+//
+//                }),
+//                new TrajectorySequenceCommand(drive, andScore),
+//                new InstantCommand(() -> {
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, parkAfter2)
+//
+//
+//        ));
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar.java
index 1fea655836e4..a855fee5c02b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar.java
@@ -1,255 +1,255 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-
-import java.util.concurrent.TimeUnit;
-
-@Autonomous(name="Red Far",group="Far")
-public class RedFar extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    int backBoard = 12;
-    Gamepad gamepad;
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseFarRed = new Pose2d(-35,-62, Math.toRadians(90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-
-        drive.setPoseEstimate(startPoseFarRed);
-
-        TrajectorySequence propFarMidRed = drive.trajectorySequenceBuilder(startPoseFarRed)
-                .lineToLinearHeading(new Pose2d(-35.6, -32, Math.toRadians(90)))
-                .back(6)
-                .strafeLeft(15)
-                .lineToLinearHeading(new Pose2d(-50.6, -14, Math.toRadians(0)))
-                .build();
-
-        TrajectorySequence propFarLeftRed =  drive.trajectorySequenceBuilder(startPoseFarRed)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(-36.6, -30.5, Math.toRadians(180)))
-                .build();
-
-        TrajectorySequence propFarRightRed = drive.trajectorySequenceBuilder(startPoseFarRed)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(-32.6, -30.5, Math.toRadians(0)))
-                .build();
-
-
-        TrajectorySequence scoreFarLeftRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
-                .back(3)
-                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(46, -14, Math.toRadians(0)))
-                .strafeRight(14)
-                .build();
-        TrajectorySequence scoreFarMidRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
-                .lineToLinearHeading(new Pose2d(46, -14, Math.toRadians(0)))
-                .strafeRight(20)
-                .build();
-         TrajectorySequence scoreFarRightRed = drive.trajectorySequenceBuilder(propFarRightRed.end())
-                 .back(3)
-                 .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
-                 .lineToLinearHeading(new Pose2d(46, -14, Math.toRadians(0)))
-                 .strafeRight(25)
-                 .build();
-
-        TrajectorySequence parkFarLeftRed = drive.trajectorySequenceBuilder(scoreFarLeftRed.end())
-                .strafeLeft(14)
-                .forward(14)
-                .build();
-
-        TrajectorySequence parkFarMidRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-                .strafeLeft(20)
-                .forward(14)
-                .build();
-
-        TrajectorySequence parkFarRightRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-                .strafeLeft(25)
-                .forward(14)
-                .build();
-
-//        Splines
-
-        TrajectorySequence leftSplines = drive.trajectorySequenceBuilder(startPoseFarRed)
-//                Prop
-                .lineToLinearHeading(new Pose2d(-37, -30, Math.toRadians(180)))
-//                Score
-                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
-                .splineToConstantHeading(new Vector2d(46, -28), Math.toRadians(0))
-//                Park
-                .strafeLeft(5)
-                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
-                .build();
-
-        TrajectorySequence rightSplines = drive.trajectorySequenceBuilder(startPoseFarRed)
-//                Prop
-                .splineToLinearHeading(new Pose2d(-34, -30, Math.toRadians(0)), 0)
-//                Score
-                .back(5)
-                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
-                .splineToConstantHeading(new Vector2d(46, -39), Math.toRadians(0))
-//                Park
-                .strafeLeft(10)
-                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
-                .build();
-
-        TrajectorySequence midSplines = drive.trajectorySequenceBuilder(startPoseFarRed)
-//                Prop
-                .splineToLinearHeading(new Pose2d(-52, -48, Math.toRadians(90)), Math.toRadians(90))
-                .splineToLinearHeading(new Pose2d(-39, -28, Math.toRadians(0)), 0)
-//                Score
-                .back(5)
-                .splineToLinearHeading(new Pose2d(-39, -14, Math.toRadians(0)), 0)
-                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
-                .splineToConstantHeading(new Vector2d(46, -34), Math.toRadians(0))
-//                Park
-                .strafeLeft(10)
-                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
-                .build();
-
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 2) { //If red, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propFarLeftRed;
-                score = scoreFarLeftRed;
-                park = parkFarLeftRed;
-                break;
-            case "MIDDLE":
-                prop = propFarMidRed;
-                score = scoreFarMidRed;
-                park = parkFarMidRed;
-                break;
-            default:
-                prop = propFarRightRed;
-                score = scoreFarRightRed;
-                park = parkFarRightRed;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propFarMidRed),
-//                    new TrajectorySequenceCommand(drive, scoreFarMidRed),
-//                    new TrajectorySequenceCommand(drive, parkFarMidRed)
-//                ),
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.acmerobotics.roadrunner.geometry.Vector2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//@Autonomous(name="Red Far",group="Far")
+//public class RedFar extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    int backBoard = 12;
+//    Gamepad gamepad;
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseFarRed = new Pose2d(-35,-62, Math.toRadians(90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//
+//        drive.setPoseEstimate(startPoseFarRed);
+//
+//        TrajectorySequence propFarMidRed = drive.trajectorySequenceBuilder(startPoseFarRed)
+//                .lineToLinearHeading(new Pose2d(-35.6, -32, Math.toRadians(90)))
+//                .back(6)
+//                .strafeLeft(15)
+//                .lineToLinearHeading(new Pose2d(-50.6, -14, Math.toRadians(0)))
+//                .build();
+//
+//        TrajectorySequence propFarLeftRed =  drive.trajectorySequenceBuilder(startPoseFarRed)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(-36.6, -30.5, Math.toRadians(180)))
+//                .build();
+//
+//        TrajectorySequence propFarRightRed = drive.trajectorySequenceBuilder(startPoseFarRed)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(-32.6, -30.5, Math.toRadians(0)))
+//                .build();
+//
+//
+//        TrajectorySequence scoreFarLeftRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
+//                .back(3)
+//                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(46, -14, Math.toRadians(0)))
+//                .strafeRight(14)
+//                .build();
+//        TrajectorySequence scoreFarMidRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
+//                .lineToLinearHeading(new Pose2d(46, -14, Math.toRadians(0)))
+//                .strafeRight(20)
+//                .build();
+//         TrajectorySequence scoreFarRightRed = drive.trajectorySequenceBuilder(propFarRightRed.end())
+//                 .back(3)
+//                 .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
+//                 .lineToLinearHeading(new Pose2d(46, -14, Math.toRadians(0)))
+//                 .strafeRight(25)
+//                 .build();
+//
+//        TrajectorySequence parkFarLeftRed = drive.trajectorySequenceBuilder(scoreFarLeftRed.end())
+//                .strafeLeft(14)
+//                .forward(14)
+//                .build();
+//
+//        TrajectorySequence parkFarMidRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+//                .strafeLeft(20)
+//                .forward(14)
+//                .build();
+//
+//        TrajectorySequence parkFarRightRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+//                .strafeLeft(25)
+//                .forward(14)
+//                .build();
+//
+////        Splines
+//
+//        TrajectorySequence leftSplines = drive.trajectorySequenceBuilder(startPoseFarRed)
+////                Prop
+//                .lineToLinearHeading(new Pose2d(-37, -30, Math.toRadians(180)))
+////                Score
+//                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
+//                .splineToConstantHeading(new Vector2d(46, -28), Math.toRadians(0))
+////                Park
+//                .strafeLeft(5)
+//                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
+//                .build();
+//
+//        TrajectorySequence rightSplines = drive.trajectorySequenceBuilder(startPoseFarRed)
+////                Prop
+//                .splineToLinearHeading(new Pose2d(-34, -30, Math.toRadians(0)), 0)
+////                Score
+//                .back(5)
+//                .lineToLinearHeading(new Pose2d(-34, -14, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
+//                .splineToConstantHeading(new Vector2d(46, -39), Math.toRadians(0))
+////                Park
+//                .strafeLeft(10)
+//                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
+//                .build();
+//
+//        TrajectorySequence midSplines = drive.trajectorySequenceBuilder(startPoseFarRed)
+////                Prop
+//                .splineToLinearHeading(new Pose2d(-52, -48, Math.toRadians(90)), Math.toRadians(90))
+//                .splineToLinearHeading(new Pose2d(-39, -28, Math.toRadians(0)), 0)
+////                Score
+//                .back(5)
+//                .splineToLinearHeading(new Pose2d(-39, -14, Math.toRadians(0)), 0)
+//                .lineToLinearHeading(new Pose2d(35, -14, Math.toRadians(0)))
+//                .splineToConstantHeading(new Vector2d(46, -34), Math.toRadians(0))
+////                Park
+//                .strafeLeft(10)
+//                .splineToConstantHeading(new Vector2d(60, -11), Math.toRadians(0))
+//                .build();
+//
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 2) { //If red, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propFarLeftRed;
+//                score = scoreFarLeftRed;
+//                park = parkFarLeftRed;
+//                break;
+//            case "MIDDLE":
+//                prop = propFarMidRed;
+//                score = scoreFarMidRed;
+//                park = parkFarMidRed;
+//                break;
+//            default:
+//                prop = propFarRightRed;
+//                score = scoreFarRightRed;
+//                park = parkFarRightRed;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propFarMidRed),
+////                    new TrajectorySequenceCommand(drive, scoreFarMidRed),
+////                    new TrajectorySequenceCommand(drive, parkFarMidRed)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+////                    s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.Close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//              new TrajectorySequenceCommand(drive, prop),
+//              new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-//                    s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.Close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//               new TrajectorySequenceCommand(drive, park)
 //        ));
-
-        schedule(new SequentialCommandGroup(
-              new TrajectorySequenceCommand(drive, prop),
-              new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-               new TrajectorySequenceCommand(drive, park)
-        ));
-    }
-}
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar2plus2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar2plus2.java
index aab71abb65b2..07cd8652435f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar2plus2.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RedFar2plus2.java
@@ -1,273 +1,273 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.command.SequentialCommandGroup;
-import com.arcrobotics.ftclib.command.WaitCommand;
-import com.qualcomm.hardware.dfrobot.HuskyLens;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Intake;
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-
-import java.util.concurrent.TimeUnit;
-
-
-@Autonomous(name="Red Far 2plus2", group="Far")
-public class RedFar2plus2 extends CommandOpMode {
-    //Add Motors and servos not for drivebase here
-    SampleMecanumDrive drive;
-    Slides s;
-    Outtake o;
-    V4B v4b;
-    Intake i;
-    Gamepad gamepad;
-    private final int READ_PERIOD = 1;
-    private HuskyLens huskyLens;
-    public static Pose2d startPoseFarRed = new Pose2d(-35,-62, Math.toRadians(90));
-
-    TrajectorySequence prop;
-    TrajectorySequence score;
-    TrajectorySequence parkAfter2;
-    TrajectorySequence scorePlus;
-    TrajectorySequence intakeIt;
-    TrajectorySequence park;
-
-    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
-    String section;
-
-    @Override
-    public void initialize() {
-        drive = new SampleMecanumDrive(hardwareMap);
-        s = new Slides(hardwareMap);
-        o = new Outtake(gamepad, hardwareMap);
-        v4b = new V4B(hardwareMap);
-        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
-
-        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
-        rateLimit.expire();
-
-        if (!huskyLens.knock()) {
-            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
-            telemetry.update();
-        }
-        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
-
-        drive.setPoseEstimate(startPoseFarRed);
-
-        TrajectorySequence propFarMidRed = drive.trajectorySequenceBuilder(startPoseFarRed)
-
-                .lineToLinearHeading(new Pose2d(-35.6, -32, Math.toRadians(90)))
-                .back(6)
-                .strafeLeft(15)
-                .lineToLinearHeading(new Pose2d(-51.6, -7.4, Math.toRadians(0)))
-                .build();
-
-        TrajectorySequence propFarLeftRed =  drive.trajectorySequenceBuilder(startPoseFarRed)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(-36.6, -30.5, Math.toRadians(180)))
-                .build();
-
-        TrajectorySequence propFarRightRed = drive.trajectorySequenceBuilder(startPoseFarRed)
-                .forward(25)
-                .lineToLinearHeading(new Pose2d(-32.6, -30.5, Math.toRadians(0)))
-                .build();
-
-
-        TrajectorySequence scoreFarLeftRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
-                .lineToLinearHeading(new Pose2d(47.4, -7.4, Math.toRadians(0)))
-                .strafeRight(25-3)
-                .build();
-        TrajectorySequence scoreFarMidRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
-                .lineToLinearHeading(new Pose2d(47.4, -7.4, Math.toRadians(0)))
-                .strafeRight(25)
-                .build();
-        TrajectorySequence scoreFarRightRed = drive.trajectorySequenceBuilder(propFarRightRed.end())
-                .lineToLinearHeading(new Pose2d(47.4, -7.4, Math.toRadians(0)))
-                .strafeRight(25+3)
-                .build();
-
-//        TrajectorySequence parkFarLeftRed = drive.trajectorySequenceBuilder(scoreFarLeftRed.end())
-//                .back(4)
-//                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
-//                .forward(10)
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.arcrobotics.ftclib.command.CommandOpMode;
+//import com.arcrobotics.ftclib.command.InstantCommand;
+//import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+//import com.arcrobotics.ftclib.command.WaitCommand;
+//import com.qualcomm.hardware.dfrobot.HuskyLens;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.subsystems.Intake;
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.Slides;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//
+//import java.util.concurrent.TimeUnit;
+//
+//
+//@Autonomous(name="Red Far 2plus2", group="Far")
+//public class RedFar2plus2 extends CommandOpMode {
+//    //Add Motors and servos not for drivebase here
+//    SampleMecanumDrive drive;
+//    Slides s;
+//    Outtake o;
+//    V4B v4b;
+//    Intake i;
+//    Gamepad gamepad;
+//    private final int READ_PERIOD = 1;
+//    private HuskyLens huskyLens;
+//    public static Pose2d startPoseFarRed = new Pose2d(-35,-62, Math.toRadians(90));
+//
+//    TrajectorySequence prop;
+//    TrajectorySequence score;
+//    TrajectorySequence parkAfter2;
+//    TrajectorySequence scorePlus;
+//    TrajectorySequence intakeIt;
+//    TrajectorySequence park;
+//
+//    public static Pose2d startPoseCloseRed = new Pose2d(12,62, Math.toRadians(-90));
+//    String section;
+//
+//    @Override
+//    public void initialize() {
+//        drive = new SampleMecanumDrive(hardwareMap);
+//        s = new Slides(hardwareMap);
+//        o = new Outtake(gamepad, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//        huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+//
+//        Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+//        rateLimit.expire();
+//
+//        if (!huskyLens.knock()) {
+//            telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+//            telemetry.update();
+//        }
+//        huskyLens.selectAlgorithm(HuskyLens.Algorithm.COLOR_RECOGNITION);
+//
+//        drive.setPoseEstimate(startPoseFarRed);
+//
+//        TrajectorySequence propFarMidRed = drive.trajectorySequenceBuilder(startPoseFarRed)
+//
+//                .lineToLinearHeading(new Pose2d(-35.6, -32, Math.toRadians(90)))
+//                .back(6)
+//                .strafeLeft(15)
+//                .lineToLinearHeading(new Pose2d(-51.6, -7.4, Math.toRadians(0)))
 //                .build();
 //
-//        TrajectorySequence parkFarMidRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-//                .back(4)
-//                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
-//                .forward(10)
+//        TrajectorySequence propFarLeftRed =  drive.trajectorySequenceBuilder(startPoseFarRed)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(-36.6, -30.5, Math.toRadians(180)))
+//                .build();
+//
+//        TrajectorySequence propFarRightRed = drive.trajectorySequenceBuilder(startPoseFarRed)
+//                .forward(25)
+//                .lineToLinearHeading(new Pose2d(-32.6, -30.5, Math.toRadians(0)))
+//                .build();
+//
+//
+//        TrajectorySequence scoreFarLeftRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
+//                .lineToLinearHeading(new Pose2d(47.4, -7.4, Math.toRadians(0)))
+//                .strafeRight(25-3)
+//                .build();
+//        TrajectorySequence scoreFarMidRed = drive.trajectorySequenceBuilder(propFarMidRed.end())
+//                .lineToLinearHeading(new Pose2d(47.4, -7.4, Math.toRadians(0)))
+//                .strafeRight(25)
+//                .build();
+//        TrajectorySequence scoreFarRightRed = drive.trajectorySequenceBuilder(propFarRightRed.end())
+//                .lineToLinearHeading(new Pose2d(47.4, -7.4, Math.toRadians(0)))
+//                .strafeRight(25+3)
 //                .build();
 //
-//        TrajectorySequence parkFarRightRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-//                .back(4)
-//                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////        TrajectorySequence parkFarLeftRed = drive.trajectorySequenceBuilder(scoreFarLeftRed.end())
+////                .back(4)
+////                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////                .forward(10)
+////                .build();
+////
+////        TrajectorySequence parkFarMidRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+////                .back(4)
+////                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////                .forward(10)
+////                .build();
+////
+////        TrajectorySequence parkFarRightRed = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+////                .back(4)
+////                .lineToLinearHeading(new Pose2d(46.0, -11.6, Math.toRadians(0)))
+////                .forward(10)
+////                .build();
+//        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
+//                .lineToLinearHeading(new Pose2d(46.5, -10.9, Math.toRadians(180)))
+//                .lineToLinearHeading(new Pose2d(-60, -10.9, Math.toRadians(180)))
+//                .build();
+//        //intake here(code this late)
+//
+//        TrajectorySequence andScore = drive.trajectorySequenceBuilder(getPixel.end())
+//                .lineToLinearHeading(new Pose2d(46.5, -10.9, Math.toRadians(0)))
+//                .lineToLinearHeading(new Pose2d(47.2, -29.3, Math.toRadians(0)))
+//                .build();
+//        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
+//                .lineToLinearHeading(new Pose2d(46.5, -10.9, 0))
 //                .forward(10)
 //                .build();
-        TrajectorySequence getPixel = drive.trajectorySequenceBuilder(scoreFarMidRed.end())
-                .lineToLinearHeading(new Pose2d(46.5, -10.9, Math.toRadians(180)))
-                .lineToLinearHeading(new Pose2d(-60, -10.9, Math.toRadians(180)))
-                .build();
-        //intake here(code this late)
-
-        TrajectorySequence andScore = drive.trajectorySequenceBuilder(getPixel.end())
-                .lineToLinearHeading(new Pose2d(46.5, -10.9, Math.toRadians(0)))
-                .lineToLinearHeading(new Pose2d(47.2, -29.3, Math.toRadians(0)))
-                .build();
-        TrajectorySequence parkFromScore = drive.trajectorySequenceBuilder(andScore.end())
-                .lineToLinearHeading(new Pose2d(46.5, -10.9, 0))
-                .forward(10)
-                .build();
-
-
-        while (opModeInInit()) {
-            if (!rateLimit.hasExpired()) {
-                continue;
-            }
-            rateLimit.reset();
-
-            HuskyLens.Block[] blocks = huskyLens.blocks();
-
-            if (blocks.length > 0) {
-                telemetry.addData("Objects Detected: ", blocks.length);
-                for (int i = 0; i < blocks.length; i++) {
-                    if (blocks[i].id == 2) { //If red, then detect
-                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
-
-                        // Determine which section the object is in based on its X-coordinate
-                        int xCoordinate = blocks[i].x;
-                        int yCoordinate = blocks[i].y;
-
-                        // Calculate section boundaries
-                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
-                        int cropHeight = 50;
-
-                        // Determine the section
-                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
-                            section = "LEFT";
-                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
-                            section = "MIDDLE";
-                        } else {
-                            section = "RIGHT";
-                        }
-                        telemetry.addData("Object Section: ", section);
-                    } else { //No objects detected or only red objects detected
-                        section = "RIGHT";
-                        telemetry.addLine("Default - RIGHT");
-                    }
-                }
-            }
-            telemetry.update();
-        }
-
-        telemetry.addLine("START - Section is " + section);
-        telemetry.update();
-
-        switch(section) {
-            case "LEFT":
-                prop = propFarLeftRed;
-                score = scoreFarLeftRed;
-//                park = parkFarLeftRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-            case "MIDDLE":
-                prop = propFarMidRed;
-                score = scoreFarMidRed;
-//                park = parkFarMidRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-
-                break;
-            default:
-                prop = propFarRightRed;
-                score = scoreFarRightRed;
-//                park = parkFarRightRed;
-                intakeIt = getPixel;
-                scorePlus = andScore;
-                parkAfter2 = parkFromScore;
-                break;
-        }
-
-//        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
-//            new ParallelCommandGroup(
-//                new SequentialCommandGroup(
-//                    new TrajectorySequenceCommand(drive, propFarMidRed),
-//                    new TrajectorySequenceCommand(drive, scoreFarMidRed),
-//                    new TrajectorySequenceCommand(drive, parkFarMidRed)
-//                ),
+//
+//
+//        while (opModeInInit()) {
+//            if (!rateLimit.hasExpired()) {
+//                continue;
+//            }
+//            rateLimit.reset();
+//
+//            HuskyLens.Block[] blocks = huskyLens.blocks();
+//
+//            if (blocks.length > 0) {
+//                telemetry.addData("Objects Detected: ", blocks.length);
+//                for (int i = 0; i < blocks.length; i++) {
+//                    if (blocks[i].id == 2) { //If red, then detect
+//                        telemetry.addData("Object " + (i + 1) + ": ", blocks[i].toString());
+//
+//                        // Determine which section the object is in based on its X-coordinate
+//                        int xCoordinate = blocks[i].x;
+//                        int yCoordinate = blocks[i].y;
+//
+//                        // Calculate section boundaries
+//                        int sectionWidth = 320 / 3; // Because horizontal screen resolution is 320
+//                        int cropHeight = 50;
+//
+//                        // Determine the section
+//                        if (xCoordinate < sectionWidth && yCoordinate > cropHeight) {
+//                            section = "LEFT";
+//                        } else if (xCoordinate < 2 * sectionWidth && yCoordinate > cropHeight) {
+//                            section = "MIDDLE";
+//                        } else {
+//                            section = "RIGHT";
+//                        }
+//                        telemetry.addData("Object Section: ", section);
+//                    } else { //No objects detected or only red objects detected
+//                        section = "RIGHT";
+//                        telemetry.addLine("Default - RIGHT");
+//                    }
+//                }
+//            }
+//            telemetry.update();
+//        }
+//
+//        telemetry.addLine("START - Section is " + section);
+//        telemetry.update();
+//
+//        switch(section) {
+//            case "LEFT":
+//                prop = propFarLeftRed;
+//                score = scoreFarLeftRed;
+////                park = parkFarLeftRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//            case "MIDDLE":
+//                prop = propFarMidRed;
+//                score = scoreFarMidRed;
+////                park = parkFarMidRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//
+//                break;
+//            default:
+//                prop = propFarRightRed;
+//                score = scoreFarRightRed;
+////                park = parkFarRightRed;
+//                intakeIt = getPixel;
+//                scorePlus = andScore;
+//                parkAfter2 = parkFromScore;
+//                break;
+//        }
+//
+////        schedule(new SequentialCommandGroup ( //Makes the following code run one after another, like norma
+////            new ParallelCommandGroup(
+////                new SequentialCommandGroup(
+////                    new TrajectorySequenceCommand(drive, propFarMidRed),
+////                    new TrajectorySequenceCommand(drive, scoreFarMidRed),
+////                    new TrajectorySequenceCommand(drive, parkFarMidRed)
+////                ),
+////                new InstantCommand(() -> {
+////                    new WaitCommand(4000);
+//// /                   s.goToPosition(Slides.SlidePos.LOW);
+//////                    new WaitCommand(2000);
+//////                    v4b.togglePower();
+//////                    new WaitCommand(2000);
+//////                    o.open();
+//////                    new WaitCommand(2000);
+//////                    o.close();
+//////                    v4b.togglePower();
+//////                    new WaitCommand(1000);
+//////                    s.goToPosition(Slides.SlidePos.DOWN);
+////                })
+////            )
+////        ));
+//
+//        schedule(new SequentialCommandGroup(
+//                new TrajectorySequenceCommand(drive, prop),
+////                new InstantCommand(() -> {
+////                    s.runToPos(Slides.SlidePos.LOW.position);
+////                })
+//                new TrajectorySequenceCommand(drive, score),
 //                new InstantCommand(() -> {
-//                    new WaitCommand(4000);
-// /                   s.goToPosition(Slides.SlidePos.LOW);
-////                    new WaitCommand(2000);
-////                    v4b.togglePower();
-////                    new WaitCommand(2000);
-////                    o.open();
-////                    new WaitCommand(2000);
-////                    o.close();
-////                    v4b.togglePower();
-////                    new WaitCommand(1000);
-////                    s.goToPosition(Slides.SlidePos.DOWN);
-//                })
-//            )
-//        ));
-
-        schedule(new SequentialCommandGroup(
-                new TrajectorySequenceCommand(drive, prop),
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, park),
+//                new TrajectorySequenceCommand(drive, intakeIt),
 //                new InstantCommand(() -> {
-//                    s.runToPos(Slides.SlidePos.LOW.position);
-//                })
-                new TrajectorySequenceCommand(drive, score),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, park),
-                new TrajectorySequenceCommand(drive, intakeIt),
-                new InstantCommand(() -> {
-                    i.autoIntake(30);//get better number
-                    new WaitCommand(3000);
-                    i.forceStop();
-                    i.autoIntake(-30);
-                    new WaitCommand(1000);
-                    i.forceStop();
-
-                }),
-                new TrajectorySequenceCommand(drive, andScore),
-                new InstantCommand(() -> {
-                    new WaitCommand(20);
-                    s.goToPosition(70);
-                    new WaitCommand(200);
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    o.open();
-                    new WaitCommand(400);
-                    o.close();
-                    v4b.togglePower();
-                    new WaitCommand(200);
-                    s.goToPosition(50);
-                }),
-                new TrajectorySequenceCommand(drive, parkAfter2)
-
-
-        ));
-    }
-}
+//                    i.autoIntake(30);//get better number
+//                    new WaitCommand(3000);
+//                    i.forceStop();
+//                    i.autoIntake(-30);
+//                    new WaitCommand(1000);
+//                    i.forceStop();
+//
+//                }),
+//                new TrajectorySequenceCommand(drive, andScore),
+//                new InstantCommand(() -> {
+//                    new WaitCommand(20);
+//                    s.goToPosition(70);
+//                    new WaitCommand(200);
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    o.open();
+//                    new WaitCommand(400);
+//                    o.close();
+//                    v4b.togglePower();
+//                    new WaitCommand(200);
+//                    s.goToPosition(50);
+//                }),
+//                new TrajectorySequenceCommand(drive, parkAfter2)
+//
+//
+//        ));
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/TrajectorySequenceCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/TrajectorySequenceCommand.java
index c37d4ed80d4e..35c0669b425b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/TrajectorySequenceCommand.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/TrajectorySequenceCommand.java
@@ -1,32 +1,32 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.arcrobotics.ftclib.command.CommandBase;
-
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
-
-public class TrajectorySequenceCommand extends CommandBase {
-
-    private SampleMecanumDrive drive;
-    private TrajectorySequence trajectory;
-
-    public TrajectorySequenceCommand(SampleMecanumDrive drive, TrajectorySequence trajectory){
-        this.drive = drive;
-        this.trajectory = trajectory;
-    }
-
-    @Override
-    public void initialize(){
-        drive.followTrajectorySequenceAsync(trajectory);
-    }
-
-    @Override
-    public void execute(){
-        drive.update();
-    }
-
-    @Override
-    public boolean isFinished(){
-        return !drive.isBusy();
-    }
-}
\ No newline at end of file
+//package org.firstinspires.ftc.teamcode.auto;
+//
+//import com.arcrobotics.ftclib.command.CommandBase;
+//
+//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+//import org.firstinspires.ftc.teamcode.vision.trajectorysequence.TrajectorySequence;
+//
+//public class TrajectorySequenceCommand extends CommandBase {
+//
+//    private SampleMecanumDrive drive;
+//    private TrajectorySequence trajectory;
+//
+//    public TrajectorySequenceCommand(SampleMecanumDrive drive, TrajectorySequence trajectory){
+//        this.drive = drive;
+//        this.trajectory = trajectory;
+//    }
+//
+//    @Override
+//    public void initialize(){
+//        drive.followTrajectorySequenceAsync(trajectory);
+//    }
+//
+//    @Override
+//    public void execute(){
+//        drive.update();
+//    }
+//
+//    @Override
+//    public boolean isFinished(){
+//        return !drive.isBusy();
+//    }
+//}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AarushsIntakeCommandEx.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AarushsIntakeCommandEx.java
new file mode 100644
index 000000000000..ccb44a1ad8a2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AarushsIntakeCommandEx.java
@@ -0,0 +1,97 @@
+//package org.firstinspires.ftc.teamcode.commands;
+//
+//import com.arcrobotics.ftclib.command.CommandBase;
+//import com.outoftheboxrobotics.photoncore.Photon;
+//import com.qualcomm.robotcore.hardware.DistanceSensor;
+//import com.qualcomm.robotcore.hardware.HardwareMap;
+//import com.qualcomm.robotcore.util.ElapsedTime;
+//
+//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+//import org.firstinspires.ftc.teamcode.subsystems.Claw;
+//import org.firstinspires.ftc.teamcode.subsystems.Intake;
+//
+//public class AarushsIntakeCommandEx extends CommandBase {
+//    private ElapsedTime runtime;
+//    private ElapsedTime intakeTimer, waitTimer; // Timer for the intake process
+//    private Intake intake;
+//    private DistanceSensor distanceSensor;
+//    private Claw claw;
+//    private int pixelsDetectedState = 0;
+//    private Intake.IntakePowers power;
+//    private static final double DETECTION_THRESHOLD = 4.0; // Threshold for the distance sensor
+//
+//    // consider reducing if need faster cycle times
+//    private static final double REQUIRED_TIME_MS = 900; // Required time in milliseconds,
+//
+//    private static final double FINISH_LOWSPEED_THRESHOLD = 400;
+//    private static final double OUTTAKE_TIME_ROBOT = 1350;
+//
+//    public AarushsIntakeCommandEx(HardwareMap hardwareMap, Claw claw, Intake intake, Intake.IntakePowers power) {
+//        this.intake = intake;
+//        this.claw = claw;
+//        this.distanceSensor = hardwareMap.get(DistanceSensor.class, "distanceSensor");
+//        this.runtime = new ElapsedTime();
+//        this.intakeTimer = new ElapsedTime();
+//        this.waitTimer = new ElapsedTime();
+//        this.power = power;
+//
+//        addRequirements(claw, intake); // If using Command-based structure, declare subsystem dependencies.
+//    }
+//
+//    @Override
+//    public void initialize() {
+//        claw.open();
+//        pixelsDetectedState = 0;
+//        runtime.reset();
+//        intakeTimer.reset();
+//        intake.setPower(power);
+//    }
+//
+//    @Override
+//    public void execute() {
+//        switch (pixelsDetectedState) {
+//            case 0: // Pixel not detected
+//                intake.setPower(power);
+//                if (distanceSensor.getDistance(DistanceUnit.CM) < DETECTION_THRESHOLD) {
+//                    intakeTimer.reset();
+//                    pixelsDetectedState = 1;
+//                }
+//                break;
+//            case 1: // Pixel detected, timer running
+//                if (intakeTimer.milliseconds() < REQUIRED_TIME_MS) {
+//                    intake.setPower(power);
+//                    if (distanceSensor.getDistance(DistanceUnit.CM) > DETECTION_THRESHOLD) {
+//                        pixelsDetectedState = 0; // Reset if the distance goes above the threshold
+//                    }
+//                } else {
+//                    pixelsDetectedState = 2; // Time required has passed, and pixel is consistently detected
+//                    waitTimer.reset();
+//                }
+//                break;
+//            case 2: // Pixel intake process is complete
+//                claw.close();
+//                if (waitTimer.milliseconds() < FINISH_LOWSPEED_THRESHOLD) {
+//                    intake.setPower(Intake.IntakePowers.SLOW);
+//                } else if (waitTimer.milliseconds() < OUTTAKE_TIME_ROBOT) {
+//                    intake.setPower(Intake.OuttakePowers.NORMAL);
+//                } else {
+//                    intake.setPower(0);
+//                    pixelsDetectedState = 3;
+//                }
+//                break;
+//        }
+//    }
+//
+//    @Override
+//    public boolean isFinished() {
+//        return pixelsDetectedState == 3;
+//    }
+//
+//    @Override
+//    public void end(boolean interrupted) {
+//        if (interrupted) {
+//            intake.setPower(0);
+//        }
+//        claw.close(); // Close the claw at the end of the command
+//    }
+//}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesManualCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesManualCommand.java
deleted file mode 100644
index bd69b5192c10..000000000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesManualCommand.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package org.firstinspires.ftc.teamcode.commands;
-
-import com.arcrobotics.ftclib.command.CommandBase;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.arcrobotics.ftclib.gamepad.GamepadKeys;
-
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-
-public class SlidesManualCommand extends CommandBase {
-    private final Slides slides;
-    private final GamepadEx mechanisms;
-    private final double up = 1.0;
-    private final double down = -0.7; //Comment so can push
-    private final double slowUp = 0.65;
-    private final double slowDown = -0.01;
-
-    public SlidesManualCommand(Slides slides, GamepadEx mechanisms) {
-        addRequirements(slides);
-
-        this.slides = slides;
-        this.mechanisms = mechanisms;
-    }
-
-    public boolean isActive() {
-        return mechanisms.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON);
-    }
-
-    @Override
-    public void execute() {
-        boolean slow = mechanisms.getButton(GamepadKeys.Button.X);
-
-        if (mechanisms.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON) && slides.atUpper()) {
-            slides.setSlidesPower((slow) ? slowUp : up);
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesResetCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesResetCommand.java
deleted file mode 100644
index 07e920eb3e0c..000000000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/SlidesResetCommand.java
+++ /dev/null
@@ -1,39 +0,0 @@
-package org.firstinspires.ftc.teamcode.commands;
-
-import com.arcrobotics.ftclib.command.CommandBase;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.arcrobotics.ftclib.gamepad.GamepadKeys;
-
-import org.firstinspires.ftc.teamcode.subsystems.Slides;
-
-public class SlidesResetCommand extends CommandBase {
-    private final Slides slides;
-    private final GamepadEx mechanisms; // comment
-
-    public SlidesResetCommand(Slides slides, GamepadEx mechanisms) {
-        addRequirements(slides);
-
-        this.slides = slides;
-        this.mechanisms = mechanisms;
-    }
-
-    @Override
-    public void execute() {
-        if (mechanisms.getButton(GamepadKeys.Button.DPAD_DOWN)){
-            slides.setSlidesPower(-0.2);
-        } else {
-            slides.stop();
-        }
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public void end(boolean interrupted){
-        slides.stop();
-        slides.resetSlide();
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/LiftPositionCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/LiftPositionCommand.java
new file mode 100644
index 000000000000..9d24877894da
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/LiftPositionCommand.java
@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode.commands.liftcommands;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Slides;
+
+@Config
+public class LiftPositionCommand extends CommandBase {
+
+    private PIDFController liftController;
+    public static PIDCoefficients coefficients =
+            new PIDCoefficients(0.08, 0.000, 0.001);
+    // 0.0269, 0.003, 0.0014
+
+    private double tolerance = 15;
+    private double targetPosition = 0;
+    private boolean holdAtEnd;
+    private final Slides lift;
+
+    public static double setpointPos;
+    public static double liftPosition = 0;
+    public static double liftVelocity = 0;
+    public static double controllerOutput = 0;
+
+    public LiftPositionCommand(Slides lift, int targetPosition, boolean holdAtEnd){
+        addRequirements(lift);
+
+        this.holdAtEnd = holdAtEnd;
+        this.lift = lift;
+        this.targetPosition = targetPosition;
+        setpointPos = targetPosition;
+
+        //Add a feedforward term to counteract gravity
+        liftController = new PIDFController(coefficients, 0.0, 0.0, 0.00, (x, v) -> {
+            double kG = 0;
+            if (liftPosition < 850) kG = 0.09;
+            else if (liftPosition < 1380) kG = 0.11;
+            else kG = 0.13;
+
+            return kG * lift.getVoltageComp();
+        });
+        liftController.setOutputBounds(-0.7, 1.0); // gravity CA
+    }
+    @Override
+    public void initialize(){
+        //once
+        liftController.reset();
+        liftController.setTargetPosition(targetPosition);
+    }
+
+    //Run repeatedly while the command is active
+    @Override
+    public void execute(){
+        liftPosition = lift.getLiftPosition();
+        liftVelocity = lift.getLiftVelocity();
+
+        liftController.setTargetPosition(targetPosition);
+
+        controllerOutput = liftController.update(liftPosition, liftVelocity);
+
+        //Update the lift power with the controller
+        lift.setLiftPower(controllerOutput);
+    }
+
+    @Override
+    public boolean isFinished(){
+        //End if the lift position is within the tolerance
+        return Math.abs(targetPosition - liftPosition) <= tolerance;
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        if (holdAtEnd) lift.setLiftPower(0.09);
+        else lift.brake_power();
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftCommand.java
new file mode 100644
index 000000000000..8670ef5e0a34
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftCommand.java
@@ -0,0 +1,76 @@
+package org.firstinspires.ftc.teamcode.commands.liftcommands;
+
+import static org.firstinspires.ftc.teamcode.util.CrossBindings.square;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+
+import org.firstinspires.ftc.teamcode.subsystems.Slides;
+
+public class ManualLiftCommand extends CommandBase {
+
+    private final Slides lift;
+    private final GamepadEx manipulator;
+
+    private final double up = 0.75;
+    private final double down = -0.4;
+
+    private final double slowUp = 0.37;
+    private final double slowDown = -1.0;
+
+    public ManualLiftCommand(Slides lift, GamepadEx manipulator) {
+        addRequirements(lift);
+
+        this.lift = lift;
+        this.manipulator = manipulator;
+
+    }
+
+    public boolean isManualActive() {
+        return manipulator.getButton(GamepadKeys.Button.DPAD_UP) ||
+                manipulator.getButton(GamepadKeys.Button.DPAD_DOWN);
+    }
+
+    @Override
+    public void execute() {
+        //Two dpad buttons cant be pressed at the same time so we don't have to worry about that.
+
+        boolean slow = manipulator.getButton(square);
+
+        //Check if the up button is pressed
+        if (manipulator.getButton(GamepadKeys.Button.DPAD_UP) && !lift.atUpperLimit()) {
+            lift.setLiftPower((slow) ? slowUp : up);
+        }
+
+        //Then check if the down is pressed
+        else if (manipulator.getButton(GamepadKeys.Button.DPAD_DOWN) && !lift.atLowerLimit()) {
+
+            if (lift.getLiftVelocity() < -500 && lift.getLiftPosition() < 350) {
+                lift.setLiftPower(0);
+            } else {
+                lift.setLiftPower((slow) ? slowDown : down);
+            }
+
+        }
+
+        //Otherwise, do nothing
+        else {
+            if (lift.getLiftPosition() < 10) lift.setLiftPower(0);
+                // Counter motor being on close to the bottom
+            else if (lift.getLiftPosition() < 40) lift.setLiftPower(-.05);
+                // Slide 1 Counter
+            else if (lift.getLiftPosition() < 850)
+                lift.setLiftPower(0.09*lift.getVoltageComp()); //0.19 *
+                // Slide 2 counter
+            else if (lift.getLiftPosition() < 1380)
+                lift.setLiftPower(0.10*lift.getVoltageComp()); //0.20 * lift.getVoltageComp()
+                // Slide 3 counter
+            else if (lift.getLiftPosition() < 1640)
+                lift.setLiftPower(0.11*lift.getVoltageComp()); //0.20 * lift.getVoltageComp()
+                // Other Counter
+            else lift.setLiftPower(0.13*lift.getVoltageComp());
+        }
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftResetCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftResetCommand.java
new file mode 100644
index 000000000000..c14a7af3fc2b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ManualLiftResetCommand.java
@@ -0,0 +1,42 @@
+package org.firstinspires.ftc.teamcode.commands.liftcommands;
+
+
+import com.arcrobotics.ftclib.command.CommandBase;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+
+import org.firstinspires.ftc.teamcode.subsystems.Slides;
+
+public class ManualLiftResetCommand extends CommandBase {
+    private final Slides lift;
+    private final GamepadEx manipulator;
+
+
+    public ManualLiftResetCommand(Slides lift, GamepadEx manipulator){
+        addRequirements(lift);
+
+        this.lift = lift;
+        this.manipulator = manipulator;
+    }
+
+    @Override
+    public void execute(){
+        if(manipulator.getButton(GamepadKeys.Button.DPAD_LEFT)){
+            lift.setLiftPower(-0.3);
+        } else {
+            lift.brake_power();
+        }
+    }
+
+
+    @Override
+    public boolean isFinished(){
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        lift.brake_power();
+        lift.resetLiftPosition();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ProfiledLiftCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ProfiledLiftCommand.java
new file mode 100644
index 000000000000..1567fb2ae168
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/liftcommands/ProfiledLiftCommand.java
@@ -0,0 +1,137 @@
+package org.firstinspires.ftc.teamcode.commands.liftcommands;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.arcrobotics.ftclib.command.CommandBase;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystems.Slides;
+
+@Config
+public class ProfiledLiftCommand extends CommandBase {
+
+    PIDFController liftController;
+    private MotionProfile profile;
+    ElapsedTime timer = new ElapsedTime();
+
+    PIDCoefficients coefficients = new PIDCoefficients(0.02, 0.0, 0.0); // Adjust PID coefficients as needed
+
+    // Feedforward Coefficients
+    double kV = 0.0, kA = 0.0, kStatic = 0.00;
+
+    // The tolerance for getting to a certain position. Strict tries to get just a bit closer.
+    private double LIFT_POSITION_TOLERANCE = 15,
+            LIFT_POSITION_TOLERANCE_STRICT = 10;
+
+    private double liftPosition = 0;
+    private double liftVelocity = 0;
+    private double controllerOutput = 0;
+
+    final double MOTION_PROFILE_MAX_VELOCITY = 2500,
+            MOTION_PROFILE_MAX_ACCEL = 3250,
+            MOTION_PROFILE_MAX_JERK = 0;
+
+    final double GRAVITY_FEEDFORWARD_COMPENSATION_FIRST_STAGE = 0.03,
+            GRAVITY_FEEDFORWARD_COMPENSATION_SECOND_STAGE = 0.05,
+            GRAVITY_FEEDFORWARD_COMPENSATION_THIRD_STAGE = 0.10;
+    final double LIFT_FIRST_STAGE_POSITION_TICKS = 600,
+            LIFT_SECOND_STAGE_POSITION_TICKS = 1200;
+
+    boolean holdAtEnd;
+    final Slides lift;
+    final double targetPosition;
+
+    double setPointPos;
+
+    public ProfiledLiftCommand(Slides lift, int targetPosition, boolean holdAtEnd){
+        this(lift, targetPosition, holdAtEnd, false);
+    }
+
+    public ProfiledLiftCommand(Slides lift, int targetPosition, boolean holdAtEnd, boolean strict){
+        addRequirements(lift);
+
+        if (strict) this.LIFT_POSITION_TOLERANCE = LIFT_POSITION_TOLERANCE_STRICT;
+
+        this.holdAtEnd = holdAtEnd;
+        this.lift = lift;
+        this.targetPosition = targetPosition;
+
+        setPointPos = targetPosition;
+
+        // Gravity feedforward term to counteract gravity
+        liftController = new PIDFController(coefficients, kV, kA, kStatic, (x, v) -> {
+            // Feedforward Gravitational Below
+            double kG = 0;
+            if (liftPosition < LIFT_FIRST_STAGE_POSITION_TICKS) {
+                kG = GRAVITY_FEEDFORWARD_COMPENSATION_FIRST_STAGE;
+            }
+            else if (liftPosition < LIFT_SECOND_STAGE_POSITION_TICKS) {
+                kG = GRAVITY_FEEDFORWARD_COMPENSATION_SECOND_STAGE;
+            }
+            else {
+                kG = GRAVITY_FEEDFORWARD_COMPENSATION_THIRD_STAGE;
+            }
+
+            return kG * lift.getVoltageComp();
+        });
+
+        // Prevent it from going down TOO fast
+        // This is the same as the maximum amount of kG compensation subtracted from max negative value
+        liftController.setOutputBounds(-0.87, 1.0);
+    }
+
+    @Override
+    public void initialize(){
+        liftController.reset();
+
+        // Generate the motion profile
+        profile = MotionProfileGenerator.generateSimpleMotionProfile(
+                new MotionState(lift.getLiftPosition(), lift.getLiftVelocity()),
+                new MotionState(targetPosition, 0),
+                MOTION_PROFILE_MAX_VELOCITY,
+                MOTION_PROFILE_MAX_ACCEL,
+                MOTION_PROFILE_MAX_JERK
+        );
+
+        timer.reset();
+    }
+
+    @Override
+    public void execute(){
+        liftPosition = lift.getLiftPosition();
+        liftVelocity = lift.getLiftVelocity();
+
+        double currentTime = timer.seconds();
+
+        MotionState state = profile.get(currentTime);
+
+        // Update the PIDFController with the profile's state
+        liftController.setTargetPosition(state.getX());
+        liftController.setTargetVelocity(state.getV());
+        liftController.setTargetAcceleration(state.getA());
+
+        controllerOutput = liftController.update(liftPosition, liftVelocity);
+
+        // Update the lift power with the controller
+        lift.setLiftPower(controllerOutput);
+
+        // Additional SetPoint variables can be set here if needed
+        setPointPos = state.getX();
+    }
+
+    @Override
+    public boolean isFinished(){
+        // End if the lift position is within the tolerance
+        return Math.abs(targetPosition - liftPosition) <= LIFT_POSITION_TOLERANCE;
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        if (holdAtEnd) lift.setLiftPower(0.078);
+        else lift.brake_power(); // Assuming brake_power() is a method to stop the lift
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/MoveToScoringCommand.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/MoveToScoringCommand.kt
new file mode 100644
index 000000000000..e142a83cd8eb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/MoveToScoringCommand.kt
@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.commands.presets
+
+import android.transition.Slide
+import com.arcrobotics.ftclib.command.InstantCommand
+import com.arcrobotics.ftclib.command.ParallelCommandGroup
+import com.arcrobotics.ftclib.command.SequentialCommandGroup
+import com.arcrobotics.ftclib.command.WaitCommand
+import org.firstinspires.ftc.teamcode.commands.liftcommands.ProfiledLiftCommand
+import org.firstinspires.ftc.teamcode.subsystems.Outtake
+import org.firstinspires.ftc.teamcode.subsystems.Slides
+import org.firstinspires.ftc.teamcode.subsystems.Slides.SlidePositions
+import org.firstinspires.ftc.teamcode.subsystems.V4B
+
+class MoveToScoringCommand(lift: Slides, v4b: V4B, outtake: Outtake, preset: Presets) : ParallelCommandGroup() {
+
+    enum class Presets {
+        SHORT,
+        MID,
+        HIGH
+    }
+
+    init {
+        addCommands(
+            ParallelCommandGroup(
+                SequentialCommandGroup(
+                    // Change this ms to change when the arm comes up
+                    WaitCommand(800),
+                    InstantCommand({
+                        v4b.extend()
+                    }),
+                ),
+                InstantCommand({
+                    outtake.transport()
+                }),
+                when (preset) {
+                    Presets.SHORT ->
+                        ProfiledLiftCommand(lift, SlidePositions.SHORT.position, true)
+                    Presets.MID ->
+                        ProfiledLiftCommand(lift, SlidePositions.MID.position, true)
+                    Presets.HIGH ->
+                        ProfiledLiftCommand(lift, SlidePositions.HIGH.position, true)
+
+                }
+            )
+        )
+        addRequirements(lift)
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/RetractOuttakeCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/RetractOuttakeCommand.java
new file mode 100644
index 000000000000..078e07e3f37d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/presets/RetractOuttakeCommand.java
@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode.commands.presets;
+
+import com.arcrobotics.ftclib.command.InstantCommand;
+import com.arcrobotics.ftclib.command.ParallelCommandGroup;
+import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+import com.arcrobotics.ftclib.command.WaitCommand;
+
+import org.firstinspires.ftc.teamcode.commands.liftcommands.LiftPositionCommand;
+import org.firstinspires.ftc.teamcode.commands.liftcommands.ProfiledLiftCommand;
+import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+import org.firstinspires.ftc.teamcode.subsystems.Slides;
+import org.firstinspires.ftc.teamcode.subsystems.V4B;
+
+//Brings everything to rest position
+public class RetractOuttakeCommand extends ParallelCommandGroup {
+
+    public RetractOuttakeCommand(Slides lift, V4B v4b, Outtake outtake){
+        addCommands(
+            new InstantCommand(outtake::transport),
+            new InstantCommand(v4b::retract),
+            new SequentialCommandGroup(
+                    new WaitCommand(800),
+                    new LiftPositionCommand(lift, Slides.SlidePositions.DOWN.position, false)
+            )
+//            new InstantCommand(outtake::open)
+        );
+
+        addRequirements(lift);
+    }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivebase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivebase.java
new file mode 100644
index 000000000000..0aa8cbbdd1ce
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivebase.java
@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static java.lang.Math.abs;
+import static java.lang.Math.max;
+
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.kauailabs.navx.ftc.AHRS;
+import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+public class Drivebase {
+    public AHRS imu;
+
+    private double imuPrevPositionRad = 0.0;
+
+    public DcMotorEx frontLeft, frontRight, backLeft, backRight;
+
+
+    public Drivebase (HardwareMap hardwareMap) {
+        this.frontLeft = hardwareMap.get(DcMotorEx.class,"rightFront"); // Drivebase
+        this.frontRight = hardwareMap.get(DcMotorEx.class,"leftFront"); // Drivebase
+        this.backLeft = hardwareMap.get(DcMotorEx.class,"rightRear"); // Drivebase
+        this.backRight = hardwareMap.get(DcMotorEx.class,"leftRear"); // Drivebase
+        this.imu = AHRS.getInstance(hardwareMap.get(NavxMicroNavigationSensor.class, "navx"), AHRS.DeviceDataType.kProcessedData);
+
+        commonMotorSetup();
+    }
+
+    private void commonMotorSetup() {
+        frontLeft.setDirection(DcMotor.Direction.REVERSE);
+        backLeft.setDirection(DcMotor.Direction.REVERSE);
+
+//        frontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+//        backRight.setDirection(DcMotorSimple.Direction.REVERSE);
+    }
+
+//    public void reset() {
+//        imu.zeroYaw();
+//    }
+
+    public void drive(double y, double x, double rx) {
+        // rx = right stick x
+        // x = left stick x
+        // y = left stick y (reversed in hardware)
+        y = -y; // y && strafe forward back
+        x = x * 1.1; // x && strafe right and left
+        rx = rx; // rx && turn left right angular
+
+        // Calculate the robot's heading from the IMU
+        double botHeading = getCorrectedYaw();
+
+        // botHeading is in Radians
+        Vector2d botVector = new Vector2d(x, y).rotated(botHeading);
+
+//        // Apply the calculated heading to the input vector for field centric
+        x = botVector.getX(); // strafe r/l transform values
+        y = botVector.getY(); // strafe f/b transform values
+        // note rx is not here since rotation is always field centric!
+
+        // Calculate the motor powers
+        double frontLeftPower = y + x + rx;
+        double frontRightPower = y - x - rx;
+
+        double backLeftPower = y - x + rx;
+        double backRightPower = y + x - rx;
+
+        // Find the max power and make sure it ain't greater than 1
+        double denominator = max(
+                max(abs(frontLeftPower), abs(backLeftPower)),
+                max(abs(frontRightPower), abs(backRightPower))
+        );
+
+        if (denominator > 1.0) {
+            frontLeftPower /= denominator;
+            backLeftPower /= denominator;
+            frontRightPower /= denominator;
+            backRightPower /= denominator;
+        }
+
+        // Old method below, to ensure the CONTROLLERS no more than 1
+        // double denominator = max(abs(y) + abs(x) + abs(rx), 1);
+
+        // Set the motor powers
+        frontLeft.setPower(frontLeftPower);
+        backLeft.setPower(backLeftPower);
+        frontRight.setPower(frontRightPower);
+        backRight.setPower(backRightPower);
+
+//        double botHeading = Math.toRadians(imu.getYaw());
+//
+//        // reverse the controller left stick because it's reversed in hardware.
+//        y = -y;
+//
+//        // Rotate the movement direction counter to the bot's rotation
+//        double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
+//        double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
+//
+//        rotX = rotX * 1.1;  // Counteract imperfect strafing
+//
+//        double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
+//
+//        double flpwr = (rotY + rotX + rx) / denominator;
+//        double blpwr = (rotY - rotX + rx) / denominator;
+//        double frpwr = (rotY - rotX - rx) / denominator;
+//        double brpwr = (rotY + rotX - rx) / denominator;
+//
+//        frontLeft.setPower(flpwr);
+//        backLeft.setPower(blpwr);
+//        frontRight.setPower(frpwr);
+//        backRight.setPower(brpwr);
+    }
+
+    public double getCorrectedYaw () {
+        double imuDeg = imu.getYaw();
+        double imuRad = AngleUnit.RADIANS.fromDegrees(imuDeg);
+//        double imuRad = imuDeg;
+        double correctedRadReset = imuRad-imuPrevPositionRad;
+        return (1.0) * correctedRadReset; // * (14.0/180.0);
+    }
+
+    public void reset() {
+        imuPrevPositionRad = AngleUnit.RADIANS.fromDegrees(imu.getYaw());
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Impasta.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Impasta.java
deleted file mode 100644
index a15d5294d690..000000000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Impasta.java
+++ /dev/null
@@ -1,242 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.kauailabs.navx.ftc.AHRS;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.IMU;
-import com.qualcomm.robotcore.hardware.Servo;
-
-@Config
-public class Impasta {
-    // Hardware variables
-    private AHRS imu;
-    private IMU.Parameters parameters;
-    private DcMotor fl, fr, bl, br, leftSlide, rightSlide, Intake;
-    private Servo leftOut, rightOut, leftV4B, rightV4B;
-    private PID pid = new PID(0.02, 0, 0);
-
-    private boolean leftOutRaised = true;
-    private boolean rightOutRaised = true;
-
-    private boolean v4bAtRest = true;
-
-
-    // Constructor for Impasta class
-    public Impasta(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br, DcMotor leftSlide, DcMotor rightSlide, DcMotor Intake, AHRS imu) {
-        // Assigning hardware references to local variables
-        this.fl = fl;
-        this.fr = fr;
-        this.bl = bl;
-        this.br = br;
-        this.leftSlide = leftSlide;
-        this.rightSlide = rightSlide;
-        this.Intake = Intake;
-        this.imu = imu;
-
-        fr.setDirection(DcMotor.Direction.REVERSE);
-        br.setDirection(DcMotor.Direction.REVERSE);
-
-        rightSlide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        rightSlide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-    }
-
-    public Impasta(DcMotor leftSlide, DcMotor rightSlide) {
-        this.leftSlide = leftSlide;
-        this.rightSlide = rightSlide;
-    }
-
-    public Impasta(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br, DcMotor leftSlide, DcMotor rightSlide, DcMotor Intake) {
-        this.fl = fl;
-        this.fr = fr;
-        this.bl = bl;
-        this.br = br;
-        this.leftSlide = leftSlide;
-        this.rightSlide = rightSlide;
-        this.Intake = Intake;
-
-        fr.setDirection(DcMotor.Direction.REVERSE);
-        br.setDirection(DcMotor.Direction.REVERSE);
-    }
-
-    public Impasta(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br, DcMotor leftSlide, DcMotor rightSlide, DcMotor Intake, Servo leftOut, Servo rightOut, Servo leftV4B, Servo rightV4b) {
-        this.fl = fl;
-        this.fr = fr;
-        this.bl = bl;
-        this.br = br;
-        this.leftSlide = leftSlide;
-        this.rightSlide = rightSlide;
-        this.Intake = Intake;
-        this.leftOut = leftOut;
-        this.rightOut = rightOut;
-        this.leftV4B = leftV4B;
-        this.rightV4B = rightV4b;
-
-        fr.setDirection(DcMotor.Direction.REVERSE);
-        br.setDirection(DcMotor.Direction.REVERSE);
-    }
-
-    // Method to reset IMU yaw angle
-    public void reset() {
-        imu.zeroYaw();
-    }
-
-    // Method to drive the robot based on gamepad input
-    public void driveBaseRobot(double y, double x, double rx) {
-        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-
-        double flpwr = (y + x + rx) / denominator;
-        double blpwr = (y - x + rx) / denominator;
-        double frpwr = (y - x - rx) / denominator;
-        double brpwr = (y + x - rx) / denominator;
-
-        fl.setPower(flpwr);
-        bl.setPower(blpwr);
-        fr.setPower(frpwr);
-        br.setPower(brpwr);
-    }
-
-    public void driveBaseField(double y, double x, double rx) {
-        double botHeading = Math.toRadians(imu.getYaw());
-
-        // Rotate the movement direction counter to the bot's rotation
-        double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
-        double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
-
-        rotX = rotX * 1.1;  // Counteract imperfect strafing
-
-        double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
-
-        double flpwr = (rotY + rotX + rx) / denominator;
-        double blpwr = (rotY - rotX + rx) / denominator;
-        double frpwr = (rotY - rotX - rx) / denominator;
-        double brpwr = (rotY + rotX - rx) / denominator;
-
-        fl.setPower(flpwr);
-        bl.setPower(blpwr);
-        fr.setPower(frpwr);
-        br.setPower(brpwr);
-    }
-
-    // Method to control intake motor
-    public void intake(double ip) {
-        Intake.setPower(ip);
-    }
-
-    //Slide Control via power
-
-
-//  ================================================ Slide Stuff ================================================
-
-    public void setSlidesPower(double power) {
-        leftSlide.setPower(-power);
-        rightSlide.setPower(power);
-    }
-
-    public void runManual(double dr4bp, boolean doClimbing) {
-        if (doClimbing) {
-            leftSlide.setPower(-dr4bp);
-            rightSlide.setPower(dr4bp);
-        } else {
-            rightSlide.setPower(dr4bp);
-        }
-    }
-
-    public double getSlidesPos() {
-        return leftSlide.getCurrentPosition();
-    }
-
-    public void runPID(double targetPos) {
-        double output = pid.update(leftSlide.getCurrentPosition(), targetPos);
-        rightSlide.setPower(-output);
-    }
-
-    public void resetSlide() {
-        leftSlide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        rightSlide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        leftSlide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        rightSlide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-    }
-
-    public boolean atUpper() {
-        return getSlidesPos() > 600;
-    }
-
-    public boolean atLower() {
-        return getSlidesPos() < 5;
-    }
-
-//  =============================================================================================================
-
-    public void toggleLeftOut() {
-        if (leftOutRaised) {
-            leftOut.setPosition(0.75);
-        } else {
-            leftOut.setPosition(0.55);
-        }
-        leftOutRaised = !leftOutRaised;
-    }
-
-    public void toggleRightOut() {
-        if (rightOutRaised) {
-            rightOut.setPosition(0.5);
-        } else {
-            rightOut.setPosition(0.6);
-        }
-        rightOutRaised = !rightOutRaised;
-    }
-
-    public void toggleV4B() {
-        if (v4bAtRest) {
-            leftV4B.setPosition(180);
-            rightV4B.setPosition(180);
-        } else {
-            leftV4B.setPosition(0);
-            rightV4B.setPosition(0);
-        }
-        v4bAtRest = !v4bAtRest;
-    }
-
-    public double getLeftV4Bpos() {
-        return leftV4B.getPosition();
-    }
-
-    public double getRightV4Bpos() {
-        return rightV4B.getPosition();
-    }
-
-    public double getLeftOutPos() {
-        return leftOut.getPosition();
-    }
-
-    public double getRightOutPos() {
-        return rightOut.getPosition();
-    }
-
-    public void driveBaseAuto(double x) {
-        double denominator = Math.max(Math.abs(0) + Math.abs(x) + Math.abs(0), 1);
-
-        double flpwr = (0 + x + 0) / denominator;
-        double blpwr = (0 - x + 0) / denominator;
-        double frpwr = (0 - x - 0) / denominator;
-        double brpwr = (0 + x - 0) / denominator;
-
-        fl.setPower(flpwr);
-        bl.setPower(blpwr);
-        fr.setPower(frpwr);
-        br.setPower(brpwr);
-    }
-
-    public void driveBaseAutoFar(double y) {
-        double denominator = Math.max(Math.abs(y) + Math.abs(0) + Math.abs(0), 1);
-
-        double flpwr = (y + 0 + 0) / denominator;
-        double blpwr = (y - 0 + 0) / denominator;
-        double frpwr = (y - 0 - 0) / denominator;
-        double brpwr = (y + 0 - 0) / denominator;
-
-        fl.setPower(flpwr);
-        bl.setPower(blpwr);
-        fr.setPower(frpwr);
-        br.setPower(brpwr);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
index 8cfc9408ea49..1c21e6b9a0b8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
@@ -10,31 +10,17 @@
 public class Intake extends SubsystemBase {
 
     private DcMotorEx intakeMotor;
-    private Gamepad gamepad;
-    private boolean isAuto;
-
-    public Intake(Gamepad gamepad, HardwareMap hardwareMap) {
-        intakeMotor = hardwareMap.get(DcMotorEx.class, "leftEncoder");
-        isAuto = false;
-
-        this.gamepad = gamepad;
-    }
 
     public Intake(HardwareMap hardwareMap) {
-        intakeMotor = hardwareMap.get(DcMotorEx.class, "leftEncoder");
-        isAuto = true;
+        intakeMotor = hardwareMap.get(DcMotorEx.class, "intake_leftEncoder");
     }
 
     public void intake() {
-        if (!isAuto) {
-            double power = gamepad.left_trigger - gamepad.right_trigger;
-            intakeMotor.setPower(power);
-        } else {
-            intakeMotor.setPower(1);
-        }
+        intakeMotor.setPower(1);
     }
-    public void autoIntake(double p) {
-        intakeMotor.setPower(p);
+
+    public void setPower(double power) {
+        intakeMotor.setPower(power);
     }
 
     public void outtake() {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Launcher.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Launcher.java
index 7068ab7946fd..f15f5d5d4fe2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Launcher.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Launcher.java
@@ -12,19 +12,27 @@
 public class Launcher extends SubsystemBase {
 
     private Servo launchServo;
-    private Gamepad gamepad;
 
-    public Launcher(Gamepad gamepad, HardwareMap hardwareMap) {
-        launchServo = hardwareMap.get(Servo.class, "launch");
+    public enum PlaneState {
+        LAUNCH(1),
+        RESET(0);
 
-        this.gamepad = gamepad;
-    }
+        public double position;
 
-    public void airplaneLauncher() {
-        if (gamepad.triangle) {
-            launchServo.setPosition(0);
-        } else if (gamepad.square) {
-            launchServo.setPosition(1);
+        PlaneState(double position) {
+            this.position = position;
         }
     }
+
+    public Launcher(HardwareMap hardwareMap) {
+        launchServo = hardwareMap.get(ServoImplEx.class, "launcher");
+    }
+
+    public void launch() {
+        launchServo.setPosition(PlaneState.LAUNCH.position);
+    }
+
+    public void reset() {
+        launchServo.setPosition(PlaneState.RESET.position);
+    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Mechanisms.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Mechanisms.java
index 0d648ba43d0a..ed7e81255fd7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Mechanisms.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Mechanisms.java
@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
+import com.acmerobotics.roadrunner.drive.Drive;
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -12,78 +13,19 @@
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 
 public class Mechanisms extends SubsystemBase {
-    Impasta impasta;
     private Gamepad gamepad1, gamepad2;
-    private Servo out1, out2, launchPlane;
-    private Servo DRV4BL, DRV4BR;
-    //    private CRServo DRV4BL, DRV4BR;
-
-    public Mechanisms(Gamepad gamepad1, Gamepad gamepad2, HardwareMap hardwareMap) {
-//        DRV4BL = hardwareMap.crservo.get("leftV4B"); //Virtual Four Bar Servos // Left Side
-//        DRV4BR = hardwareMap.crservo.get("rightV4B"); //Virtual Four Bar Servos //Right Side
-        DRV4BL = hardwareMap.servo.get("leftV4B");
-        DRV4BR = hardwareMap.servo.get("rightV4B");
-        launchPlane = hardwareMap.servo.get("launcher");
-        out1 = hardwareMap.servo.get("leftOut"); //Outtake
-        out2 = hardwareMap.servo.get("rightOut"); //Outtake
-
-        this.gamepad1 = gamepad1;
-        this.gamepad2 = gamepad2;
-    }
-
-    public void launch() {
-        launchPlane.setPosition(0);
-    }
-
-    public void resetLaunch() {
-        launchPlane.setPosition(1);
-    }
-
-//    public void v4b() {
-//        if (gamepad2.circle) {
-//            // Rotate the CR servos clockwise as long as the button is pressed
-//            while (gamepad2.circle) {
-//                DRV4BL.setPower(1.0);  // Adjust the power as needed
-//                DRV4BR.setPower(1.0);  // Adjust the power as needed
-//            }
-//            // Stop the servos when the button is released
-//            DRV4BL.setPower(0);
-//            DRV4BR.setPower(0);
-//
-//        } else if (gamepad2.cross) {
-//            // Rotate the CR servos counterclockwise as long as the button is pressed
-//            while (gamepad2.cross) {
-//                DRV4BL.setPower(-1.0);  // Adjust the power as needed
-//                DRV4BR.setPower(-1.0);  // Adjust the power as needed
-//            }
-//            // Stop the servos when the button is released
-//            DRV4BL.setPower(0);
-//            DRV4BR.setPower(0);
-//
-//        }
-//    }
-
-    public void extend() {
-        DRV4BL.setPosition(0.45);
-        DRV4BR.setPosition(0.45);
-    }
-
-    public void retract() {
-        DRV4BL.setPosition(0.01);
-        DRV4BR.setPosition(0.01);
-    }
-//  TODO: Test distance sensors
-    public void outtakes() {
-        if (gamepad2.left_trigger > 0.3) {
-            out1.setPosition(1);
-        } else {
-            out1.setPosition(0.91);
-        }
-
-        if (gamepad2.right_trigger > 0.3) {
-            out2.setPosition(1);
-        } else {
-            out2.setPosition(0.91);
-        }
+    private ServoImplEx out1, out2, launchPlane;
+    public V4B v4b;
+    public Launcher launcher;
+    public Outtake outtake;
+    public Intake intake;
+    public Drivebase drivebase;
+
+    public Mechanisms(HardwareMap hardwareMap) {
+        v4b = new V4B(hardwareMap);
+        launcher = new Launcher(hardwareMap);
+        outtake = new Outtake(hardwareMap);
+        intake = new Intake(hardwareMap);
+        drivebase = new Drivebase(hardwareMap);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java
index d8a6707b841e..0e63e86b843e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java
@@ -2,7 +2,6 @@
 
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.qualcomm.robotcore.hardware.DistanceSensor;
-import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.PwmControl;
 import com.qualcomm.robotcore.hardware.ServoImplEx;
@@ -12,8 +11,9 @@
 public class Outtake extends SubsystemBase {
 
     public enum outtakePos {
-        OPEN(1),
-        CLOSE(0.92);
+        OPEN(0.961),
+        CLOSED(0.86),
+        TRANSPORT(0.83);
 
         public double position;
 
@@ -22,12 +22,11 @@ public enum outtakePos {
         }
     }
 
-    private ServoImplEx leftOuttake, rightOuttake;
-    private Gamepad gamepad;
-    private DistanceSensor leftSensor, rightSensor;
-    private  int distance = 6;
+    ServoImplEx leftOuttake, rightOuttake;
+    DistanceSensor leftSensor, rightSensor;
+    int distance = 6;
 
-    public Outtake(Gamepad gamepad, HardwareMap hardwareMap) {
+    public Outtake(HardwareMap hardwareMap) {
         leftOuttake = hardwareMap.get(ServoImplEx.class, "leftOut");
         rightOuttake = hardwareMap.get(ServoImplEx.class, "rightOut");
         leftSensor = hardwareMap.get(DistanceSensor.class, "leftDistance");
@@ -36,8 +35,6 @@ public Outtake(Gamepad gamepad, HardwareMap hardwareMap) {
         //TODO: Test the following to see if it fixes DC issues of power module
         leftOuttake.setPwmRange(new PwmControl.PwmRange(500, 2500));
         rightOuttake.setPwmRange(new PwmControl.PwmRange(500, 2500));
-
-        this.gamepad = gamepad;
     }
 
     public double returnDistanceLeft() {
@@ -50,40 +47,60 @@ public double returnDistanceRight() {
 
     public void detectDistance() {
         if (leftSensor.getDistance(DistanceUnit.CM) < distance) {
-            leftOuttake.setPosition(outtakePos.CLOSE.position);
+            leftOuttake.setPosition(outtakePos.CLOSED.position);
         } else {
             leftOuttake.setPosition(outtakePos.OPEN.position);
         }
 
         if (rightSensor.getDistance(DistanceUnit.CM) < distance) {
-            rightOuttake.setPosition(outtakePos.CLOSE.position);
+            rightOuttake.setPosition(outtakePos.CLOSED.position);
         } else {
             rightOuttake.setPosition(outtakePos.OPEN.position);
         }
     }
 
-    public void open() {
+    void left_open () {
         leftOuttake.setPosition(outtakePos.OPEN.position);
+    }
+
+    void right_open () {
         rightOuttake.setPosition(outtakePos.OPEN.position);
     }
 
-    public void close() {
-        leftOuttake.setPosition(outtakePos.CLOSE.position);
-        rightOuttake.setPosition(outtakePos.CLOSE.position);
+    void left_close () {
+        leftOuttake.setPosition(outtakePos.CLOSED.position);
     }
 
-    public void update() {
-        if (gamepad.left_trigger > 0.3) {
-            leftOuttake.setPosition(outtakePos.OPEN.position);
-        } else {
-            leftOuttake.setPosition(outtakePos.OPEN.position);
-        }
+    void right_close () {
+        rightOuttake.setPosition(outtakePos.CLOSED.position);
+    }
 
-        if (gamepad.right_trigger > 0.3) {
-            rightOuttake.setPosition(outtakePos.CLOSE.position);
-        } else {
-            rightOuttake.setPosition(outtakePos.CLOSE.position);
-        }
+    void left_transport () {
+        leftOuttake.setPosition(outtakePos.TRANSPORT.position);
+    }
+
+    void right_transport () {
+        rightOuttake.setPosition(outtakePos.TRANSPORT.position);
+    }
+
+    public void close () {
+        left_close();
+        right_close();
+    }
+
+    public void open () {
+        right_open();
+        left_open();
+    }
+
+    public void transport () {
+        right_transport();
+        left_transport();
+    }
+
+    public void setPosition (double position) {
+        leftOuttake.setPosition(position);
+        rightOuttake.setPosition(position);
     }
 
     public double getPosition() {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PID.java
deleted file mode 100644
index f80b8b0456cb..000000000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PID.java
+++ /dev/null
@@ -1,19 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-public class PID {
-    private final double p;
-    private final double i;
-    private final double d;
-
-    public PID(double kp, double ki, double kd) {
-        p = kp;
-        i = ki;
-        d = kd;
-    }
-
-    public int update(double state, double target) {
-        double error = target - state;
-        int output = Math.toIntExact(Math.round(p * error));
-        return output;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/SlidePID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/SlidePID.java
index 1752e25f1526..01881b802081 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/SlidePID.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/SlidePID.java
@@ -1,34 +1,34 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.arcrobotics.ftclib.command.CommandBase;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-public class SlidePID extends CommandBase {
-    private Impasta impasta;
-    private double target;
-
-
-
-    public SlidePID(Impasta slides, double target) {
-        this.impasta = slides;
-        this.target = target;
-    }
-
-    @Override
-    public void initialize() {}
-
-    @Override
-    public void execute(){
-        impasta.runPID(target);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return Math.abs(impasta.getSlidesPos() - target) < 25;
-    }
-
-    @Override
-    public void end(boolean interupted) {
-        impasta.runManual(0, false);
-    }
-}
+//package org.firstinspires.ftc.teamcode.subsystems;
+//
+//import com.arcrobotics.ftclib.command.CommandBase;
+//import com.qualcomm.robotcore.hardware.DcMotor;
+//
+//public class SlidePID extends CommandBase {
+//    private Impasta impasta;
+//    private double target;
+//
+//    double tolerance = 25;
+//
+//    public SlidePID(Impasta slides, double target) {
+//        this.impasta = slides;
+//        this.target = target;
+//    }
+//
+//    @Override
+//    public void initialize() {}
+//
+//    @Override
+//    public void execute(){
+//        impasta.runPID(target);
+//    }
+//
+//    @Override
+//    public boolean isFinished() {
+//        return Math.abs(impasta.getSlidesPos() - target) < tolerance;
+//    }
+//
+//    @Override
+//    public void end (boolean interrupted) {
+//        impasta.runManual(0, false);
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Slides.java
index 6a3d1d10a2e5..627afd7544da 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Slides.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Slides.java
@@ -3,83 +3,115 @@
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.ServoImpl;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
 
 public class Slides extends SubsystemBase {
-
-    public enum SlidePos {
-        DOWN(50),
-        LOW(70),
-        MED(90),
-        HIGH(130);
+    public enum SlidePositions {
+        DOWN(0),
+        SHORT(1560),
+        MID(1900),
+        HIGH(2300);
 
         public int position;
 
-        SlidePos(int position) {
+        SlidePositions(int position){
             this.position = position;
         }
+
+        public int getPosition() {
+            return this.position;
+        }
     }
 
-    private PID pid = new PID(0.02, 0, 0);
+    final double winchServoInit = 0.5, winchServoEngage = 0.66;
+
+    DcMotorEx leftMotor;
+    DcMotorEx rightMotor;
+
+    private VoltageSensor voltageSensor;
+    private double voltageComp;
+    private double VOLTAGE_WHEN_LIFT_TUNED = 13.0;
+
+    public Slides(HardwareMap hardwareMap){
+
+        leftMotor = hardwareMap.get(DcMotorEx.class, "leftSlide_frontEncoder");
+        rightMotor = hardwareMap.get(DcMotorEx.class, "rightSlide");
 
-    private DcMotorEx lMotor, rMotor;
+        // Zero both the motors
+        leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
-    public Slides(HardwareMap hardwareMap) {
-        lMotor = hardwareMap.get(DcMotorEx.class, "frontEncoder");
-        rMotor = hardwareMap.get(DcMotorEx.class, "Right Slide");
+        // We don't need to use builtin PID
+        // THIS DOES NOT DISABLE THE ENCODER.
+        leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        lMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
-        lMotor.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
-        rMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
-        rMotor.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
+        // Make it so the when positive power applied, lift moves up.
+//        leftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        rightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        lMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
-        rMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
+        // Negate the gravity when stopped
+        //TODO gravity PID coefficients?
+        leftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); // change to brake if bad
+        rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        voltageSensor = hardwareMap.voltageSensor.iterator().next();
+        voltageComp = VOLTAGE_WHEN_LIFT_TUNED / voltageSensor.getVoltage();
     }
 
     @Override
-    public void periodic() {}
+    public void periodic(){
+        // happens every loop
+    }
+
+    public void setLiftPower(double power){
+        //TODO this could be the PID demon
+        leftMotor.setPower(power);
+        rightMotor.setPower(power);
+    }
 
-    private void setPower(double power) {
-        lMotor.setPower(-power);
-        rMotor.setPower(power);
+    public void brake_power(){
+        setLiftPower(0);
     }
 
-    public void setSlidesPower(double power) {
-        lMotor.setPower(-power);
-        rMotor.setPower(power);
+    public double getLiftPosition(){
+        return rightMotor.getCurrentPosition();
     }
 
-    public void runManual(double dr4bp) {
-        lMotor.setPower(-dr4bp);
-        rMotor.setPower(dr4bp);
+    public double getLiftVelocity(){
+        return leftMotor.getVelocity();
     }
 
-    public void stop() {
-        setPower(0.1);
+    public boolean atUpperLimit(){
+        return getLiftPosition() > 2350;
     }
 
-    public double getPos() {
-        return lMotor.getCurrentPosition();
+    public boolean atLowerLimit(){
+        return getLiftPosition() < 5;
     }
 
-    public void goToPosition(double targetPos) {
-        double output = pid.update(lMotor.getCurrentPosition(), targetPos);
-        lMotor.setPower(output);
-        rMotor.setPower(-output);
+    public void setLeftPower (double power) {
+        leftMotor.setPower(power);
     }
-    public boolean atUpper() {
-        return getPos() > 600;
+
+    public void setRightMotor (double power) {
+        rightMotor.setPower(power);
     }
 
-    public boolean atLower() {
-        return getPos() < 5;
+    public void resetLiftPosition(){
+        leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
     }
 
-    public void resetSlide() {
-        lMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        rMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        lMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        rMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    public double getVoltageComp(){
+        return voltageComp;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/V4B.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/V4B.java
index 9ae340921187..02be56f77925 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/V4B.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/V4B.java
@@ -1,15 +1,18 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
 import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.arcrobotics.ftclib.trajectory.TrapezoidProfile;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+import com.qualcomm.robotcore.util.ElapsedTime;
 
 public class V4B extends SubsystemBase {
-
+    // Enum states
     public enum V4BState {
-        RETRACT(0.01),
-        EXTEND(0.45);
+        EXTEND(0.45),
+        RETRACT(0.01);
 
         public double position;
 
@@ -18,41 +21,83 @@ public enum V4BState {
         }
     }
 
-    private Servo servo1, servo2;
+    // Motion profiling
+    private ElapsedTime timer = new ElapsedTime();
+    private double prevPositionTarget = V4BState.RETRACT.position; // retracted is the position initially
+    private TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 1);
+    private TrapezoidProfile motionProfile = new TrapezoidProfile(
+            constraints,
+            new TrapezoidProfile.State(V4BState.RETRACT.position, 0),
+            new TrapezoidProfile.State(V4BState.RETRACT.position, 0)
+    );
 
-    public V4B(HardwareMap hardwareMap) {
-        servo1 = hardwareMap.get(Servo.class, "leftV4B"); // Left Side
-        servo2 = hardwareMap.get(Servo.class, "rightV4B"); // Right Side
 
+    // Initial initialization
+    Servo leftV4B, rightV4B;
+    final double deadband = 0.05;
+    public V4B(HardwareMap hardwareMap) {
+        leftV4B = hardwareMap.get(ServoImplEx.class, "leftV4B"); // Left Side
+        rightV4B = hardwareMap.get(ServoImplEx.class, "rightV4B"); // Right Side
     }
 
+    // Overriden functions
     @Override
-    public void periodic() {}
+    public void periodic() {
+        if(!motionProfile.isFinished(timer.seconds())) {
+            // Read the current target for the profile
+            double leftPosition = motionProfile.calculate(timer.seconds()).position;
+            double rightPosition = motionProfile.calculate(timer.seconds()).position;
+
+            // Set servo positions according to the profile
+            leftV4B.setPosition(leftPosition);
+//            rightV4B.setPosition(rightPosition);
+        }
+    }
+
+    // Normal functions
+    public void setMotionPosition(double target) {
+        if(prevPositionTarget != target){
+            motionProfile = new TrapezoidProfile(
+                    constraints,
+                    new TrapezoidProfile.State(target, 0),
+                    new TrapezoidProfile.State(this.getPosition(), 0)
+            );
+
+            //Reset the timer
+            timer.reset();
+        }
+        prevPositionTarget = target;
+    }
+
+    public void setPosition(V4BState target) {
+        setPosition(target.position);
+    }
 
-    public void setPosition(V4BState state) {
-        servo1.setPosition(state.position);
-        servo2.setPosition(state.position);
+    public void setPosition (double forced) {
+        leftV4B.setPosition(forced);
+        rightV4B.setPosition(forced);
     }
 
     public void extend() {
-        servo1.setPosition(V4BState.EXTEND.position);
-        servo2.setPosition(V4BState.EXTEND.position);
+//        setMotionPosition(V4BState.EXTEND.position);
+        setPosition(V4BState.EXTEND.position);
     }
 
     public void retract() {
-        servo1.setPosition(V4BState.RETRACT.position);
-        servo2.setPosition(V4BState.RETRACT.position);
+//        setMotionPosition(V4BState.RETRACT.position);
+        setPosition(V4BState.RETRACT.position);
     }
 
     public double getPosition() {
-        return servo1.getPosition();
+        return leftV4B.getPosition();
     }
 
     public void togglePower() {
-        if (getPosition() == V4BState.RETRACT.position) {
-            setPosition(V4BState.EXTEND);
+        // If the arm is in the retract position
+        if (Math.abs(getPosition() - V4BState.RETRACT.position) < deadband) {
+            extend();
         } else {
-            setPosition(V4BState.RETRACT);
+            retract();
         }
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Winch.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Winch.java
deleted file mode 100644
index 3f3c7bbecf91..000000000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Winch.java
+++ /dev/null
@@ -1,34 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.arcrobotics.ftclib.command.SubsystemBase;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-public class Winch extends SubsystemBase {
-    private DcMotorEx winchMotor;
-
-    public Winch(HardwareMap hardwareMap) {
-        winchMotor = hardwareMap.get(DcMotorEx.class, "winch");
-
-        winchMotor.setDirection(DcMotorSimple.Direction.FORWARD);
-        winchMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
-    }
-
-    @Override
-    public void periodic() {
-        // Add any periodic tasks or checks here
-    }
-
-    public void extendWinch(double power) {
-        winchMotor.setPower(power);
-    }
-
-    public void retractWinch(double power) {
-        winchMotor.setPower(-power);
-    }
-
-    public void stopWinch() {
-        winchMotor.setPower(0);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaCommandTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaCommandTeleOp.java
deleted file mode 100644
index fb7594758f0c..000000000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaCommandTeleOp.java
+++ /dev/null
@@ -1,145 +0,0 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-import com.arcrobotics.ftclib.command.CommandOpMode;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.arcrobotics.ftclib.gamepad.GamepadKeys;
-import com.kauailabs.navx.ftc.AHRS;
-import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.auto.BulkCacheCommand;
-import org.firstinspires.ftc.teamcode.subsystems.Impasta;
-import org.firstinspires.ftc.teamcode.subsystems.Mechanisms;
-import org.firstinspires.ftc.teamcode.subsystems.SlidePID;
-
-
-@TeleOp
-public class ImpastaCommandTeleOp extends CommandOpMode {
-
-    // Declaring hardware variables
-    Impasta impasta;
-    Mechanisms mechanisms;
-
-    //Slide Positions
-    final int HIGH = 200;
-    final int MID = 150;
-    final int LOW = 100;
-    final int GROUND = 10;
-
-    private boolean pidActive = false;
-
-    private DcMotor fl, fr, bl, br, leftSlide, rightSlide, Intake;
-    private boolean doClimbing = false;
-
-    @Override
-    public void initialize() {
-        // Initializing hardware
-        GamepadEx mechanism = new GamepadEx(gamepad2);
-
-        fl = hardwareMap.dcMotor.get("leftFront"); // Drivebase
-        fr = hardwareMap.dcMotor.get("rightFront"); // Drivebase
-        bl = hardwareMap.dcMotor.get("leftRear"); // Drivebase
-        br = hardwareMap.dcMotor.get("rightRear"); // Drivebase
-
-        leftSlide = hardwareMap.dcMotor.get("leftSlide_frontEncoder"); // Slides
-        rightSlide = hardwareMap.dcMotor.get("rightSlide"); // Slides
-        Intake = hardwareMap.dcMotor.get("intake_leftEncoder"); //Pixel Intake
-
-        rightSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
-        if (doClimbing) {
-            leftSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        } else {
-            leftSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        }
-
-
-        AHRS imu = AHRS.getInstance(hardwareMap.get(NavxMicroNavigationSensor.class, "navx"), AHRS.DeviceDataType.kProcessedData);
-
-        impasta = new Impasta(fl, fr, bl, br, leftSlide, rightSlide, Intake, imu);
-        mechanisms = new Mechanisms(gamepad1, gamepad2, hardwareMap);
-
-        schedule(new BulkCacheCommand(hardwareMap));
-
-        // Commands for controlling the lift using buttons
-        mechanism.getGamepadButton(GamepadKeys.Button.DPAD_UP)
-                .whenPressed(new InstantCommand(() -> {pidActive = true;}))
-                .whenPressed(new SlidePID(impasta, HIGH).withTimeout(1500))
-                .whenReleased(new InstantCommand(() -> {pidActive = false;}));
-
-        mechanism.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
-                .whenPressed(new InstantCommand(() -> {pidActive = true;}))
-                .whenPressed(new SlidePID(impasta, MID).withTimeout(1500))
-                .whenReleased(new InstantCommand(() -> {pidActive = false;}));
-
-        mechanism.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)
-                .whenPressed(new InstantCommand(() -> {pidActive = true;}))
-                .whenPressed(new SlidePID(impasta, LOW).withTimeout(1500))
-                .whenReleased(new InstantCommand(() -> {pidActive = false;}));
-
-        mechanism.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
-                .whenPressed(new InstantCommand(() -> {pidActive = true;}))
-                .whenPressed(new SlidePID(impasta, GROUND).withTimeout(1500))
-                .whenReleased(new InstantCommand(() -> {pidActive = false;}));
-
-        // Command for resetting the lift
-        mechanism.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                .whenPressed(new InstantCommand(() -> impasta.reset()));
-
-        impasta.resetSlide();
-
-        telemetry.addLine("Initialization Done");
-        telemetry.update();
-    }
-
-    @Override
-    public void run(){
-        super.run();
-        impasta.driveBaseField(-gamepad1.left_stick_y, -gamepad1.left_stick_x * 1.1, -gamepad1.right_stick_x);
-
-        if (gamepad1.left_bumper || gamepad1.right_bumper) {
-            impasta.reset();
-            gamepad1.rumble(250); // Angle recalibrated
-        }
-
-        impasta.intake(gamepad1.left_trigger - gamepad1.right_trigger);
-
-        if (gamepad2.left_bumper) {
-            impasta.resetSlide();
-        }
-
-        if (!pidActive) {
-            impasta.runManual(gamepad2.left_stick_y, doClimbing);
-            telemetry.addLine("Lift Position: " + -rightSlide.getCurrentPosition());
-        }
-
-        if (gamepad1.triangle) {
-            mechanisms.resetLaunch();
-        } else if (gamepad1.square) {
-            mechanisms.launch();
-        }
-
-        if (gamepad2.right_bumper) {
-            doClimbing = !doClimbing;
-            if (doClimbing) {
-                leftSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            } else {
-                leftSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-            }
-        }
-
-        telemetry.addLine("isClimbing State: " + doClimbing);
-
-        if (gamepad2.circle) {
-            mechanisms.extend();
-        } else if (gamepad2.cross) {
-            mechanisms.retract();
-        }
-
-        mechanisms.outtakes();
-
-        telemetry.update();
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaRobotCentricTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaRobotCentricTeleOp.java
index 34af36672192..d6773b5ba1ca 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaRobotCentricTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaRobotCentricTeleOp.java
@@ -1,124 +1,124 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.subsystems.Impasta;
-
-@TeleOp
-public class ImpastaRobotCentricTeleOp extends LinearOpMode {
-    // Declaring hardware variables
-    private DcMotor fl, fr, bl, br, leftSlide, rightSlide, Winch, Intake;
-    private Servo out1, out2, launchPlane, aimLauncher;
-    private CRServo DRV4BL, DRV4BR;
-    private double up, down, current;
-    Impasta impasta;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        // Initializing motors, servos, and sensors
-        fl = hardwareMap.dcMotor.get("m0"); //Drivebase // ControlHub 2
-        fr = hardwareMap.dcMotor.get("m1"); //Drivebase // ExpansionHub 2
-        bl = hardwareMap.dcMotor.get("m2"); //Drivebase // ControlHub 3
-        br = hardwareMap.dcMotor.get("m3"); //Drivebase // ExpansionHub 3
-
-        leftSlide = hardwareMap.dcMotor.get("Left Slide"); //Slides ControlHub 1
-        rightSlide = hardwareMap.dcMotor.get("Right Slide"); //Slides ExpansionHub 1
-
-        Winch = hardwareMap.dcMotor.get("Winch"); //Climbing ControlHub 0
-        Intake = hardwareMap.dcMotor.get("Intake"); //Pixel // Intake ExpansionHub 0
-
-        DRV4BL = hardwareMap.crservo.get("V4BL"); //Virtual Four Bar Servos // Left Side
-        DRV4BR = hardwareMap.crservo.get("V4BR"); //Virtual Four Bar Servos //Right Side
-
-        launchPlane = hardwareMap.servo.get("lP");
-//        aimLauncher = hardwareMap.servo.get("aL");
-
-        out1 = hardwareMap.servo.get("OL"); //Outtake
-        out2 = hardwareMap.servo.get("OR"); //Outtake
-
-        // Creating an instance of the Impasta class
-        impasta = new Impasta(fl, fr, bl, br, leftSlide, rightSlide, Intake);
-
-        impasta.reset();
-        boolean reset = true;
-
-        boolean DRV4BReset = false;
-
-        telemetry.addLine("Initialization Done, pos reset: " + reset + " DRV4B reset: " + DRV4BReset);
-        telemetry.update();
-
-        // Waiting for the start button to be pressed
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        while (opModeIsActive()) {
-            /** gamepad1                                                                                */
-            // Driving the robot based on gamepad input
-            impasta.driveBaseRobot(gamepad1.left_stick_y, -gamepad1.left_stick_x * 1.1, -gamepad1.right_stick_x);
-
-            // Controlling intake based on trigger input
-            impasta.intake(gamepad1.left_trigger - gamepad1.right_trigger);
-
-            // Resetting IMU yaw angle if left bumper is pressed
-            if (gamepad1.left_bumper || gamepad1.right_bumper) {
-                impasta.reset();
-            }
-
-            /** gamepad2                                                                                */
-            // Controlling slides with gamepad input
-            impasta.setSlidesPower(-gamepad2.left_stick_y);
-            telemetry.addData("PowerForSlides", -gamepad2.left_stick_y);
-
-            telemetry.addLine("SlidePos: " + -leftSlide.getCurrentPosition());
-            telemetry.update();
-
-//            //TODO Test Airplane Aim
-            if (gamepad2.triangle) {
-                launchPlane.setPosition(0);
-            } else if (gamepad2.square) {
-                launchPlane.setPosition(1);
-            }
-
-            // Controlling the Resting/Scoring state of the Virtual Four Bar
-            if (gamepad2.circle) {
-                // Rotate the CR servos clockwise as long as the button is pressed
-                while (gamepad2.circle) {
-                    DRV4BL.setPower(1.0);  // Adjust the power as needed
-                    DRV4BR.setPower(1.0);  // Adjust the power as needed
-                }
-                // Stop the servos when the button is released
-                DRV4BL.setPower(0);
-                DRV4BR.setPower(0);
-
-            } else if (gamepad2.cross) {
-                // Rotate the CR servos counterclockwise as long as the button is pressed
-                while (gamepad2.cross) {
-                    DRV4BL.setPower(-1.0);  // Adjust the power as needed
-                    DRV4BR.setPower(-1.0);  // Adjust the power as needed
-                }
-                // Stop the servos when the button is released
-                DRV4BL.setPower(0);
-                DRV4BR.setPower(0);
-
-            }
-
-            // Outtake switches between scoring and rest position based on button press //Swap later
-            if (gamepad2.left_trigger > 0.3) {
-                out1.setPosition(0.75); // left //lower
-            } else {
-                out1.setPosition(0.55); // left //raise
-            }
-
-            if (gamepad2.right_trigger > 0.3) {
-                out2.setPosition(0.5); // right //lower
-            } else {
-                out2.setPosition(0.6); // right //raise
-            }
-        }
-    }
-}
+//package org.firstinspires.ftc.teamcode.teleop;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+//import com.qualcomm.robotcore.hardware.CRServo;
+//import com.qualcomm.robotcore.hardware.DcMotor;
+//import com.qualcomm.robotcore.hardware.Servo;
+//
+//import org.firstinspires.ftc.teamcode.subsystems.Impasta;
+//
+//@TeleOp
+//public class ImpastaRobotCentricTeleOp extends LinearOpMode {
+//    // Declaring hardware variables
+//    private DcMotor fl, fr, bl, br, leftSlide, rightSlide, Winch, Intake;
+//    private Servo out1, out2, launchPlane, aimLauncher;
+//    private CRServo DRV4BL, DRV4BR;
+//    private double up, down, current;
+//    Impasta impasta;
+//
+//    @Override
+//    public void runOpMode() throws InterruptedException {
+//        // Initializing motors, servos, and sensors
+//        fl = hardwareMap.dcMotor.get("m0"); //Drivebase // ControlHub 2
+//        fr = hardwareMap.dcMotor.get("m1"); //Drivebase // ExpansionHub 2
+//        bl = hardwareMap.dcMotor.get("m2"); //Drivebase // ControlHub 3
+//        br = hardwareMap.dcMotor.get("m3"); //Drivebase // ExpansionHub 3
+//
+//        leftSlide = hardwareMap.dcMotor.get("Left Slide"); //Slides ControlHub 1
+//        rightSlide = hardwareMap.dcMotor.get("Right Slide"); //Slides ExpansionHub 1
+//
+//        Winch = hardwareMap.dcMotor.get("Winch"); //Climbing ControlHub 0
+//        Intake = hardwareMap.dcMotor.get("Intake"); //Pixel // Intake ExpansionHub 0
+//
+//        DRV4BL = hardwareMap.crservo.get("V4BL"); //Virtual Four Bar Servos // Left Side
+//        DRV4BR = hardwareMap.crservo.get("V4BR"); //Virtual Four Bar Servos //Right Side
+//
+//        launchPlane = hardwareMap.servo.get("lP");
+////        aimLauncher = hardwareMap.servo.get("aL");
+//
+//        out1 = hardwareMap.servo.get("OL"); //Outtake
+//        out2 = hardwareMap.servo.get("OR"); //Outtake
+//
+//        // Creating an instance of the Impasta class
+//        impasta = new Impasta(fl, fr, bl, br, leftSlide, rightSlide, Intake);
+//
+//        impasta.reset();
+//        boolean reset = true;
+//
+//        boolean DRV4BReset = false;
+//
+//        telemetry.addLine("Initialization Done, pos reset: " + reset + " DRV4B reset: " + DRV4BReset);
+//        telemetry.update();
+//
+//        // Waiting for the start button to be pressed
+//        waitForStart();
+//
+//        if (isStopRequested()) return;
+//
+//        while (opModeIsActive()) {
+//            /** gamepad1                                                                                */
+//            // Driving the robot based on gamepad input
+//            impasta.driveBaseRobot(gamepad1.left_stick_y, -gamepad1.left_stick_x * 1.1, -gamepad1.right_stick_x);
+//
+//            // Controlling intake based on trigger input
+//            impasta.intake(gamepad1.left_trigger - gamepad1.right_trigger);
+//
+//            // Resetting IMU yaw angle if left bumper is pressed
+//            if (gamepad1.left_bumper || gamepad1.right_bumper) {
+//                impasta.reset();
+//            }
+//
+//            /** gamepad2                                                                                */
+//            // Controlling slides with gamepad input
+//            impasta.setSlidesPower(-gamepad2.left_stick_y);
+//            telemetry.addData("PowerForSlides", -gamepad2.left_stick_y);
+//
+//            telemetry.addLine("SlidePos: " + -leftSlide.getCurrentPosition());
+//            telemetry.update();
+//
+////            //TODO Test Airplane Aim
+//            if (gamepad2.triangle) {
+//                launchPlane.setPosition(0);
+//            } else if (gamepad2.square) {
+//                launchPlane.setPosition(1);
+//            }
+//
+//            // Controlling the Resting/Scoring state of the Virtual Four Bar
+//            if (gamepad2.circle) {
+//                // Rotate the CR servos clockwise as long as the button is pressed
+//                while (gamepad2.circle) {
+//                    DRV4BL.setPower(1.0);  // Adjust the power as needed
+//                    DRV4BR.setPower(1.0);  // Adjust the power as needed
+//                }
+//                // Stop the servos when the button is released
+//                DRV4BL.setPower(0);
+//                DRV4BR.setPower(0);
+//
+//            } else if (gamepad2.cross) {
+//                // Rotate the CR servos counterclockwise as long as the button is pressed
+//                while (gamepad2.cross) {
+//                    DRV4BL.setPower(-1.0);  // Adjust the power as needed
+//                    DRV4BR.setPower(-1.0);  // Adjust the power as needed
+//                }
+//                // Stop the servos when the button is released
+//                DRV4BL.setPower(0);
+//                DRV4BR.setPower(0);
+//
+//            }
+//
+//            // Outtake switches between scoring and rest position based on button press //Swap later
+//            if (gamepad2.left_trigger > 0.3) {
+//                out1.setPosition(0.75); // left //lower
+//            } else {
+//                out1.setPosition(0.55); // left //raise
+//            }
+//
+//            if (gamepad2.right_trigger > 0.3) {
+//                out2.setPosition(0.5); // right //lower
+//            } else {
+//                out2.setPosition(0.6); // right //raise
+//            }
+//        }
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaTeleOp.java
index f56f423f8178..6fc0a7145f22 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ImpastaTeleOp.java
@@ -1,151 +1,135 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DistanceSensor;
-import com.qualcomm.robotcore.hardware.Servo;
-import com.kauailabs.navx.ftc.AHRS;
-import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
-
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.teamcode.subsystems.Impasta;
-
-@Disabled
-@TeleOp
-public class ImpastaTeleOp extends LinearOpMode {
-    // Declaring hardware variables
-    private DcMotor fl, fr, bl, br, leftSlide, rightSlide, Intake;
-    private Servo out1, out2, launchPlane, DRV4BL, DRV4BR;
-    private DistanceSensor leftSensor;
-    private DistanceSensor rightSensor;
-    private AHRS imu;
-    private double up, down, current, distance;
-    private boolean doDetectDistance;
-    Impasta impasta;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        // Initializing motors, servos, and sensors
-        fl = hardwareMap.dcMotor.get("leftFront"); //Drivebase
-        fr = hardwareMap.dcMotor.get("rightFront"); //Drivebase
-        bl = hardwareMap.dcMotor.get("leftRear"); //Drivebase
-        br = hardwareMap.dcMotor.get("rightRear"); //Drivebase
-
-        leftSlide = hardwareMap.dcMotor.get("frontEncoder"); //Slides
-        rightSlide = hardwareMap.dcMotor.get("Right Slide"); //Slides
-
-        Intake = hardwareMap.dcMotor.get("leftEncoder"); //Pixel Intake
-
-        DRV4BL = hardwareMap.servo.get("leftV4B"); //Virtual Four Bar Servos // Left Side
-        DRV4BR = hardwareMap.servo.get("rightV4B"); //Virtual Four Bar Servos //Right Side
-
-        launchPlane = hardwareMap.servo.get("launcher");
-
-        out1 = hardwareMap.servo.get("leftOut"); //Outtake
-        out2 = hardwareMap.servo.get("rightOut"); //Outtake
-
-//        leftSensor = hardwareMap.get(DistanceSensor.class, "leftSensor");
-//        rightSensor = hardwareMap.get(DistanceSensor.class, "rightSensor");
-
-        distance = 1;
-        doDetectDistance = false;
-
-        imu = AHRS.getInstance(hardwareMap.get(NavxMicroNavigationSensor.class, "navx"), AHRS.DeviceDataType.kProcessedData);
-
-        // Creating an instance of the Impasta class
-        impasta = new Impasta(fl, fr, bl, br, leftSlide, rightSlide, Intake, imu);
-
-        impasta.reset();
-        boolean reset = true;
-
-        boolean DRV4BReset = false;
-
-        telemetry.addLine("Initialization Done, pos reset: " + reset + " DRV4B reset: " + DRV4BReset);
-        telemetry.update();
-
-        // Waiting for the start button to be pressed
-        waitForStart();
-
-        if (isStopRequested()) return;
-        // Main TeleOp loop
-        while (opModeIsActive()) {
-            /** gamepad1                                                                                */
-            // Driving the robot based on gamepad input
-            impasta.driveBaseField(-gamepad1.left_stick_y, gamepad1.left_stick_x * 1.1, -gamepad1.right_stick_x);
-
-            // Controlling intake based on trigger input
-            impasta.intake(gamepad1.left_trigger - gamepad1.right_trigger);
-
-            // Resetting IMU yaw angle if left bumper is pressed
-            if (gamepad1.left_bumper || gamepad1.right_bumper) {
-                impasta.reset();
-                gamepad1.rumble(2000);
-            }
-
-            if (gamepad2.left_bumper || gamepad1.right_bumper) {
-                impasta.resetSlide();
-            }
-
-            if (gamepad2.dpad_down && !impasta.atLower()) {
-                impasta.runPID(0);
-            } else if (gamepad2.dpad_left) {
-                impasta.runPID(150);
-            } else if (gamepad2.dpad_right) {
-                impasta.runPID(300);
-            } else if (gamepad2.dpad_up && !impasta.atUpper()) {
-                impasta.runPID(450);
-            } else {
-                impasta.setSlidesPower(gamepad2.left_stick_y);
-            }
-
-            telemetry.addLine("Left Slide Pos: " + leftSlide.getCurrentPosition());
-
-            //TODO Test Airplane Aim
-            if (gamepad2.triangle) {
-                launchPlane.setPosition(0);
-            } else if (gamepad2.square) {
-                launchPlane.setPosition(1);
-            }
-
-            if (gamepad2.circle) {
-                DRV4BL.setPosition(0.65);
-                DRV4BR.setPosition(0.65);
-            } else if (gamepad2.cross) {
-                DRV4BL.setPosition(0);
-                DRV4BR.setPosition(0);
-            }
-
-            if (gamepad1.dpad_left) {
-                doDetectDistance = !doDetectDistance;
-            }
-
-            // Outtake switches between scoring and rest position based on button press //Swap later
-            if (gamepad2.left_trigger > 0.3 /*|| DetectDistance() <= distance*/) {
-                gamepad1.rumble(1000);
-                out1.setPosition(0.75); // left //lower
-            } else {
-                out1.setPosition(0.55); // left //raise
-            }
-
-            if (gamepad2.right_trigger > 0.3 /*|| DetectDistance() <= distance */) {
-                gamepad1.rumble(1000);
-                out2.setPosition(0.5); // right //lower
-            } else {
-                out2.setPosition(0.6); // right //raise
-            }
-
-            telemetry.update();
-        }
-    }
-
-//    public double DetectDistance() {
-//        double whatToDo;
-//        if (doDetectDistance) {
-//            whatToDo = leftSensor.getDistance(DistanceUnit.INCH);
-//        } else {whatToDo = 10;}
-//        return whatToDo;
+//package org.firstinspires.ftc.teamcode.teleop;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+//import com.qualcomm.robotcore.hardware.CRServo;
+//import com.qualcomm.robotcore.hardware.DcMotor;
+//import com.qualcomm.robotcore.hardware.DistanceSensor;
+//import com.qualcomm.robotcore.hardware.Servo;
+//import com.kauailabs.navx.ftc.AHRS;
+//import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
+//
+//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+//import org.firstinspires.ftc.teamcode.subsystems.Drivebase;
+//import org.firstinspires.ftc.teamcode.subsystems.Impasta;
+//import org.firstinspires.ftc.teamcode.subsystems.Mechanisms;
+//
+//@Disabled
+//@TeleOp
+//public class ImpastaTeleOp extends LinearOpMode {
+//    // Declaring hardware variables
+//    private DcMotor fl, fr, bl, br, leftSlide, rightSlide, Intake;
+//    private Servo out1, out2, launchPlane, DRV4BL, DRV4BR;
+//    private DistanceSensor leftSensor;
+//    private DistanceSensor rightSensor;
+//    private AHRS imu;
+//    private double up, down, current, distance;
+//    private boolean doDetectDistance;
+//    Impasta impasta;
+//
+//    @Override
+//    public void runOpMode() throws InterruptedException {
+//        // Initializing motors, servos, and sensors
+//        Mechanisms mechanisms = new Mechanisms()
+//
+////        leftSensor = hardwareMap.get(DistanceSensor.class, "leftSensor");
+////        rightSensor = hardwareMap.get(DistanceSensor.class, "rightSensor");
+//
+//        distance = 1;
+//        doDetectDistance = false;
+//
+//        imu = AHRS.getInstance(hardwareMap.get(NavxMicroNavigationSensor.class, "navx"), AHRS.DeviceDataType.kProcessedData);
+//
+//        // Creating an instance of the Impasta class
+//
+//        boolean reset = true;
+//
+//        boolean DRV4BReset = false;
+//
+//        telemetry.addLine("Initialization Done, pos reset: " + reset + " DRV4B reset: " + DRV4BReset);
+//        telemetry.update();
+//
+//        // Waiting for the start button to be pressed
+//        waitForStart();
+//
+//        if (isStopRequested()) return;
+//        // Main TeleOp loop
+//        while (opModeIsActive()) {
+//            /** gamepad1                                                                                */
+//            // Driving the robot based on gamepad input
+//            impasta.driveBaseField(-gamepad1.left_stick_y, gamepad1.left_stick_x * 1.1, -gamepad1.right_stick_x);
+//
+//            // Controlling intake based on trigger input
+//            impasta.intake(gamepad1.left_trigger - gamepad1.right_trigger);
+//
+//            // Resetting IMU yaw angle if left bumper is pressed
+//            if (gamepad1.left_bumper || gamepad1.right_bumper) {
+//                impasta.reset();
+//                gamepad1.rumble(2000);
+//            }
+//
+//            if (gamepad2.left_bumper || gamepad1.right_bumper) {
+//                impasta.resetSlide();
+//            }
+//
+//            if (gamepad2.dpad_down && !impasta.atLower()) {
+//                impasta.runPID(0);
+//            } else if (gamepad2.dpad_left) {
+//                impasta.runPID(150);
+//            } else if (gamepad2.dpad_right) {
+//                impasta.runPID(300);
+//            } else if (gamepad2.dpad_up && !impasta.atUpper()) {
+//                impasta.runPID(450);
+//            } else {
+//                impasta.setSlidesPower(gamepad2.left_stick_y);
+//            }
+//
+//            telemetry.addLine("Left Slide Pos: " + leftSlide.getCurrentPosition());
+//
+//            //TODO Test Airplane Aim
+//            if (gamepad2.triangle) {
+//                launchPlane.setPosition(0);
+//            } else if (gamepad2.square) {
+//                launchPlane.setPosition(1);
+//            }
+//
+//            if (gamepad2.circle) {
+//                DRV4BL.setPosition(0.65);
+//                DRV4BR.setPosition(0.65);
+//            } else if (gamepad2.cross) {
+//                DRV4BL.setPosition(0);
+//                DRV4BR.setPosition(0);
+//            }
+//
+//            if (gamepad1.dpad_left) {
+//                doDetectDistance = !doDetectDistance;
+//            }
+//
+//            // Outtake switches between scoring and rest position based on button press //Swap later
+//            if (gamepad2.left_trigger > 0.3 /*|| DetectDistance() <= distance*/) {
+//                gamepad1.rumble(1000);
+//                out1.setPosition(0.75); // left //lower
+//            } else {
+//                out1.setPosition(0.55); // left //raise
+//            }
+//
+//            if (gamepad2.right_trigger > 0.3 /*|| DetectDistance() <= distance */) {
+//                gamepad1.rumble(1000);
+//                out2.setPosition(0.5); // right //lower
+//            } else {
+//                out2.setPosition(0.6); // right //raise
+//            }
+//
+//            telemetry.update();
+//        }
 //    }
-}
+//
+////    public double DetectDistance() {
+////        double whatToDo;
+////        if (doDetectDistance) {
+////            whatToDo = leftSensor.getDistance(DistanceUnit.INCH);
+////        } else {whatToDo = 10;}
+////        return whatToDo;
+////    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/LiftTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/LiftTester.java
new file mode 100644
index 000000000000..e6556f9d45e7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/LiftTester.java
@@ -0,0 +1,99 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import android.annotation.SuppressLint;
+
+import com.arcrobotics.ftclib.command.CommandOpMode;
+import com.arcrobotics.ftclib.command.PerpetualCommand;
+import com.arcrobotics.ftclib.command.button.Trigger;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.kauailabs.navx.ftc.AHRS;
+import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.IMU;
+
+import org.firstinspires.ftc.teamcode.auto.BulkCacheCommand;
+import org.firstinspires.ftc.teamcode.commands.liftcommands.ManualLiftCommand;
+import org.firstinspires.ftc.teamcode.commands.liftcommands.ManualLiftResetCommand;
+import org.firstinspires.ftc.teamcode.commands.presets.MoveToScoringCommand;
+import org.firstinspires.ftc.teamcode.commands.presets.RetractOuttakeCommand;
+import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+import org.firstinspires.ftc.teamcode.subsystems.Slides;
+import org.firstinspires.ftc.teamcode.subsystems.V4B;
+
+@TeleOp (name="Lift Tester")
+public class LiftTester extends CommandOpMode {
+    //    private Drivebase drivebase;
+    private Slides lift;
+    private V4B v4b;
+    private Outtake outtake;
+    private ManualLiftCommand manualLiftCommand;
+    private ManualLiftResetCommand manualLiftResetCommand;
+
+    @Override
+    public void initialize(){
+        // Use a bulk cache to loop faster using old values instead of blocking a thread kinda
+        schedule(new BulkCacheCommand(hardwareMap));
+        GamepadEx driver = new GamepadEx(gamepad1);
+        GamepadEx manipulator = new GamepadEx(gamepad2);
+
+        lift = new Slides(hardwareMap);
+        v4b = new V4B(hardwareMap);
+        outtake = new Outtake(hardwareMap);
+
+        outtake.transport();
+        v4b.retract();
+
+        manualLiftCommand = new ManualLiftCommand(lift, manipulator);
+        manualLiftResetCommand = new ManualLiftResetCommand(lift, manipulator);
+
+        lift.setDefaultCommand(new PerpetualCommand(manualLiftCommand));
+
+        new Trigger(() -> manipulator.getLeftY() > 0.4)
+                .whenActive(new MoveToScoringCommand(lift, v4b, outtake, MoveToScoringCommand.Presets.HIGH)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+
+        new Trigger(() -> manipulator.getLeftY() < -0.6)
+                .whenActive(new RetractOuttakeCommand(lift, v4b, outtake)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+
+        //Mid preset
+        new Trigger(() -> manipulator.getRightY() > -0.4)
+                .whenActive(new MoveToScoringCommand(lift, v4b, outtake, MoveToScoringCommand.Presets.MID)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+
+        //Short preset
+        new Trigger(() -> manipulator.getRightY() < 0.4)
+                .whenActive(new MoveToScoringCommand(lift, v4b, outtake, MoveToScoringCommand.Presets.SHORT)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+    }
+
+
+    @SuppressLint("DefaultLocale")
+    @Override
+    public void run() {
+        super.run();
+
+        telemetry.addData("position", lift.getLiftPosition());
+
+        if (gamepad1.left_bumper){
+            v4b.setPosition(V4B.V4BState.RETRACT);
+        } else if(gamepad1.right_bumper){
+            v4b.setPosition(V4B.V4BState.EXTEND);
+        }
+
+        if (gamepad2.right_bumper) {
+            outtake.transport();
+        } else if (gamepad2.left_bumper) {
+            outtake.open();
+        }
+
+        telemetry.update();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleOp.java
new file mode 100644
index 000000000000..0ca00b34534a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleOp.java
@@ -0,0 +1,187 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.arcrobotics.ftclib.command.CommandOpMode;
+import com.arcrobotics.ftclib.command.PerpetualCommand;
+import com.arcrobotics.ftclib.command.button.Trigger;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.auto.BulkCacheCommand;
+import org.firstinspires.ftc.teamcode.commands.liftcommands.ManualLiftCommand;
+import org.firstinspires.ftc.teamcode.commands.liftcommands.ManualLiftResetCommand;
+import org.firstinspires.ftc.teamcode.commands.presets.MoveToScoringCommand;
+import org.firstinspires.ftc.teamcode.commands.presets.RetractOuttakeCommand;
+import org.firstinspires.ftc.teamcode.subsystems.Drivebase;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Mechanisms;
+import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+import org.firstinspires.ftc.teamcode.subsystems.Slides;
+import org.firstinspires.ftc.teamcode.subsystems.V4B;
+
+@TeleOp
+public class MainTeleOp extends CommandOpMode {
+
+    private Slides lift;
+    private V4B v4b;
+    private Outtake outtake;
+    public Drivebase drivebase;
+    public Intake intake;
+
+    private ManualLiftCommand manualLiftCommand;
+    private ManualLiftResetCommand manualLiftResetCommand;
+    public static final double DEBOUNCE_THRESHOLD = 0.05;
+
+    private boolean doClimbing = false;
+
+    @Override
+    public void initialize() {
+        schedule(new BulkCacheCommand(hardwareMap));
+        GamepadEx driver = new GamepadEx(gamepad1);
+        GamepadEx manipulator = new GamepadEx(gamepad2);
+
+        lift = new Slides(hardwareMap);
+        v4b = new V4B(hardwareMap);
+        outtake = new Outtake(hardwareMap);
+        drivebase = new Drivebase(hardwareMap);
+        intake = new Intake(hardwareMap);
+
+        outtake.transport();
+        v4b.retract();
+
+        manualLiftCommand = new ManualLiftCommand(lift, manipulator);
+        manualLiftResetCommand = new ManualLiftResetCommand(lift, manipulator);
+
+        lift.setDefaultCommand(new PerpetualCommand(manualLiftCommand));
+
+        new Trigger(() -> manipulator.getLeftY() > 0.4)
+                .whenActive(new MoveToScoringCommand(lift, v4b, outtake, MoveToScoringCommand.Presets.HIGH)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+
+        new Trigger(() -> manipulator.getLeftY() < -0.6)
+                .whenActive(new RetractOuttakeCommand(lift, v4b, outtake)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+
+        //Mid preset
+        new Trigger(() -> manipulator.getRightY() > -0.4)
+                .whenActive(new MoveToScoringCommand(lift, v4b, outtake, MoveToScoringCommand.Presets.MID)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+
+        //Short preset
+        new Trigger(() -> manipulator.getRightY() < 0.4)
+                .whenActive(new MoveToScoringCommand(lift, v4b, outtake, MoveToScoringCommand.Presets.SHORT)
+                        .withTimeout(1900)
+                        .interruptOn(() -> manualLiftCommand.isManualActive()));
+
+//        manipulator.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
+//                .whenHeld(manualLiftResetCommand);
+
+        telemetry.addLine("Initialization Done");
+        telemetry.update();
+    }
+
+    @Override
+    public void run(){
+        super.run();
+
+        drivebase.drive(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
+
+        if (gamepad1.square) {
+            drivebase.reset();
+            gamepad1.rumble(250); // Angle recalibrated
+        }
+
+        if (debounce(gamepad1.left_trigger) || debounce(gamepad1.right_trigger)) {
+            double powerRight = gamepad1.left_trigger;
+            double powerLeft = gamepad1.right_trigger;
+//            if (intakeCommand.isScheduled()) {
+//                telemetry.addLine("ICSX2");
+//                intakeCommand.cancel();
+//            }
+            if (powerRight > powerLeft) {
+                intake.setPower(1);
+            } else if (powerLeft > powerRight) {
+                intake.setPower(-1);
+            }
+//        } else if (intakeCommand.isScheduled()){
+//            telemetry.addLine("Intake Command Scheduled");
+        } else {
+            intake.setPower(0);
+        }
+
+//        if (gamepad1.left_trigger > 0.05) {
+//            intake.outtake();
+//        } else if (gamepad1.right_trigger > 0.05) {
+//            intake.intake();
+//        } else {
+//            intake.forceStop();
+//        }
+
+        if (gamepad1.left_bumper){
+            v4b.setPosition(V4B.V4BState.RETRACT);
+        } else if(gamepad1.right_bumper){
+            v4b.setPosition(V4B.V4BState.EXTEND);
+        }
+
+        if (gamepad2.right_bumper) {
+            outtake.transport();
+        } else if (gamepad2.left_bumper) {
+            outtake.open();
+        }
+
+
+//        if (gamepad2.left_bumper) {
+//            impasta.resetSlide();
+//        }
+//
+//        if (!pidActive) {
+//            impasta.runManual(gamepad2.left_stick_y, doClimbing);
+//            telemetry.addLine("Lift Position: " + -rightSlide.getCurrentPosition());
+//        }
+
+//        if (gamepad1.triangle) {
+//            mechanisms.launcher.reset();
+//        } else if (gamepad1.square) {
+//            mechanisms.launcher.launch();
+//        }
+
+//        if (gamepad2.right_bumper) {
+//            doClimbing = !doClimbing;
+//            if (doClimbing) {
+//                leftSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+//            } else {
+//                leftSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+//            }
+//        }
+
+        telemetry.addLine("isClimbing State: " + doClimbing);
+        telemetry.addData("imuHeading", drivebase.getCorrectedYaw());
+        telemetry.addData("imuNONCO", drivebase.imu.getYaw());
+
+
+
+//        if (gamepad2.square) {
+//            mechanisms.drivebase.frontLeft.setPower(1);
+//        } else if (gamepad2.triangle) {
+//            mechanisms.drivebase.frontRight.setPower(1);
+//        } else if (gamepad2.cross) {
+//            mechanisms.drivebase.backLeft.setPower(1);
+//        } else if (gamepad2.circle) {
+//            mechanisms.drivebase.backRight.setPower(1);
+//        }
+
+//
+//        mechanisms.outtake.outtakes();
+
+        telemetry.update();
+    }
+
+
+    public static boolean debounce(double input) {
+        return Math.abs(input) > DEBOUNCE_THRESHOLD;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OuttakeTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OuttakeTester.java
index bca065e42614..612c6c3f97cc 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OuttakeTester.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OuttakeTester.java
@@ -1,46 +1,46 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.teamcode.subsystems.Outtake;
-import org.firstinspires.ftc.teamcode.subsystems.V4B;
-
-@TeleOp
-public class OuttakeTester extends LinearOpMode {
-    Outtake outtake;
-    V4B v4b;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Gamepad p1 = gamepad1;;
-        outtake = new Outtake(p1, hardwareMap);
-        v4b = new V4B(hardwareMap);
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        while (opModeIsActive()) {
-            if (p1.cross) {
-                v4b.retract();
-            } else if (p1.circle){
-                v4b.extend();
-            }
-
-            telemetry.addLine("V4B Pos: " + v4b.getPosition());
-
-            if (p1.square) {
-                outtake.open();
-            } else if (p1.triangle) {
-                outtake.close();
-            }
-
-            telemetry.addLine("Outtake Pos: " + outtake.getPosition());
-            telemetry.addLine("Left Distance: " + outtake.returnDistanceLeft());
-            telemetry.addLine("Right Distance: " + outtake.returnDistanceRight());
-            telemetry.update();
-        }
-    }
-}
+//package org.firstinspires.ftc.teamcode.teleop;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+//import com.qualcomm.robotcore.hardware.Gamepad;
+//
+//import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+//import org.firstinspires.ftc.teamcode.subsystems.V4B;
+//
+//@TeleOp
+//public class OuttakeTester extends LinearOpMode {
+//    Outtake outtake;
+//    V4B v4b;
+//
+//    @Override
+//    public void runOpMode() throws InterruptedException {
+//        Gamepad p1 = gamepad1;;
+//        outtake = new Outtake(p1, hardwareMap);
+//        v4b = new V4B(hardwareMap);
+//
+//        waitForStart();
+//
+//        if (isStopRequested()) return;
+//
+//        while (opModeIsActive()) {
+//            if (p1.cross) {
+//                v4b.retract();
+//            } else if (p1.circle){
+//                v4b.extend();
+//            }
+//
+//            telemetry.addLine("V4B Pos: " + v4b.getPosition());
+//
+//            if (p1.square) {
+//                outtake.open();
+//            } else if (p1.triangle) {
+//                outtake.close();
+//            }
+//
+//            telemetry.addLine("Outtake Pos: " + outtake.getPosition());
+//            telemetry.addLine("Left Distance: " + outtake.returnDistanceLeft());
+//            telemetry.addLine("Right Distance: " + outtake.returnDistanceRight());
+//            telemetry.update();
+//        }
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SlidePIDTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SlidePIDTest.java
index a07cb415a531..2b3859230987 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SlidePIDTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SlidePIDTest.java
@@ -1,51 +1,51 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.subsystems.Impasta;
-import org.firstinspires.ftc.teamcode.subsystems.PID;
-
-@TeleOp
-public class SlidePIDTest extends LinearOpMode {
-
-    DcMotor leftSlide;
-    DcMotor rightSlide;
-
-    Impasta impasta;
-    PID pid = new PID(0.5, 0, 0);
-
-    @Override
-    public void runOpMode() {
-
-        leftSlide = hardwareMap.dcMotor.get("frontEncoder");
-        rightSlide = hardwareMap.dcMotor.get("Right Slide");
-
-        impasta = new Impasta(leftSlide, rightSlide);
-
-        impasta.resetSlide();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        while (opModeIsActive()) {
-            double pos = leftSlide.getCurrentPosition();
-            telemetry.addData("Pos: ", pos);
-            telemetry.update();
-
-            if (gamepad2.dpad_down && !impasta.atLower()) {
-                impasta.runPID(0);
-            } else if (gamepad2.dpad_left) {
-                impasta.runPID(150);
-            } else if (gamepad2.dpad_right) {
-                impasta.runPID(300);
-            } else if (gamepad2.dpad_up && !impasta.atUpper()) {
-                impasta.runPID(450);
-            } else {
-                impasta.setSlidesPower(gamepad2.left_stick_y);
-            }
-        }
-    }
-}
+//package org.firstinspires.ftc.teamcode.teleop;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+//import com.qualcomm.robotcore.hardware.DcMotor;
+//
+//import org.firstinspires.ftc.teamcode.subsystems.Impasta;
+//import org.firstinspires.ftc.teamcode.subsystems.PID;
+//
+//@TeleOp
+//public class SlidePIDTest extends LinearOpMode {
+//
+//    DcMotor leftSlide;
+//    DcMotor rightSlide;
+//
+//    Impasta impasta;
+//    PID pid = new PID(0.5, 0, 0);
+//
+//    @Override
+//    public void runOpMode() {
+//
+//        leftSlide = hardwareMap.dcMotor.get("frontEncoder");
+//        rightSlide = hardwareMap.dcMotor.get("Right Slide");
+//
+//        impasta = new Impasta(leftSlide, rightSlide);
+//
+//        impasta.resetSlide();
+//
+//        waitForStart();
+//
+//        if (isStopRequested()) return;
+//
+//        while (opModeIsActive()) {
+//            double pos = leftSlide.getCurrentPosition();
+//            telemetry.addData("Pos: ", pos);
+//            telemetry.update();
+//
+//            if (gamepad2.dpad_down && !impasta.atLower()) {
+//                impasta.runPID(0);
+//            } else if (gamepad2.dpad_left) {
+//                impasta.runPID(150);
+//            } else if (gamepad2.dpad_right) {
+//                impasta.runPID(300);
+//            } else if (gamepad2.dpad_up && !impasta.atUpper()) {
+//                impasta.runPID(450);
+//            } else {
+//                impasta.setSlidesPower(gamepad2.left_stick_y);
+//            }
+//        }
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testers/ClawScanner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testers/ClawScanner.java
new file mode 100644
index 000000000000..5fcc79e4f90c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testers/ClawScanner.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.testers;
+
+import com.arcrobotics.ftclib.command.CommandOpMode;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+import org.firstinspires.ftc.teamcode.util.ServoModule;
+
+@TeleOp (name="Claw Scanner")
+public class ClawScanner extends CommandOpMode {
+    ServoModule mod1;
+    GamepadEx driver, manipulator;
+    String servoToTest = "leftOut";
+    Outtake outtake;
+
+    @Override
+    public void initialize() {
+        driver = new GamepadEx(gamepad1);
+        manipulator = new GamepadEx(gamepad2);
+
+        outtake = new Outtake(hardwareMap);
+
+        mod1 = new ServoModule(hardwareMap, servoToTest, driver);
+    }
+    @Override
+    public void run() {
+        super.run();
+
+        pseudoPeriodic(servoToTest, mod1);
+
+        if(gamepad2.y){
+            telemetry.addData("Servo off", servoToTest);
+            mod1.mode(false);
+        } else if(gamepad2.a){
+            telemetry.addData("Servo on", servoToTest);
+            mod1.mode(true);
+        }
+
+        telemetry.update();
+    }
+
+    private void pseudoPeriodic (String tabulate, ServoModule mod) {
+        outtake.setPosition(mod.getPos());
+
+        telemetry.addData(tabulate+"\n", mod.getState());
+    }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CrossBindings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CrossBindings.java
new file mode 100644
index 000000000000..b989a418ca42
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CrossBindings.java
@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+
+public class CrossBindings {
+
+    // PLAYSTATION BINDINGS FOR THE CONTROLLERS!
+    public static final GamepadKeys.Button circle = GamepadKeys.Button.B;
+    public static final GamepadKeys.Button cross = GamepadKeys.Button.A;
+    public static final GamepadKeys.Button triangle = GamepadKeys.Button.Y;
+    public static final GamepadKeys.Button square = GamepadKeys.Button.X;
+
+    public static final GamepadKeys.Button share = GamepadKeys.Button.BACK;
+    public static final GamepadKeys.Button options = GamepadKeys.Button.START;
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ServoModule.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ServoModule.java
new file mode 100644
index 000000000000..33b78a20ab02
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ServoModule.java
@@ -0,0 +1,95 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+
+public class ServoModule {
+    private ServoImplEx thisServo;
+
+    private double stepValue = 0.1;
+    private double pos = 0;
+
+    private double pos1, pos2;
+
+    public ServoModule(HardwareMap hardwareMap, String deviceName, GamepadEx gamepad, double initialPos) {
+        this(hardwareMap, deviceName, gamepad);
+        pos = initialPos;
+    }
+
+    public void mode (boolean on) {
+        if (on) {
+            thisServo.setPwmEnable();
+        }
+        else if (!on){
+            thisServo.setPwmDisable();
+        }
+    }
+
+    public ServoModule(HardwareMap hardwareMap, String deviceName, GamepadEx gamepad) {
+        thisServo = hardwareMap.get(ServoImplEx.class, deviceName);
+
+        gamepad.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenPressed(
+                (()->{ stepValue /= 10; })
+        );
+        gamepad.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whenPressed(
+                (()->{ stepValue *= 10; })
+        );
+
+        gamepad.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(
+                (()->{ if (pos <= 1 - stepValue) pos += stepValue; })
+        );
+        gamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(
+                (()->{ if (pos >= 0 + stepValue) pos -= stepValue; })
+        );
+    }
+
+    public String getState () {
+        return String.format("POS %f\nSTEP %f", pos, stepValue);
+    }
+
+    public void position1 (double p1) {
+        pos1 = p1;
+    }
+
+    public void position1 () {
+        pos1 = pos;
+    }
+
+    public void position2 () {
+        pos2 = pos;
+    }
+
+    public void position2 (double p2) {
+        pos2 = p2;
+    }
+
+    public void goToPos1 () {
+        pos = pos1;
+    }
+
+    public void goToPos2 () {
+        pos = pos2;
+    }
+
+    public double getPos () {
+        return pos;
+    }
+
+    public double getPos1 () {
+        return pos1;
+    }
+    public double getPos2 () {
+        return pos2;
+    }
+
+
+    public void periodic() {
+        thisServo.setPosition(pos);
+    }
+
+    public double returnPos(){
+        return pos;
+    }
+}
\ No newline at end of file
diff --git a/build.gradle b/build.gradle
index 8969a41590b4..adaf4cb1604b 100644
--- a/build.gradle
+++ b/build.gradle
@@ -5,13 +5,17 @@
  */
 
 buildscript {
+    ext.kotlin_version = '1.9.21'
+
     repositories {
         mavenCentral()
         google()
     }
+
     dependencies {
         // Note for FTC Teams: Do not modify this yourself.
         classpath 'com.android.tools.build:gradle:7.2.0'
+        classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version"
     }
 }