diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
index bdba1b3..e3f53ef 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
@@ -17,9 +17,9 @@ class DriveCommand(
 
     override fun execute() {
         subsystem.drive(
-            leftY = zonedDrive(-leftY.invoke(), zoneVal).pow(3),
-            leftX = zonedDrive(leftX.invoke(), zoneVal).pow(3),
-            rightX = zonedDrive(rightX.invoke(), zoneVal).pow(3),
+            leftY = zonedDrive(-leftY.invoke() * 0.9, zoneVal).pow(3),
+            leftX = zonedDrive(leftX.invoke() * 0.9, zoneVal).pow(3),
+            rightX = zonedDrive(rightX.invoke() * 0.9, zoneVal).pow(3),
         )
     }
 
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt
index 4587917..9fb0a0b 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt
@@ -1,10 +1,10 @@
 package org.firstinspires.ftc.teamcode.commands.elevator
 
 import com.arcrobotics.ftclib.command.CommandBase
-import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem
 
 class SpinDownCommand(
-    private val subsystem: OpenSlidesSubsystem
+    private val subsystem: OpenElevatorSubsystem
 ) : CommandBase() {
 
 
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt
index 75781a5..fd8c755 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt
@@ -1,10 +1,10 @@
 package org.firstinspires.ftc.teamcode.commands.elevator
 
 import com.arcrobotics.ftclib.command.CommandBase
-import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem
 
 class SpinUpCommand(
-    private val subsystem: OpenSlidesSubsystem
+    private val subsystem: OpenElevatorSubsystem
 ) : CommandBase() {
 
     override fun execute() {
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt
new file mode 100644
index 0000000..6f0ab52
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt
@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.commands.intake
+
+import com.arcrobotics.ftclib.command.CommandBase
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
+
+class IntakeBeltCommand(
+    private val intakeBelt: Boolean,
+    private val intakeBeltSubsystem: IntakeBeltSubsystem
+) : CommandBase() {
+    override fun execute() {
+        if (intakeBelt) {
+            intakeBeltSubsystem.intakePos()
+        }
+        else {
+            intakeBeltSubsystem.outtakePos()
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt
index b994fd8..bd9429c 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt
@@ -15,8 +15,4 @@ class IntakeCommand(
             subsystem.outtake()
         }
     }
-
-    override fun end(interrupted: Boolean) {
-        subsystem.stop()
-    }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
index 198ff1a..b607006 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
@@ -1,8 +1,8 @@
 package org.firstinspires.ftc.teamcode.constants
 
 enum class ArmConstants(val value: Double) {
-    kP(1.0),
+    kP(2.0),
     kI(0.0),
-    kD(0.08),
-    kCos(0.004)
+    kD(0.0),
+    kCos(0.02)
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt
index 492b5f1..708d610 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt
@@ -5,7 +5,7 @@ import com.acmerobotics.roadrunner.Pose2d
 enum class AutoStartPose(val startPose: Pose2d) {
     BLUE_LEFT(
         Pose2d(
-            -12.0, 62.0, Math.toRadians(270.0)
+            38.0, 62.0, Math.toRadians(270.0)
         )
     ),
 
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt
index f2bf6c0..07234d5 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt
@@ -20,8 +20,8 @@ enum class ControlBoard(val deviceName: String) {
     SLIDES_RIGHT("rightSlideString"),
 
     //Intake
-    INTAKE(""),
+    INTAKE("intakeRoller"),
+    INTAKE_BELT("intakeBelt")
+
 
-    // Camera
-    CAMERA("lifeCam")
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt
index 615497f..f04447e 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt
@@ -1,7 +1,11 @@
 package org.firstinspires.ftc.teamcode.constants
 
 enum class SlidesConstants(val value: Double) {
-    kP(0.0),
+    kP(0.7),
     kI(0.0),
     kD(0.0),
+    kG(0.12),
+
+    MAX_HEIGHT_TICKS(2081.0),
+    MAX_HEIGHT_INCH(33.329)
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt
new file mode 100644
index 0000000..c0c61cf
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt
@@ -0,0 +1,140 @@
+package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.constants.AutoStartPose
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+
+@Autonomous(name = "Blue 1 + 1 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
+class Blue1Plus1: OpMode() {
+    private lateinit var armLeft: Motor
+    private lateinit var armRight: Motor
+    private lateinit var elevatorLeft: Motor
+    private lateinit var elevatorRight: Motor
+
+    private lateinit var intake: Servo
+    private lateinit var intakeBelt: Servo
+
+    private lateinit var driveSubsystem: DriveSubsystem
+    private lateinit var intakeSubsystem: IntakeSubsystem
+    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+    private lateinit var armSubsystem: ArmSubsystem
+    private lateinit var elevatorSubsystem: ElevatorSubsystem
+
+    override fun init() {
+        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+
+        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
+        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+
+        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
+
+        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+
+        driveSubsystem = DriveSubsystem(hardwareMap)
+        intakeSubsystem = IntakeSubsystem(intake)
+        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
+        armSubsystem = ArmSubsystem(armRight, armLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
+
+        val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
+            .addTemporalMarker(0.1) {
+                intakeSubsystem.intake()
+                intakeBeltSubsystem.intakePos()
+            }
+            .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0))
+            .back(4.0)
+            .addTemporalMarker(3.0) {
+                armSubsystem.setpoint = Math.toRadians(98.0)
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(4.0) {
+                elevatorSubsystem.setpoint = 30.0
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(5.0) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(2.0)
+            .addTemporalMarker(7.0) {
+                intakeSubsystem.outtake()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(7.5) {
+                intakeBeltSubsystem.intakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(9.5) {
+                armSubsystem.setpoint = 0.0
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(1.0)
+            .turn(Math.toRadians(38.0))
+            .addTemporalMarker(11.0) {
+                elevatorSubsystem.setpoint = 31.0
+            }
+            .waitSeconds(2.0)
+            .addTemporalMarker(13.0) {
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(14.0) {
+                intakeSubsystem.intake()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(15.0) {
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(2.0)
+            .turn(Math.toRadians(-40.0))
+            .addTemporalMarker(17.0) {
+                armSubsystem.setpoint = Math.toRadians(98.0)
+            }
+            .waitSeconds(1.0)
+            .back(0.5)
+            .addTemporalMarker(18.0) {
+                elevatorSubsystem.setpoint = 33.0
+            }
+            .waitSeconds(3.0)
+            .addTemporalMarker(21.0) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(22.0) {
+                intakeSubsystem.outtake()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(23.0) {
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .splineToSplineHeading(Pose2d(25.0, 10.0, Math.toRadians(0.0)), Math.toRadians(180.0))
+            .build()
+
+        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
+        driveSubsystem.followTrajectorySequenceAsync(trajectory)
+    }
+
+    override fun loop() {
+        driveSubsystem.update()
+
+        elevatorSubsystem.periodic()
+        armSubsystem.periodic()
+
+        telemetry.addData("Arm Position", armSubsystem.armAngle)
+        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
+        telemetry.update()
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt
new file mode 100644
index 0000000..b74cf04
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt
@@ -0,0 +1,212 @@
+package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.constants.AutoStartPose
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+
+@Autonomous(name = "Blue 1 + 2 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
+class Blue1Plus2: OpMode() {
+    private lateinit var armLeft: Motor
+    private lateinit var armRight: Motor
+    private lateinit var elevatorLeft: Motor
+    private lateinit var elevatorRight: Motor
+
+    private lateinit var intake: Servo
+    private lateinit var intakeBelt: Servo
+
+    private lateinit var driveSubsystem: DriveSubsystem
+    private lateinit var intakeSubsystem: IntakeSubsystem
+    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+    private lateinit var armSubsystem: ArmSubsystem
+    private lateinit var elevatorSubsystem: ElevatorSubsystem
+
+    override fun init() {
+        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+
+        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
+        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+
+        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
+
+        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+
+        driveSubsystem = DriveSubsystem(hardwareMap)
+        intakeSubsystem = IntakeSubsystem(intake)
+        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
+        armSubsystem = ArmSubsystem(armRight, armLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
+
+        val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
+            .addTemporalMarker(2.0) {
+                intakeSubsystem.intake()
+                intakeBeltSubsystem.intakePos()
+            }
+            .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0))
+            .back(4.0)
+            .addTemporalMarker(1.5) {
+                armSubsystem.setpoint = Math.toRadians(98.0)
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(4.0) {
+                elevatorSubsystem.setpoint = 30.0
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(4.5) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(6.0) {
+                intakeSubsystem.outtake()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(6.5) {
+                intakeBeltSubsystem.intakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .turn(Math.toRadians(32.0))
+            .waitSeconds(1.0)
+            .forward(3.0)
+            .addTemporalMarker(7.5) {
+                armSubsystem.setpoint = Math.toRadians(0.0)
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(8.5) {
+                elevatorSubsystem.setpoint = 30.0
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(10.0) {
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(11.5) {
+                intakeSubsystem.intake()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(12.0) {
+                intakeBeltSubsystem.outtakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .turn(Math.toRadians(-30.0))
+            .waitSeconds(1.0)
+            .back(2.0)
+            .addTemporalMarker(13.0) {
+                armSubsystem.setpoint = Math.toRadians(98.0)
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(15.0) {
+                elevatorSubsystem.setpoint = 33.0
+            }
+            .waitSeconds(2.0)
+            .addTemporalMarker(17.0) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .turn(Math.toRadians(52.0))
+            .waitSeconds(1.5)
+            .addTemporalMarker(18.5) {
+                intakeSubsystem.outtake()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(19.0) {
+                intakeBeltSubsystem.intakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(20.5) {
+                intakeBeltSubsystem.outtakePos()
+                armSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(1.0)
+            .forward(2.0)
+            .addTemporalMarker(21.5) {
+                elevatorSubsystem.setpoint = 30.0
+                intakeBeltSubsystem.intakePos()
+            }
+            .turn(Math.toRadians(-60.0))
+            .waitSeconds(2.0)
+            .addTemporalMarker(23.5) {
+                intakeSubsystem.intake()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(24.0) {
+                intakeBeltSubsystem.outtakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(0.5)
+            .back(2.0)
+            .addTemporalMarker(24.5) {
+                armSubsystem.setpoint = Math.toRadians(98.0)
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(27.0) {
+                elevatorSubsystem.setpoint = 33.0
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(28.5) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(29.0) {
+                intakeSubsystem.outtake()
+            }
+            // .strafetolinearheading
+
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(6.0) {
+//                intakeBeltSubsystem.outtakePos()
+//                armSubsystem.setpoint = 0.0
+//            }
+//            .turn(Math.toRadians(40.0))
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(6.5) {
+//                elevatorSubsystem.setpoint = 30.0
+//            }
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(7.5) {
+//                intakeBeltSubsystem.intakePos()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(8.0) {
+//                intakeSubsystem.intake()
+//            }
+//            .waitSeconds(0.5)
+//            .addTemporalMarker(8.5) {
+//                elevatorSubsystem.setpoint = 0.0
+//                intakeBeltSubsystem.outtakePos()
+//            }
+//            .turn(Math.toRadians(-40.0))
+//            .waitSeconds(1.0)
+//            .addTemporalMarker(9.5) {
+//                armSubsystem.setpoint = Math.toRadians(98.0)
+//            }
+            .build()
+
+        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
+        driveSubsystem.followTrajectorySequenceAsync(trajectory)
+    }
+
+    override fun loop() {
+        driveSubsystem.update()
+
+        elevatorSubsystem.periodic()
+        armSubsystem.periodic()
+
+        telemetry.addData("Arm Position", armSubsystem.armAngle)
+        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
+        telemetry.update()
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt
new file mode 100644
index 0000000..67197c4
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt
@@ -0,0 +1,201 @@
+package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.constants.AutoStartPose
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequence
+
+@Autonomous(name = "V2 Blue 1 + 2", group = "Basket", preselectTeleOp = "MainTeleOp")
+class Blue1Plus2V2: OpMode() {
+    private lateinit var armLeft: Motor
+    private lateinit var armRight: Motor
+    private lateinit var elevatorLeft: Motor
+    private lateinit var elevatorRight: Motor
+
+    private lateinit var intake: Servo
+    private lateinit var intakeBelt: Servo
+
+    private lateinit var driveSubsystem: DriveSubsystem
+    private lateinit var intakeSubsystem: IntakeSubsystem
+    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+    private lateinit var armSubsystem: ArmSubsystem
+    private lateinit var elevatorSubsystem: ElevatorSubsystem
+
+    private lateinit var toBasket1: TrajectorySequence
+    private lateinit var toBasket2: TrajectorySequence
+
+    override fun init() {
+        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+
+        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
+        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+
+        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
+
+        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+
+        driveSubsystem = DriveSubsystem(hardwareMap)
+        intakeSubsystem = IntakeSubsystem(intake)
+        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
+        armSubsystem = ArmSubsystem(armRight, armLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
+
+        toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
+            .splineToLinearHeading(Pose2d(54.0, 56.0, Math.toRadians(225.0)), Math.toRadians(0.0))
+            .addTemporalMarker(0.1) {
+                intakeSubsystem.intake()
+                armSubsystem.setpoint = Math.toRadians(98.0)
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(1.0) {
+                elevatorSubsystem.setpoint = 34.0
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(1.5) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(2.0) {
+                intakeSubsystem.outtake()
+            }
+            .waitSeconds(0.1)
+            .addTemporalMarker(2.1) {
+                intakeBeltSubsystem.outtakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .turn(Math.toRadians(37.0))
+            .waitSeconds(2.0)
+            .addTemporalMarker(4.1) {
+                armSubsystem.setpoint = Math.toRadians(0.0)
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(4.6) {
+                elevatorSubsystem.setpoint = 19.0
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(1.25)
+            .addTemporalMarker(6.0) {
+                intakeSubsystem.intake()
+            }
+            .waitSeconds(0.25)
+            .turn(Math.toRadians(-37.0))
+            .addTemporalMarker(6.25) {
+                intakeBeltSubsystem.outtakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(7.05) {
+                armSubsystem.setpoint = Math.toRadians(98.0)
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(0.75)
+            .addTemporalMarker(7.8) {
+                elevatorSubsystem.setpoint = 31.0
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(8.8) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(1.25)
+            .turn(Math.toRadians(55.0))
+            .addTemporalMarker(10.05) {
+                intakeSubsystem.outtake()
+            }
+            .waitSeconds(0.25)
+            .addTemporalMarker(10.3) {
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(10.8) {
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(12.3) {
+                armSubsystem.setpoint = 0.0
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(13.3) {
+                elevatorSubsystem.setpoint = 19.0
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(14.3) {
+                intakeBeltSubsystem.intakePos()
+            }
+            .turn(Math.toRadians(-55.0))
+            .waitSeconds(1.2)
+            .back(2.0)
+            .addTemporalMarker(15.5) {
+                intakeSubsystem.intake()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(16.0) {
+                intakeBeltSubsystem.outtakePos()
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(17.0) {
+                armSubsystem.setpoint = Math.toRadians(98.0)
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(18.0) {
+                elevatorSubsystem.setpoint = 29.0
+            }
+            .waitSeconds(1.5)
+            .addTemporalMarker(19.5) {
+                intakeBeltSubsystem.outtakePos()
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(20.5) {
+                intakeSubsystem.outtake()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(21.0) {
+                intakeBeltSubsystem.intakePos()
+            }
+            .waitSeconds(0.5)
+            .addTemporalMarker(21.5) {
+                elevatorSubsystem.setpoint = 0.0
+            }
+            .waitSeconds(1.0)
+            .addTemporalMarker(22.5) {
+                armSubsystem.setpoint = 0.0
+            }
+            .splineToSplineHeading(Pose2d(28.0, 10.0, Math.toRadians(0.0)), 90.0)
+            .back(5.0)
+            .addDisplacementMarker {
+                driveSubsystem.followTrajectorySequenceAsync(toBasket2)
+            }
+            .build()
+        driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
+        driveSubsystem.followTrajectorySequenceAsync(toBasket1)
+    }
+
+
+    override fun loop() {
+        driveSubsystem.update()
+
+        elevatorSubsystem.periodic()
+        armSubsystem.periodic()
+
+        telemetry.addData("Arm Position", armSubsystem.armAngle)
+        telemetry.addData("Slides Position", elevatorSubsystem.slidePos)
+        telemetry.update()
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
new file mode 100644
index 0000000..016ee4a
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.config.Config
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+import com.arcrobotics.ftclib.command.CommandOpMode
+import com.arcrobotics.ftclib.command.RunCommand
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
+
+@TeleOp
+@Config
+class IntakeTuner : CommandOpMode() {
+    private lateinit var intake: Servo
+    private lateinit var intakeBelt: Servo
+
+    private lateinit var intakeSubsystem: IntakeSubsystem
+    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+
+    override fun initialize() {
+        telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+
+        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
+        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+
+        intakeSubsystem = IntakeSubsystem(intake)
+        intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
+
+        RunCommand({
+            intakeSubsystem.setSpeed(claw)
+        }).perpetually().schedule()
+
+        RunCommand({
+            intakeBeltSubsystem.setPos(belt)
+        }).perpetually().schedule()
+
+    }
+
+    companion object {
+        @JvmField
+        var claw = 0.75
+
+        @JvmField
+        var belt = 0.0
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt
new file mode 100644
index 0000000..c72bf98
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt
@@ -0,0 +1,67 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.slides
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import com.qualcomm.robotcore.util.ElapsedTime
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+
+@Autonomous
+class SlidesConstraintsTuner : LinearOpMode() {
+    private lateinit var leftMotor: Motor
+    private lateinit var rightMotor: Motor
+
+    private lateinit var elevator: ElevatorSubsystem
+
+    private var maxVel = 0.0
+    private var maxAccel = 0.0
+
+    private val timer = ElapsedTime(ElapsedTime.Resolution.SECONDS)
+    override fun runOpMode() {
+        telemetry = MultipleTelemetry(FtcDashboard.getInstance().telemetry, telemetry)
+
+        leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+        rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+
+        elevator = ElevatorSubsystem(rightMotor, leftMotor) { 0.0 }
+        waitForStart()
+
+        var dt = 0L
+        var dv = 0.0
+
+        timer.reset()
+        while (opModeIsActive()) {
+            while (timer.seconds() < TIME_TO_RUN) {
+                elevator.spinUp()
+
+                dt = timer.nanoseconds() - dt
+                dv = elevator.slideVelocity - dv / dt
+
+                maxVel = elevator.slideVelocity.coerceAtLeast(maxVel)
+                maxAccel = dv.coerceAtLeast(maxAccel)
+
+                telemetry.addData("Velocity", elevator.slideVelocity)
+                telemetry.addData("Position", elevator.slidePos)
+
+                telemetry.addData("Max Velocity", maxVel)
+                telemetry.addData("Max Acceleration", maxAccel)
+                telemetry.update()
+
+
+                if (isStopRequested) break
+            }
+
+            telemetry.addData("Final Max Velocity", maxVel)
+            telemetry.addData("Final Max Acceleration", maxAccel)
+            telemetry.update()
+        }
+    }
+
+    companion object {
+        @JvmField
+        var TIME_TO_RUN = 1.5
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
index dcf5eea..9c96cd0 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
@@ -5,34 +5,61 @@ import com.acmerobotics.dashboard.config.Config
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
 import com.arcrobotics.ftclib.command.CommandOpMode
 import com.arcrobotics.ftclib.command.RunCommand
+import com.arcrobotics.ftclib.gamepad.GamepadEx
+import com.arcrobotics.ftclib.gamepad.GamepadKeys
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem
+import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @TeleOp
 @Config
 class SlidesPIDTuner : CommandOpMode() {
+    private lateinit var slidesLeft: Motor
+    private lateinit var slidesRight: Motor
+
     private lateinit var armLeft: Motor
     private lateinit var armRight: Motor
 
-    private lateinit var slidesSubsystem: SlidesSubsytem
+    private lateinit var armSubsystem: OpenArmSubsystem
+    private lateinit var slidesSubsystem: ElevatorSubsystem
+
+    private lateinit var armUpCommand: OpenArmCommand
+    private lateinit var armDownCommand: OpenArmCommand
+
+    private lateinit var operator: GamepadEx
 
     override fun initialize() {
+        operator = GamepadEx(gamepad2)
+
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
-        armLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150)
-        armRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150)
+        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60)
+        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60)
+
+        slidesLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150)
+        slidesRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150)
+
+        armSubsystem = OpenArmSubsystem(armRight, armLeft) { 0.0 }
+        slidesSubsystem = ElevatorSubsystem(slidesRight, slidesLeft, armSubsystem::armAngle)
+
+        armUpCommand = OpenArmCommand(armSubsystem, true)
+        armDownCommand = OpenArmCommand(armSubsystem, false)
 
-        slidesSubsystem = SlidesSubsytem(armRight, armLeft)
+        operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand)
+        operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand)
 
         RunCommand({
             slidesSubsystem.setpoint = target
         }).perpetually().schedule()
 
         RunCommand({
-            telemetry.addData("Slides Position: ", slidesSubsystem.slidePos)
-            telemetry.addData("Setpoint: ", target)
+            telemetry.addData("Slides Position", slidesSubsystem.slidePos)
+            telemetry.addData("Setpoint", target)
+            telemetry.addData("arm angle", armSubsystem.armAngle)
+            telemetry.addData("slides Velocity", slidesSubsystem.slideVelocity)
             telemetry.update()
         }).perpetually().schedule()
 
@@ -41,7 +68,6 @@ class SlidesPIDTuner : CommandOpMode() {
 
     companion object {
         @JvmField
-        // NOTE THAT THIS VALUE SHOULD BE DEGREES
         var target = 0.0
     }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
index 2973060..1eea847 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
@@ -13,7 +13,7 @@ import kotlin.math.PI
 @Config
 class ArmSubsystem(
     armRight: Motor,
-    armLeft: Motor
+    armLeft: Motor,
 ) : PIDSubsystem(
     PIDController(
         ArmConstants.kP.value,
@@ -29,19 +29,36 @@ class ArmSubsystem(
     val armVelocity: Double
         get() = turnMotors.velocities[0] / GoBILDA.RPM_60.cpr * PI
 
-    private val feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0);
+    private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0);
 
     init {
-        armRight.inverted = true
-        armRight.encoder.setDirection(Motor.Direction.REVERSE)
+        armLeft.inverted = true
 
         turnMotors.resetEncoder()
         turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
     }
 
     override fun useOutput(output: Double, setpoint: Double) {
+//        controller.setPIDF(kP, kI, kD, 0.0)
+//        feedforward = ArmFeedforward(0.0, kCos, 0.0)
+
         turnMotors.set(output + feedforward.calculate(armAngle, armVelocity))
     }
 
     override fun getMeasurement() = armAngle
+
+    companion object {
+        @JvmField
+        var kP = 2.0
+
+        @JvmField
+        var kI = 0.0
+
+        @JvmField
+        var kD = 0.0
+
+        @JvmField
+        var kCos = 0.1
+
+    }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
index 3bb696f..2102f85 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
@@ -8,6 +8,7 @@ import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.teamcode.constants.ArmConstants
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.opModes.teleOp.MainTeleOp.Companion.kT
 import kotlin.math.PI
 import kotlin.math.cos
 
@@ -16,7 +17,7 @@ object OpenArmSubsystem : SubsystemBase() {
     private lateinit var turnMotors: MotorGroup
 
     val armAngle: Double
-        get() = turnMotors.positions[0] / GoBILDA.RPM_312.cpr * 2 * PI
+        get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI
 
     fun initialize(hardwareMap: HardwareMap) {
         val armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
@@ -41,7 +42,6 @@ object OpenArmSubsystem : SubsystemBase() {
     }
 
     fun stop() {
-        turnMotors.set(ArmConstants.kCos.value * cos(armAngle))
+        turnMotors.set(ArmConstants.kCos.value * cos(armAngle) + kT * 0.0)
     }
-
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt
new file mode 100644
index 0000000..7964e8c
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt
@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.subsystems.intake
+
+import com.arcrobotics.ftclib.command.SubsystemBase
+import com.qualcomm.robotcore.hardware.Servo
+
+class IntakeBeltSubsystem(
+    private val intakeBelt: Servo
+) : SubsystemBase() {
+    var beltPos = false
+
+    fun outtakePos() {
+        intakeBelt.position = 0.75
+        beltPos = !beltPos
+    }
+
+    fun intakePos() {
+        intakeBelt.position = 0.47
+        beltPos = !beltPos
+    }
+
+    fun increasePos() {
+        intakeBelt.position += 0.005
+    }
+
+    fun decreasePos() {
+        intakeBelt.position -= 0.005
+    }
+
+    fun setPos(pos: Double) {
+        intakeBelt.position = pos
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt
index 729d10c..5d387cd 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt
@@ -1,20 +1,28 @@
 package org.firstinspires.ftc.teamcode.subsystems.intake
 
 import com.arcrobotics.ftclib.command.SubsystemBase
-import com.arcrobotics.ftclib.hardware.motors.CRServo
 import com.qualcomm.robotcore.hardware.HardwareMap
+import com.qualcomm.robotcore.hardware.Servo
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
 
 object IntakeSubsystem : SubsystemBase() {
-    private lateinit var intake: CRServo
+    private lateinit var intake: Servo
 
     fun initialize(hardwareMap: HardwareMap) {
-        intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
+        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
     }
 
-    fun intake() = intake.set(1.0)
+    var intakePos = false
 
-    fun outtake() = intake.set(-0.5)
+    fun intake() {
+        intake.position = 1.0
+        intakePos = !intakePos
+    }
+
+    fun outtake() {
+        intake.position = 0.6
+        intakePos = !intakePos
+    }
 
-    fun stop() = intake.stop()
+    fun setSpeed(speed: Double) { intake.position = speed }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt
new file mode 100644
index 0000000..45f3c20
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt
@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.subsystems.slides
+
+import com.acmerobotics.dashboard.config.Config
+import com.arcrobotics.ftclib.command.SubsystemBase
+import com.arcrobotics.ftclib.controller.wpilibcontroller.ProfiledPIDController
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import com.arcrobotics.ftclib.trajectory.TrapezoidProfile
+import com.arcrobotics.ftclib.util.MathUtils.clamp
+import org.firstinspires.ftc.teamcode.constants.SlidesConstants
+import kotlin.math.sin
+
+@Config
+class ElevatorSubsystem(
+    elevatorRight : Motor,
+    elevatorLeft : Motor,
+    private val slideAngle: () -> Double
+) : SubsystemBase() {
+    private val extendMotors = MotorGroup(elevatorRight, elevatorLeft)
+    private val controller = ProfiledPIDController(
+        SlidesConstants.kP.value,
+        SlidesConstants.kI.value,
+        SlidesConstants.kD.value,
+        TrapezoidProfile.Constraints(
+            30.0, // in/s
+            30.0 // in/s^2
+        )
+    )
+
+    var setpoint = 0.0
+        set(value) {
+            val clamped = if (slideAngle.invoke() < Math.toRadians(75.0))
+                clamp(value, 0.0, 20.0)
+            else
+                clamp(value, 0.0, 30.0)
+
+            controller.goal = TrapezoidProfile.State(clamped, 0.0)
+            field = clamped
+        }
+
+    val slidePos: Double
+        get() = extendMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
+
+    val slideVelocity: Double
+        get() = -extendMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
+
+    var enabled = true
+
+    init {
+        elevatorRight.inverted = true
+        elevatorRight.encoder.setDirection(Motor.Direction.REVERSE)
+
+        extendMotors.resetEncoder()
+        extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
+
+        setpoint = 0.0
+        enabled = true
+    }
+
+    override fun periodic() {
+        controller.setPID(kP, kI, kD)
+
+        val output = controller.calculate(slidePos)
+
+        if (enabled)
+            extendMotors.set(output + kG * sin(slideAngle.invoke()))
+    }
+
+    fun toggle() {
+        enabled != enabled
+    }
+
+    fun disable() {
+        enabled = false
+    }
+
+    fun spinUp() {
+        extendMotors.set(1.0)
+    }
+
+    fun spinDown() {
+        extendMotors.set(-1.0)
+    }
+
+    fun stop() {
+        extendMotors.set(kG)
+    }
+
+    companion object {
+        @JvmField
+        var kP = 0.7
+        // kP = 0.01
+
+        @JvmField
+        var kI = 0.0
+
+        @JvmField
+        var kD = 0.0
+
+        @JvmField
+        var kG = 0.12
+        // kG = 0.115
+
+    }
+}
+
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt
new file mode 100644
index 0000000..fd8441e
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt
@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode.subsystems.slides
+
+import com.arcrobotics.ftclib.command.SubsystemBase
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import org.firstinspires.ftc.teamcode.constants.SlidesConstants
+import kotlin.math.sin
+
+class OpenElevatorSubsystem(
+    //sets them as a private variable thats a motor (same name as in driver hub)
+    elevatorRight : Motor,
+    elevatorLeft : Motor,
+    private val slideAngle: () -> Double
+) : SubsystemBase() {
+    //makes a motor group bc you have to move them at the same time
+    private val elevatorMotors = MotorGroup(elevatorRight, elevatorLeft)
+    val slidePos: Double
+        get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
+
+    init {
+        elevatorRight.inverted = true
+        elevatorRight.encoder.setDirection(Motor.Direction.REVERSE)
+        elevatorMotors.resetEncoder()
+    }
+
+    // Functions for moving slides up and down, number being speed, 1 being 100%
+    fun up() {
+        elevatorMotors.set(1.0)
+    }
+
+    fun down() {
+        elevatorMotors.set(-1.0)
+    }
+
+    fun stop() {
+        elevatorMotors.set(SlidesConstants.kG.value * sin(slideAngle.invoke()))
+    }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt
index 9bfdbf0..893cd24 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt
@@ -5,10 +5,19 @@ import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.constants.SlidesConstants
+import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
+import kotlin.math.sin
 
 object OpenSlidesSubsystem: SubsystemBase() {
     private lateinit var elevatorMotors: MotorGroup
 
+    val slidePos: Double
+        get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
+
+    val slideVelocity: Double
+        get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
+
     fun initialize(hardwareMap: HardwareMap) {
         val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
         val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
@@ -27,7 +36,7 @@ object OpenSlidesSubsystem: SubsystemBase() {
     }
 
     fun stop() {
-        elevatorMotors.set(0.0)
+        elevatorMotors.set(SlidesConstants.kG.value * sin(OpenArmSubsystem.armAngle))
     }
 
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt
deleted file mode 100644
index f1e2f6a..0000000
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt
+++ /dev/null
@@ -1,41 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems.slides
-
-import com.acmerobotics.dashboard.config.Config
-import com.arcrobotics.ftclib.controller.PIDController
-import com.arcrobotics.ftclib.hardware.motors.Motor
-import com.arcrobotics.ftclib.hardware.motors.MotorGroup
-import org.firstinspires.ftc.teamcode.constants.SlidesConstants
-import org.firstinspires.ftc.teamcode.utils.PIDSubsystem
-
-@Config
-class SlidesSubsytem(
-    elevatorRight : Motor,
-    elevatorLeft : Motor
-) : PIDSubsystem(
-    PIDController(
-        SlidesConstants.kP.value,
-        SlidesConstants.kI.value,
-        SlidesConstants.kD.value,
-    )
-) {
-    private val extendMotors = MotorGroup(elevatorRight, elevatorLeft)
-
-    val slidePos: Double
-        get() = extendMotors.positions[0]
-
-    val slideVelocity: Double
-        get() = extendMotors.velocities[0]
-
-    init {
-        elevatorRight.inverted = true
-
-        extendMotors.resetEncoder()
-        extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
-    }
-
-    override fun useOutput(output: Double, setpoint: Double) {
-        extendMotors.set(output)
-    }
-
-    override fun getMeasurement() = slidePos
-}
\ No newline at end of file