diff --git a/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt b/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt
index 2ed6eec..8537e8e 100644
--- a/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt
+++ b/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt
@@ -35,6 +35,20 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
 //    }
 //}
 
+/**
+ * A command that executes a sequence of commands based on a loop counter.
+ *
+ * @param max The maximum value for the loop counter (exclusive)
+ * @param callback A function that takes the loop counter as input and returns a Command to execute
+ *
+ * Example usage:
+ * ```
+ * ForCommand(5) { i ->
+ *   Commands.runOnce { println("Loop iteration $i") }
+ * }
+ * ```
+ * This would print the loop counter 5 times in sequence upon scheduling the ForCommand.
+ */
 class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : SequentialCommandGroup(
     *(0..<max).map { callback(it) }.toTypedArray()
 )
\ No newline at end of file
diff --git a/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt b/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt
index 5c57146..1f1a360 100644
--- a/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt
+++ b/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt
@@ -13,7 +13,7 @@ class SequentialRequest(vararg requests: Request) : Request() {
     private var startedCurrentRequest = false
 
     private fun startNextRequest() {
-        if (idleRequests.isNotEmpty() && currentRequest.isFinished()) {
+        if (idleRequests.isNotEmpty() && currentRequest.isFinished() && startedCurrentRequest) {
             currentRequest = idleRequests.removeAt(0)
             startedCurrentRequest = false
             startIfAllowed()
diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt
index c03aeaa..5a53ecb 100644
--- a/src/main/java/frc/robot/Robot.kt
+++ b/src/main/java/frc/robot/Robot.kt
@@ -55,6 +55,8 @@ object Robot : TimedRobot() {
 
         SmartDashboard.putBoolean("Start by unclimbing?", false)
 
+        SmartDashboard.putBoolean("Start with a disabled climb", false)
+
         CanBridge.runTCP()
     }
 
@@ -75,6 +77,8 @@ object Robot : TimedRobot() {
         SmartDashboard.putBoolean("Teleop pose difference?", true)
 
 
+
+
         RobotContainer
         OperatorControls
     }
@@ -117,6 +121,8 @@ object Robot : TimedRobot() {
         RobotContainer.startByUnclimbing = SmartDashboard.getBoolean("Start by unclimbing?", false)
 //        println(RobotContainer.startByUnclimbing)
 
+        RobotContainer.startWithADisabledClimb = SmartDashboard.getBoolean("Start with a disabled climb", false)
+
 
         RobotContainer.vision.updateOdometryFromDisabled()
     }
@@ -165,6 +171,12 @@ object Robot : TimedRobot() {
             println("unclilmbing")
             Superstructure.unclimb()
         }
+
+        if (RobotContainer.startWithADisabledClimb) {
+            println("starting with climb disabled")
+            Superstructure.disableClimb()
+        }
+
 //        } else if (!Superstructure.climbSystem.isStow() || !Superstructure.funnelSystem.isStow()) {
 //            Superstructure.climbStowThenStow()
 //        } else {
diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt
index 8dead42..8e1d73f 100644
--- a/src/main/java/frc/robot/RobotContainer.kt
+++ b/src/main/java/frc/robot/RobotContainer.kt
@@ -24,6 +24,8 @@ object RobotContainer {
 
     var startByUnclimbing = false
 
+    var startWithADisabledClimb = false
+
 
     val xbox = CommandXboxController(2)
 
diff --git a/src/main/java/frc/robot/commands/GoToPose.kt b/src/main/java/frc/robot/commands/GoToPose.kt
index dada3b9..8c0c506 100644
--- a/src/main/java/frc/robot/commands/GoToPose.kt
+++ b/src/main/java/frc/robot/commands/GoToPose.kt
@@ -99,7 +99,7 @@ open class GoToPose(
     override fun isFinished(): Boolean = endCondition()
 
     override fun end(interrupted: Boolean) {
-        println("drive command and then ending")
+//        println("drive command and then ending")
         RobotState.autoDriving = false
         RobotContainer.currentDriveCommand = Optional.empty()
     }
diff --git a/src/main/java/frc/robot/commands/TeleopAutoAlign.kt b/src/main/java/frc/robot/commands/TeleopAutoAlign.kt
index 42ca6c5..f57c759 100644
--- a/src/main/java/frc/robot/commands/TeleopAutoAlign.kt
+++ b/src/main/java/frc/robot/commands/TeleopAutoAlign.kt
@@ -1,6 +1,7 @@
 package frc.robot.commands
 
 import cshcyberhawks.lib.math.MiscCalculations
+import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj2.command.Command
 import frc.robot.RobotContainer
 import frc.robot.constants.AutoScoringConstants
@@ -36,14 +37,18 @@ object TeleopAutoAlign {
             if (OperatorControls.noWalk) {
                 AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
             } else {
-                val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
+                val goalPose =
+                    AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
                 // Make the position get closer as it gets closer to the final goal
 //            val adjustX: Double = 0.3
-                var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75)
+                var adjustX: Double = min(
+                    1.0,
+                    RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5
+                )
 //            val adjustX = 1.0
 
                 // Once we're close enough we can just jump to the goal
-                if (adjustX < 0.25) {
+                if (adjustX < 0.3) {
                     adjustX = 0.0
                 }
 
@@ -62,11 +67,14 @@ object TeleopAutoAlign {
                 val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0))
                 // Make the position get closer as it gets closer to the final goal
 //            val adjustX: Double = 0.3
-                var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75)
+                var adjustX: Double = min(
+                    1.0,
+                    RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * .5
+                )
 //            val adjustX = 1.0
 
                 // Once we're close enough we can just jump to the goal
-                if (adjustX < 0.25) {
+                if (adjustX < 0.3) {
                     adjustX = 0.0
                 }
 
@@ -75,18 +83,48 @@ object TeleopAutoAlign {
         })
     }
 
-    fun processorAlign(): Command = GoToPose({
-        AllianceFlipUtil.apply(FieldConstants.processorPosition)
-    }, { false })
+        fun l1ReefAlign(): Command {
+            return GoToPose({
+                val position = OperatorControls.reefPosition
 
-    fun feederAlign(): Command = GoToPose({
-        val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition)
-        val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition)
+                if (OperatorControls.noWalk) {
+                    AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0))
+                } else {
+                    val goalPose =
+                        AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0))
+                    // Make the position get closer as it gets closer to the final goal
+//            val adjustX: Double = 0.3
+                    var adjustX: Double = min(
+                        1.0,
+                        RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5
+                    )
+//            val adjustX = 1.0
+
+                    // Once we're close enough we can just jump to the goal
+                    if (adjustX < 0.3) {
+                        adjustX = 0.0
+                    }
 
-        if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(rightPos.translation))  {
-            leftPos
-        } else {
-            rightPos
+                    AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, adjustX))
+                }
+            })
         }
-    }, { false })
-}
\ No newline at end of file
+
+        fun processorAlign(): Command = GoToPose({
+            AllianceFlipUtil.apply(FieldConstants.processorPosition)
+        }, { false })
+
+        fun feederAlign(): Command = GoToPose({
+            val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition)
+            val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition)
+
+            if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(
+                    rightPos.translation
+                )
+            ) {
+                leftPos
+            } else {
+                rightPos
+            }
+        }, { false })
+    }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/constants/AutoScoringConstants.kt b/src/main/java/frc/robot/constants/AutoScoringConstants.kt
index 8a37837..2c14d57 100644
--- a/src/main/java/frc/robot/constants/AutoScoringConstants.kt
+++ b/src/main/java/frc/robot/constants/AutoScoringConstants.kt
@@ -62,6 +62,27 @@ object AutoScoringConstants {
         )
     }
 
+    fun getL1PoseAtOffset(face: Int, x: Double): Pose2d {
+        val poseDirection =
+            Pose2d(FieldConstants.Reef.center, Rotation2d.fromDegrees((180 - (60 * face)).toDouble()))
+        val adjustX: Double = reefFaceOffset + x - 0.425// + Units.inchesToMeters(3.0)
+        val adjustY: Double = 1.075
+
+        return Pose2d(
+            Translation2d(
+                poseDirection
+                    .transformBy(Transform2d(adjustX, adjustY, Rotation2d()))
+                    .x,
+                poseDirection
+                    .transformBy(Transform2d(adjustX, adjustY, Rotation2d()))
+                    .y
+            ),
+            Rotation2d(
+                poseDirection.rotation.radians
+            ).rotateBy(Rotation2d.fromDegrees(76.0))
+        )
+    }
+
     enum class ReefPositions(var left: Pose2d, var right: Pose2d, var center: Pose2d) {
         A(Pose2d(), Pose2d(), Pose2d()),
         B(Pose2d(), Pose2d(), Pose2d()),
diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt
index ea7cf5c..2035816 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt
@@ -27,6 +27,7 @@ import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOEmpty
 import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOPID
 import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOSim
 import frc.robot.util.AllianceFlipUtil
+import frc.robot.util.input.OperatorControls
 import java.util.*
 
 object Superstructure : SubsystemBase() {
@@ -178,7 +179,13 @@ object Superstructure : SubsystemBase() {
                 ),
                 EmptyRequest(),
                 ParallelRequest(
-                    stowRequest(),
+                    IfRequest(
+                        {RobotState.gamePieceState == GamePieceState.Coral && OperatorControls.highStowPosition},
+                        ParallelRequest(
+                            elevatorSystem.stowPosition(), pivotSystem.highStowAngle()
+                        ),
+                        stowRequest()
+                    ),
                     IfRequest(
                         { RobotState.gamePieceState == GamePieceState.Coral },
                         intakeSystem.coralHolding(),
@@ -201,6 +208,14 @@ object Superstructure : SubsystemBase() {
         )
     )
 
+    fun scoreL1() = requestSuperstructureAction(
+        SuperstructureAction.create(
+            ParallelRequest(pivotSystem.l1Angle(), elevatorSystem.l1Position()),
+            intakeSystem.coralScore(),
+            stowRequest()
+        )
+    )
+
     fun scoreL2() =
         requestSuperstructureAction(
             SuperstructureAction.create(
@@ -214,7 +229,8 @@ object Superstructure : SubsystemBase() {
     fun scoreL3() =
         requestSuperstructureAction(
             SuperstructureAction.create(
-                ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position(), intakeSystem.coralHalfSpit()),
+                ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position(), intakeSystem.coralHalfSpit().withPrerequisite(
+                    elevatorSystem.closeL3())),
                 intakeSystem.coralScore(),
                 stowRequest(),
                 safeRetract = true
@@ -225,11 +241,11 @@ object Superstructure : SubsystemBase() {
         ParallelRequest(
             pivotSystem.l4Angle(),
             elevatorSystem.safeUpPosition(),
-            intakeSystem.coralHalfSpit()
         ),
         elevatorSystem
             .l4Position()
             .withPrerequisite(pivotSystem.safeTravelUp()),
+        //intakeSystem.coralHalfSpit().withPrerequisite(elevatorSystem.closeL4())
     )
 
     fun scoreL4() =
@@ -331,11 +347,13 @@ object Superstructure : SubsystemBase() {
     fun climb() = requestSuperstructureAction(
         SuperstructureAction.create(
                 ParallelRequest(
-                pivotSystem.oldClimbAngle(),
-                climbSystem.deploy(),
-                funnelSystem.deploy()
+                    climbSystem.unspoolForTime(),
+//                    pivotSystem.climbAngle(),
+                    //TODO: a timed climb system unspool
+//                    climbSystem.deploy(),
+                    funnelSystem.deploy()
                 ),
-            climbSystem.climb(),
+            SequentialRequest(pivotSystem.oldClimbAngleWithPID(), WaitRequest(1.5), climbSystem.climb()),
             EmptyRequest()//IfRequest({RobotState.actionCancelled}, SequentialRequest(ParallelRequest(funnelSystem.stow(), climbSystem.stow())), pivotSystem.stowAngle())
         )
     )
@@ -352,6 +370,10 @@ object Superstructure : SubsystemBase() {
         )
     )
 
+    fun disableClimb() = requestSuperstructureAction(
+        Superstructure.climbSystem.setDisable(RobotContainer.startWithADisabledClimb)
+    )
+
     fun climbStowThenStow() = requestSuperstructureAction(
         SequentialRequest(
             ParallelRequest(climbSystem.stow(), pivotSystem.climbStowAngle()),
diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt
index fa6e8f1..772bef5 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt
@@ -2,8 +2,8 @@ package frc.robot.subsystems.superstructure.climb
 
 object ClimbConstants {
     val stowAngle = 223.0
-    val deployAngle = 80.0
+    val deployAngle = 70.0
 //    val climbAngle = 145.0
-    var climbAngle = 125.0
+    var climbAngle = 141.0
 
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt
index 2244f64..8422584 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt
@@ -3,6 +3,7 @@ package frc.robot.subsystems.superstructure.climb
 interface ClimbIO {
     fun getAngle(): Double
     fun angleDegrees(angleDegrees: Double)
+    fun setMotor(current: Double)
     fun periodic()
 
     var climbing: Boolean
diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt
index a91a869..5e7c2cb 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt
@@ -3,19 +3,31 @@ package frc.robot.subsystems.superstructure.climb
 import cshcyberhawks.lib.math.MiscCalculations
 import cshcyberhawks.lib.requests.Request
 import cshcyberhawks.lib.requests.SequentialRequest
+import cshcyberhawks.lib.requests.WaitRequest
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 
 class ClimbSystem(private val io: ClimbIO): SubsystemBase() {
     private fun setAngle(angleDegrees: Double) = Request.withAction { io.angleDegrees(angleDegrees) }
     private fun setClimbing(climbing: Boolean) = Request.withAction { io.climbing = climbing }
-    private fun setDisable(disable: Boolean) = Request.withAction {io.disable = disable}
+    fun setDisable(disable: Boolean) = Request.withAction {
+        println("set disable to: " + disable)
+        io.disable = disable
+    }
+    fun setMotor(current: Double) = Request.withAction { io.setMotor(current) }
 
     fun deploy() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.deployAngle))
     fun climb() = SequentialRequest(setClimbing(true), setDisable(false), setAngle(ClimbConstants.climbAngle))
 
     fun stow() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.stowAngle))
 
+    fun unspoolForTime() = SequentialRequest(
+        setDisable(true),
+        setMotor(-40.0),
+        WaitRequest(8.0),
+        setMotor(0.0)
+    )
+
     fun isStow(): Boolean {
         return MiscCalculations.calculateDeadzone(io.getAngle() - ClimbConstants.stowAngle, 5.0) == 0.0
     }
diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt
index aa758e0..32ec52f 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt
@@ -10,6 +10,10 @@ class ClimbIOEmpty(): ClimbIO {
         return 0.0
     }
 
+    override fun setMotor(current: Double) {
+
+    }
+
     override fun angleDegrees(angleDegrees: Double) {
     }
 
diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt
index 962ce30..6011af3 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt
@@ -26,12 +26,16 @@ class ClimbIOReal() : ClimbIO {
     override var climbing = false
 
     override var disable = false
+        set(value) {
+            motor.setControl(torqueRequest.withOutput(0.0))
+            field = value
+        }
 
     private var neutralCoast = false
 
 
     private val pidController = ProfiledPIDController(
-        0.2315,
+        1.2315,
         0.0,
         0.0315,
         TrapezoidProfile.Constraints(
@@ -88,22 +92,28 @@ class ClimbIOReal() : ClimbIO {
         pidController.goal = TrapezoidProfile.State(angleDegrees, 0.0)
     }
 
+    override fun setMotor(current: Double) {
+        motor.setControl(torqueRequest.withOutput(current))
+    }
+
     override fun periodic() {
+        if (disable) {
+//            println("climb io disabled")
+//            motor.setControl(torqueRequest.withOutput(0.0))
+            return
+        }
 
 
         val positionPIDOut = pidController.calculate(getAngle())
 
 //        val gravityFF = 3.15 * sin(Math.toRadians(getAngle()))
 
-        val motorOut = if (climbing && (ClimbConstants.climbAngle - getAngle()) > 10.0) {positionPIDOut + 30.0} else if (climbing) {positionPIDOut + 15.0} else positionPIDOut
+        val motorOut = if (climbing && (ClimbConstants.climbAngle - getAngle()) > 10.0) {positionPIDOut + 30.0} else if (climbing) {positionPIDOut + 8.5} else positionPIDOut
 
 
-        if (!disable) {
 //            println(motorOut)
             motor.setControl(torqueRequest.withOutput(motorOut))
 
-        }
-
 
         val sdCoast = SmartDashboard.getBoolean("Climb coast", false)
 //        println(sdCoast)
diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt
index d6291cd..85c4436 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt
@@ -4,14 +4,17 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile
 import edu.wpi.first.math.util.Units
 
 object ElevatorConstants {
-    const val velocityInches = 40.0 // Guessed values for now
-    const val accelationInches = 30.0
+    const val velocityInches = 70.0 // Guessed values for now
+    const val accelationInches = 75.0
 
     // Conversion so one (I guess its mechanism rotation) is 1 inch on the elevator
     const val conversionFactor =
-        20.0 * 1.751 * Math.PI / 30.0
+//        20.0 * 1.751 * Math.PI / 30.0
+    9.0 * 1.751 * Math.PI / 30.0
 
-    const val positionTolerance = 0.1
+
+    // Large tolerance, not used for high accuracy things
+    const val positionTolerance = 0.5
 
     // The torque to hold the elevator against gravity (Newton-Meters)
     val kGNM = Units.inchesToMeters(1.751 / 2.0) * (25.6 * 4.4482216153)
diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt
index 38e937d..d9ea491 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt
@@ -12,7 +12,7 @@ import frc.robot.subsystems.superstructure.elevator.ElevatorIO
 // By making a subsystem a Kotlin object, we ensure there is only ever one instance of it.
 // It also reduces the need to have reference variables for the subsystems to be passed around.
 class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() {
-    private val safeUpPosition = 9.0
+    private val safeUpPosition = 8.0
     private val safeDownPosition = 4.0
 
     fun getPosition(): Double = io.getPosition()
@@ -23,10 +23,13 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() {
     fun atDesiredPosition() = io.atDesiredPosition()
 
     fun awaitDesiredPosition() = AwaitRequest { atDesiredPosition() }
+    fun prereqAtDesiredPosition() = Prerequisite.withCondition { io.atDesiredPosition() }
 
     fun belowSafeUpPosition() = Prerequisite.withCondition { getPosition() < safeUpPosition }
     fun safeIntakePosition() = Prerequisite.withCondition { getPosition() < 7.0 }
     fun safeToFlipPivot() = Prerequisite.withCondition { getPosition() > 4.5 }
+    fun closeL3() = Prerequisite.withCondition { getPosition() > 3.5 }
+    fun closeL4() = Prerequisite.withCondition { getPosition() > 29.5 }
 
     private fun setPosition(positionInches: Double) = Request.withAction {
         io.setPosition(positionInches)
@@ -39,13 +42,14 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() {
         SmartDashboard.putNumber("Elevator Position", getPosition())
     }
 
-    fun feederPosition() = setPosition(6.5)
+    fun feederPosition() = setPosition(6.0)
 
     fun stowPosition() = setPosition(1.0)
 
     fun safeUpPosition() = setPosition(safeUpPosition)
     fun safeDownPosition() = setPosition(safeDownPosition)
 
+    fun l1Position() = setPosition(0.0)
     fun l2Position() = setPosition(0.0)
     fun l3Position() = setPosition(4.0)
     fun l4Position() = setPosition(29.95) // Should be 30 eventually but not safe right now
diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt
index 7558cca..5041d51 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt
@@ -19,9 +19,9 @@ class ElevatorIOPID() : ElevatorIO {
 
     val elevatorPIDController =
             ProfiledPIDController(
-                    11.5,
-                    0.0,
-                    0.6,
+                    6.5,
+                    0.003,
+                    0.616,
                     TrapezoidProfile.Constraints(
                             ElevatorConstants.velocityInches,
                             ElevatorConstants.accelationInches
@@ -113,7 +113,9 @@ class ElevatorIOPID() : ElevatorIO {
     override fun periodic() {
         SmartDashboard.putNumber("Elevator Desired Position", desiredPosition)
 
-        val gravityFF = 3.85
+//        val gravityFF = 3.85
+        val gravityFF = 7.15
+
         val positionPIDOut = elevatorPIDController.calculate(getPosition())
 
         SmartDashboard.putNumber("Elevator Position Error", elevatorPIDController.positionError)
diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt
index c613f2c..109b76b 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt
@@ -5,7 +5,7 @@ object FunnelConstants {
     const val accelerationDegrees = 250.0
 
 //    const val stowAngle = 215.0
-    const val stowAngle = 212.0
+    const val stowAngle = 218.0
 //    const val deployAngle = 50.5
     const val deployAngle = 65.5
 
diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt
index eae5ab1..e35af6f 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt
@@ -23,7 +23,7 @@ class FunnelIOReal(): FunnelIO {
     private val encoder = CANcoder(CANConstants.Funnel.encoderId)
 
     private val pidController = ProfiledPIDController(
-        0.145,
+        0.265,
         0.0,
         0.0315,
         TrapezoidProfile.Constraints(
diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt
index 7cc26ec..b1d6c59 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt
@@ -2,7 +2,7 @@ package frc.robot.subsystems.superstructure.intake
 
 object IntakeConstants {
     const val coralScoreTimeoutSeconds = 0.5
-    const val coralHalfSpitTimeSeconds = 0.2
+    const val coralHalfSpitTimeSeconds = 0.15
     const val algaeScoreTimeoutSeconds = 0.5
 
     const val intakeCurrentThreshold = 10.0 // 10 Amps before system knows it has a game piece
diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt
index fe7a87d..21547b2 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt
@@ -3,8 +3,8 @@ package frc.robot.subsystems.superstructure.pivot
 import edu.wpi.first.math.trajectory.TrapezoidProfile
 
 object PivotConstants {
-    const val velocityDegrees = 180.0 // Guessed values for now
-    const val accelerationDegrees = 250.0
+    const val velocityDegrees = 225.0 // Guessed values for now
+    const val accelerationDegrees = 375.0
 
     // Conversion so one (I guess its mechanism rotation) is 1 rotation of the pivot
     const val conversionFactor =
diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt
index e01c2a9..025f54f 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt
@@ -35,6 +35,8 @@ class PivotSystem(private val io: PivotIO) : SubsystemBase() {
             }
         }
 
+    fun prereqAtDesiredAngle() = Prerequisite.withCondition { atDesiredAngle() }
+
     fun safeTravelUp() = Prerequisite.withCondition { getAngle() < 245.0 }
     fun safeTravelDown() = Prerequisite.withCondition { getAngle() > 180.0 }
     fun safeToClimbDisable() = Prerequisite.withCondition { getAngle() < 20.0 }
@@ -60,6 +62,9 @@ class PivotSystem(private val io: PivotIO) : SubsystemBase() {
 
     fun climbStowAngle() = setAngle(325.0)
 
+    fun highStowAngle() = setAngle(130.0)
+
+    fun l1Angle() = setAngle(300.0)
     fun l2Angle() = setAngle(150.0)
 //    fun l3Angle() = setAngle(SmartDashboard.getNumber("L3 Angle", 125.0))
     fun l3Angle() = setAngle(120.0)
diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt
index 0fde92e..3ac1acc 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt
+++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt
@@ -40,9 +40,9 @@ class PivotIOPID() : PivotIO {
 //                    )
 //            )
         ProfiledPIDController(
-            1.0,
+            0.65,
             0.0,
-            0.06,
+            0.1,
             TrapezoidProfile.Constraints(
                 PivotConstants.velocityDegrees,
                 PivotConstants.accelerationDegrees
@@ -61,8 +61,7 @@ class PivotIOPID() : PivotIO {
     private var desiredAngle = 290.0
 
     //138.6 = 180
-
-    private val tbOffset = -43.2
+    private val tbOffset = -68
 //    private val tbOffset = -23.2//-43.2
 //    private fun getTBDegrees() =
 
@@ -145,6 +144,7 @@ class PivotIOPID() : PivotIO {
     override fun periodic() {
         SmartDashboard.putNumber("Pivot Raw TB Angle", MiscCalculations.wrapAroundAngles(encoder.absolutePosition.valueAsDouble * 360.0))
         SmartDashboard.putNumber("Pivot Desired Angle", desiredAngle)
+        SmartDashboard.putNumber("Pivot Velocity", encoder.velocity.valueAsDouble * 360.0)
 
         val sdCoast = SmartDashboard.getBoolean("Pivot coast", false)
         if (sdCoast != neutralCoast) {
@@ -154,14 +154,14 @@ class PivotIOPID() : PivotIO {
             )
         }
 
-        if (pidDistabled) {
+        if (pidDistabled || (desiredAngle == 295.0 && MiscCalculations.calculateDeadzone(desiredAngle, 4.0) == 0.0)) {
             motor.set(0.0)
             return
         }
 
 //        val kG = if (getAngle() > 270) 8.25 else 10.5
-        val kG = if (getAngle() > 270) 7.25 else 7.5
-        val gravityFF = kG * sin(Math.toRadians(getAngle() + 90))
+        val kG = if (getAngle() > 270 && getAngle() < 305.0) 0.0 else 7.5
+        val gravityFF = kG * -sin(Math.toRadians(getAngle() - 110.0))
         val positionPIDOut = pivotPIDController.calculate(getAngle())
 //        val correctivePIDOut = correctivePID.calculate(getAngle(), desiredAngle)
 
diff --git a/src/main/java/frc/robot/util/Vision.kt b/src/main/java/frc/robot/util/Vision.kt
index bc591de..05c5e36 100644
--- a/src/main/java/frc/robot/util/Vision.kt
+++ b/src/main/java/frc/robot/util/Vision.kt
@@ -54,7 +54,8 @@ class VisionSystem {
             }
 
             if (!bottomM2Reset) {
-                if (llName == "limelight-tright" || llName == "limelight-tleft") {
+//                if (llName == "limelight-tright" || llName == "limelight-tleft") {
+                if (llName == "limelight-tright") {
                     continue;
                 }
             }
@@ -117,7 +118,9 @@ class VisionSystem {
                         degStds = 300.0
                     }
 
-                    if (llName == "limelight-tright" || llName == "limelight-tleft") {
+                    if (llName == "limelight-tright") {
+
+//                        if (llName == "limelight-tright" || llName == "limelight-tleft") {
                         xyStds *= 4.0
                         degStds *= 4.0
                     }
diff --git a/src/main/java/frc/robot/util/input/ManualDriverInput.kt b/src/main/java/frc/robot/util/input/ManualDriverInput.kt
index 70f5d2c..8f73728 100644
--- a/src/main/java/frc/robot/util/input/ManualDriverInput.kt
+++ b/src/main/java/frc/robot/util/input/ManualDriverInput.kt
@@ -14,7 +14,8 @@ object ManualDriverInput {
         RobotContainer.rightJoystick.button(2)
         .onTrue(Commands.runOnce({ RobotContainer.drivetrain.seedFieldCentric(); println("seeding field relative") }))
 
-        RobotContainer.rightJoystick.button(cancel).onTrue(Commands.runOnce({
+        //NOTE: rebound
+        RobotContainer.rightJoystick.button(10).onTrue(Commands.runOnce({
             if (!RobotContainer.currentDriveCommand.isEmpty) {
                 println("cancelling auto drive")
                 RobotContainer.currentDriveCommand.get().cancel()
@@ -23,6 +24,17 @@ object ManualDriverInput {
             }
         }))
 
+        RobotContainer.rightJoystick.button(4).onTrue(Commands.run({
+            OperatorControls.noWalk = true
+            alignCommand.cancel()
+            alignCommand = OperatorControls.action.alignCommand
+            alignCommand.schedule()
+        }))
+
+        RobotContainer.rightJoystick.button(4).onFalse(Commands.runOnce({
+            alignCommand.cancel()
+        }))
+
         RobotContainer.rightJoystick.button(3).onTrue(Commands.runOnce({
             OperatorControls.noWalk = false
             alignCommand.cancel()
diff --git a/src/main/java/frc/robot/util/input/OperatorControls.kt b/src/main/java/frc/robot/util/input/OperatorControls.kt
index 833fd97..f4799d0 100644
--- a/src/main/java/frc/robot/util/input/OperatorControls.kt
+++ b/src/main/java/frc/robot/util/input/OperatorControls.kt
@@ -10,6 +10,7 @@ import frc.robot.subsystems.superstructure.Superstructure
 enum class DriverAction(val superStructureCommand: Command, val alignCommand: Command = Commands.runOnce({})) {
     //    Stow(Commands.runOnce({ Superstructure.stow() })),
     IntakeFeeder(Commands.runOnce({ Superstructure.intakeFeeder() }), TeleopAutoAlign.feederAlign()),
+    ScoreL1(Commands.runOnce({ Superstructure.scoreL1() }), TeleopAutoAlign.l1ReefAlign()),
     ScoreL2(Commands.runOnce({ Superstructure.scoreL2() }), TeleopAutoAlign.coralReefAlign()),
     ScoreL3(Commands.runOnce({ Superstructure.scoreL3() }), TeleopAutoAlign.coralReefAlign()),
     ScoreL4(Commands.runOnce({ Superstructure.scoreL4() }), TeleopAutoAlign.coralReefAlign()),
@@ -32,6 +33,7 @@ object OperatorControls {
     val action
         get() = when (SmartDashboard.getString("ConsoleDriverAction", "")) {
             "Feeder" -> DriverAction.IntakeFeeder
+            "L1" -> DriverAction.ScoreL1
             "L2" -> DriverAction.ScoreL2
             "L3" -> DriverAction.ScoreL3
             "L4" -> DriverAction.ScoreL4
@@ -60,6 +62,9 @@ object OperatorControls {
             else -> AutoScoringConstants.ReefPositions.A
         }
 
+    val highStowPosition
+        get() = SmartDashboard.getBoolean("ConsoleHighStowPosition", false)
+
 //    val actionChooser = SendableChooser<DriverAction>()
 
 //    val coralSideChooser = SendableChooser<AutoTargeting.CoralSide>()