diff --git a/.glass/glass.json b/.glass/glass.json
index da2e636a..6fdd498e 100644
--- a/.glass/glass.json
+++ b/.glass/glass.json
@@ -22,10 +22,16 @@
     },
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Autonomous/Autonomous": "String Chooser",
+      "/SmartDashboard/Arm/sendable": "Subsystem",
       "/SmartDashboard/Drivetrain/sendable": "Subsystem",
       "/SmartDashboard/FIELD": "Field2d",
       "/SmartDashboard/Field": "Field2d",
-      "/SmartDashboard/elevcmd": "Subsystem"
+      "/SmartDashboard/Manipulator/sendable": "Subsystem",
+      "/SmartDashboard/elevator/sendable": "Subsystem",
+      "/SmartDashboard/elevcmd": "Subsystem",
+      "/SmartDashboard/field": "Field2d",
+      "/SmartDashboard/huhhh": "Subsystem"
     },
     "windows": {
       "/FMSInfo": {
@@ -33,11 +39,6 @@
           "visible": true
         }
       },
-      "/SmartDashboard/FIELD": {
-        "window": {
-          "visible": true
-        }
-      },
       "/SmartDashboard/Field": {
         "Robot": {
           "length": 0.7620000243186951,
@@ -54,6 +55,18 @@
         "window": {
           "visible": true
         }
+      },
+      "/SmartDashboard/field": {
+        "bottom": 544,
+        "height": 8.013679504394531,
+        "image": "C:\\Users\\frc\\IdeaProjects\\FRC2023\\field\\2023-field.png",
+        "left": 46,
+        "right": 1088,
+        "top": 36,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
       }
     }
   },
@@ -81,7 +94,7 @@
     }
   },
   "NetworkTables Settings": {
-    "mode": "Client (NT4)",
+    "mode": "Client (NT3)",
     "serverTeam": "6502"
   },
   "Plots": {
@@ -133,7 +146,10 @@
             }
           ]
         }
-      ]
+      ],
+      "window": {
+        "visible": false
+      }
     }
   }
 }
diff --git a/src/main/kotlin/frc/robot/Robot.kt b/src/main/kotlin/frc/robot/Robot.kt
index 8a180bf1..4e01e470 100644
--- a/src/main/kotlin/frc/robot/Robot.kt
+++ b/src/main/kotlin/frc/robot/Robot.kt
@@ -33,7 +33,7 @@ class Robot : TimedRobot() {
     }
 
     override fun teleopInit() {
-        ZeroModeMotor(robotContainer.intake).schedule()
+        ZeroModeMotor(robotContainer).schedule()
     }
 
     var auto: Command? = null
diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt
index 7572b4f1..c83b50b3 100644
--- a/src/main/kotlin/frc/robot/RobotContainer.kt
+++ b/src/main/kotlin/frc/robot/RobotContainer.kt
@@ -12,8 +12,10 @@ import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.DriverStation.Alliance.Blue
 import edu.wpi.first.wpilibj.DriverStation.Alliance.Red
 import edu.wpi.first.wpilibj.GenericHID
+import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.Command
@@ -41,6 +43,7 @@ import frc.robot.commands.pathing.*
 import frc.robot.commands.pathing.building.blocks.BuildingBlocks.goToHumanPlayerStation
 import frc.robot.commands.pathing.building.blocks.BuildingBlocks.goToPlacementPoint
 import frc.robot.constants.Field2dLayout
+import frc.robot.constants.drivetrain
 import frc.robot.constants.leds.count
 import frc.robot.controls.BryanControlScheme
 import frc.robot.controls.ChrisControlScheme
@@ -314,8 +317,10 @@ class RobotContainer {
                 shootToLThree
                     .whileTrue(ShootToLTwo(intake, this@RobotContainer))
 
-//                snapTo180
-//                    .whileTrue(RotateTo180(this@RobotContainer))
+                snapTo180
+                    .whileTrue(
+                        MoveToPosition(drivetrain, drivetrain.estimatedPose2d, 180.0)
+                    )
             }
         }
     }
@@ -472,7 +477,10 @@ class RobotContainer {
             return@AnimationCustom List<Color>(len) { i ->
                 if (i >= index) color else Color.black
             }
-        }, { false && !drivetrain.canTrustPose && (lightStatus != TeleopFMSRed || lightStatus != TeleopFMSBlue) })
+        }, {
+            @Suppress("KotlinConstantConditions", "SimplifyBooleanWithConstants")
+            false && !drivetrain.canTrustPose && (lightStatus != TeleopFMSRed || lightStatus != TeleopFMSBlue)
+        })
 
 
         val body = KLEDRegion(
@@ -538,10 +546,11 @@ class RobotContainer {
     }
 
     // shuffleboard auto chooser
-    val autoChooserTab: ShuffleboardTab = Shuffleboard.getTab("Autonomous")
-    val autoChooserWidget = autoChooserTab.add("Autonomous", autoChooser)
+    private val autoChooserTab: ShuffleboardTab = Shuffleboard.getTab("Autonomous")
 
-    val armFieldPosition = drivetrain.field2d.getObject("arm")
+    @Suppress("unused")
+    val autoChooserWidget: ComplexWidget = autoChooserTab.add("Autonomous", autoChooser)
+    val armFieldPosition: FieldObject2d = drivetrain.field2d.getObject("arm")
 
 //    val DriveTab: ShuffleboardTab = Shuffleboard.getTab("DriveTab")
 //    val autoChoice = DriveTab.add("Autonomous", autoChooser)
diff --git a/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt b/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt
index cb5f9809..71f475eb 100644
--- a/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt
+++ b/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt
@@ -79,10 +79,10 @@ enum class IOLevel(
         0.0, 0.0
     ),
     Balance(
-        bottomLimit,
-        Rotation2d.fromRadians(-PI / 2),
-        bottomLimit,
-        Rotation2d.fromRadians(-PI / 2),
+        bottomLimit + 0.28,
+        Rotation2d.fromRadians(-PI / 2).plus(Rotation2d.fromDegrees(-10.0)),
+        bottomLimit + 0.28,
+        Rotation2d.fromRadians(-PI / 2).plus(Rotation2d.fromDegrees(-10.0)),
         0.0, 0.0
     )
 }
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt b/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt
index 343321dc..56a78f4b 100644
--- a/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt
+++ b/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt
@@ -78,3 +78,5 @@ class SetSubsystemPosition(
             (elevator.height - goalElevatorPosition).absoluteValue < 0.05
 }
 
+
+
diff --git a/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt b/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt
index 7fc86344..948ca35d 100644
--- a/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt
+++ b/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt
@@ -50,7 +50,7 @@ class AutoBalance(private val drivetrain: Drivetrain) : CommandBase() {
                 true
             )
         } else {
-            drivetrain.drive(ChassisSpeeds(2.0 * speedMul, 0.0, 0.0), true)
+            drivetrain.drive(ChassisSpeeds(3.0 * speedMul, 0.0, 0.0), true)
         }
 
 
diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/BackupSnapTo180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/BackupSnapTo180.kt
new file mode 100644
index 00000000..afc65299
--- /dev/null
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/BackupSnapTo180.kt
@@ -0,0 +1,22 @@
+package frc.robot.commands.drivetrain
+
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.wpilibj2.command.CommandBase
+import frc.robot.RobotContainer
+import frc.robot.commands.pathing.MoveToPosition
+import frc.robot.constants.drivetrain
+
+class BackupSnapTo180 (
+    val robotContainer: RobotContainer
+) : CommandBase(){
+
+    override fun execute() {
+        //MoveToPosition(robotContainer.drivetrain, Pose2d())
+        DriveCommand(robotContainer.drivetrain, { 0.0 }, { 0.0 }, )
+        MoveToPosition()
+    }
+    override fun end(interrupted: Boolean) {
+        robotContainer.rotateTo180 = false
+    }
+
+}
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt
index a18233dc..22e99307 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt
@@ -20,7 +20,7 @@ class DriveCommand(
     override fun execute() {
         drivetrain.drive(
             ChassisSpeeds(x(), y(), rotation()),
-            true,
+            isFieldOriented,
             Translation2d(0.0, 0.0)
         )
     }
@@ -28,7 +28,7 @@ class DriveCommand(
     override fun end(interrupted: Boolean) {
         drivetrain.drive(
             ChassisSpeeds(0.0, 0.0, 0.0),
-            true,
+            isFieldOriented,
             Translation2d(0.0, 0.0)
         )
     }
diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
index b2dc372f..325856b0 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
@@ -4,10 +4,15 @@ import edu.wpi.first.math.controller.ProfiledPIDController
 import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.trajectory.TrapezoidProfile
+import edu.wpi.first.math.util.Units.degreesToRadians
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.CommandBase
 import frc.kyberlib.command.Game
+import frc.robot.commands.pathing.MoveToPosition
 import frc.robot.constants.Constants
+import frc.robot.constants.drivetrain.maxVelocity
+import frc.robot.constants.drivetrain.maxAutonomousAngularAcceleration
+import frc.robot.constants.drivetrain.maxAutonomousAngularVelocity
 import frc.robot.controls.ControlScheme
 import frc.robot.subsystems.Drivetrain
 import frc.robot.subsystems.slewLimited
@@ -19,21 +24,31 @@ class DriverCommand(
     var controlScheme: ControlScheme,
     var nearestStation: () -> Boolean,
 ) : CommandBase() {
-    private val rotationPid = ProfiledPIDController(1.0, 0.0, 0.0, TrapezoidProfile.Constraints(2*PI, 4*PI)).apply {
+    private val rotationPid = ProfiledPIDController(
+        MoveToPosition.rP, 0.0, 0.0, TrapezoidProfile.Constraints(
+            maxAutonomousAngularVelocity, maxAutonomousAngularAcceleration
+        )
+    ).apply {
         enableContinuousInput(-PI, PI)
+
     }
+    var lastNearestStation = nearestStation()
 
     init {
         addRequirements(drivetrain)
     }
 
     override fun execute() {
+        if (nearestStation() != lastNearestStation) {
+            lastNearestStation = nearestStation()
+            rotationPid.reset(drivetrain.estimatedPose2d.rotation.radians)
+        }
         val allianceMulitplier = when (Game.alliance) {
             DriverStation.Alliance.Invalid -> 1.0
             else -> Game.alliance.xMul
         }
         val vec = Translation2d(-controlScheme.forward, -controlScheme.strafe)
-            .times(3.5)
+            .times(maxVelocity)
         drivetrain.drive(
             ChassisSpeeds(
                 vec.x * Constants.powerPercent *
@@ -44,14 +59,14 @@ class DriverCommand(
                         * controlScheme.speedMutiplier,
                 if(nearestStation()) {
                     rotationPid.calculate(drivetrain.estimatedPose2d.rotation.radians,
-                         Nearest180.findNearest180(drivetrain)
+                        degreesToRadians(findNearest180(drivetrain.estimatedPose2d.rotation.degrees))
                     ) // desired radians
                 }
                 else{
                     -controlScheme.rotation * 2 * Math.PI *
                             Constants.powerPercent * .5 * (if (drivetrain.invertrot.getBoolean(false)) -1 else 1) * controlScheme.speedMutiplier
                 }
-            ).slewLimited(drivetrain.xSlewRateLimiter, drivetrain.ySlewRateLimiter, drivetrain.rotSlewRateLimiter),
+            ),//.slewLimited(drivetrain.xSlewRateLimiter, drivetrain.ySlewRateLimiter, drivetrain.rotSlewRateLimiter),
             true,
             Translation2d() // chris wants in the middle
         )
diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
index 9a6535d7..5b09cfd3 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
@@ -1,14 +1,16 @@
 package frc.robot.commands.drivetrain
 
-import edu.wpi.first.math.util.Units
-import edu.wpi.first.wpilibj2.command.CommandBase
-import frc.robot.subsystems.Drivetrain
-import kotlin.math.round
-
-object Nearest180{
-    fun findNearest180(
-        drivetrain: Drivetrain
-    ): Double {
-        return Units.degreesToRadians(round((drivetrain.estimatedPose2d.rotation.degrees/180) * 180))
+fun findNearest180(
+    currentAngle: Double,
+): Double {
+    // Find the nearest 180 degree angle. the input should be in degrees with a range of -180 to 180
+    // the output will be in degrees with a range of -180 to 180 and will be the closest 180 degree angle to the input
+    // for example, if the input is 0, the output will be 0, if the input is 179, the output will be 180, if the input
+    // is -179, the output will be 180 etc.
+    val nearest180 = currentAngle % 360
+    return if (nearest180 >=90 || nearest180 <= -90) {
+        180.0
+    } else {
+        0.0
     }
-}
\ No newline at end of file
+}
diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt
index 9fee6753..f055c3cd 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt
@@ -6,9 +6,10 @@ import frc.robot.RobotContainer
 class RotateTo180(
     val robotContainer: RobotContainer,
 ) : CommandBase() {
-    override fun execute() {
+    override fun initialize() {
         robotContainer.rotateTo180 = true
     }
+
     override fun end(interrupted: Boolean) {
         robotContainer.rotateTo180 = false
     }
diff --git a/src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt b/src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt
new file mode 100644
index 00000000..3149617b
--- /dev/null
+++ b/src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt
@@ -0,0 +1,33 @@
+//package frc.robot.commands.intake
+//
+//import edu.wpi.first.wpilibj.Timer
+//import edu.wpi.first.wpilibj2.command.CommandBase
+//import frc.robot.RobotContainer
+//import frc.robot.constants.intake
+//import frc.robot.subsystems.Intake
+//
+//class ManualZeroIntake(private val robotContainer: RobotContainer): CommandBase() {
+//
+////    val timer = Timer()
+//    init {
+//        addRequirements(robotContainer.intake)
+//    }
+//
+////    override fun initialize() {
+////        timer.restart()
+////        robotContainer.intake.modeZeroed = false
+////        robotContainer.intake.setDeployAngle(robotContainer.intake.deployPosition)
+////    }
+//    override fun execute() {
+//        robotContainer.intake.modeVoltage = -3.0
+//        robotContainer.intake.setModeAngle(-1.7)
+//    }
+//
+//    override fun end(interrupted: Boolean) {
+//        robotContainer.intake.zeroModeMotor()
+//        robotContainer.intake.modeVoltage = 0.0
+//    }
+//
+//    override fun isFinished() = intake.an
+//
+//}
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt b/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt
index f4ad37c5..5b037d10 100644
--- a/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt
+++ b/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt
@@ -2,27 +2,28 @@ package frc.robot.commands.intake
 
 import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj2.command.CommandBase
+import frc.robot.RobotContainer
 import frc.robot.subsystems.Intake
 
-class ZeroModeMotor(private val intake: Intake): CommandBase() {
+class ZeroModeMotor(private val robotContainer: RobotContainer): CommandBase() {
 
     val timer = Timer()
     init {
-        addRequirements(intake)
+        addRequirements(robotContainer.intake)
     }
 
     override fun initialize() {
         timer.restart()
-        intake.modeZeroed = false
-        intake.setDeployAngle(intake.deployPosition)
+        robotContainer.intake.modeZeroed = false
+        robotContainer.intake.setDeployAngle(robotContainer.intake.deployPosition)
     }
     override fun execute() {
-        intake.modeVoltage = -3.0
+        robotContainer.intake.modeVoltage = -3.0
     }
 
     override fun end(interrupted: Boolean) {
-        intake.zeroModeMotor()
-        intake.modeVoltage = 0.0
+        robotContainer.intake.zeroModeMotor()
+        robotContainer.intake.modeVoltage = 0.0
     }
 
     override fun isFinished() = timer.hasElapsed(1.0)
diff --git a/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt b/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt
index 5398b4cd..fcfe9978 100644
--- a/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt
+++ b/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt
@@ -16,6 +16,7 @@ import frc.robot.utils.GamePiece
 import frc.robot.utils.grid.PlacementGroup
 import frc.robot.utils.grid.PlacementLevel
 import frc.robot.utils.grid.PlacementSide
+import java.lang.Exception
 
 class AutoPlaceAndBalance(
     private val robotContainer: RobotContainer,
@@ -52,25 +53,31 @@ class AutoPlaceAndBalance(
                 Throw(
                     robotContainer.manipulator,
                     { GamePiece.cone },
-                    { PlacementLevel.Level3 }).withTimeout(0.75)
+                    { PlacementLevel.Level3 })
+                    .withTimeout(0.5)
             ) // shoot cube
             .withTimeout(4.0)
             .andThen(
-                MoveToPosition(robotContainer.drivetrain, 14.4, 2.72, 180.0, maxPosSpeed = 3.7)
+                MoveToPosition(robotContainer.drivetrain,
+                    14.4, 2.72, 180.0,
+                    maxPosSpeed = 3.7
+                )
                     .andThen(
-                        MoveToPosition(
-                            robotContainer.drivetrain,
-                            (13.34 - 0.5),
-                            2.72,
-                            180.0,
+                        MoveToPosition(robotContainer.drivetrain,
+                            (13.34 - 0.5), 2.72, 180.0,
                             maxPosSpeed = 3.7
                         ).withTimeout(4.0)
                     )
-                    .alongWith(
+                    .deadlineWith(
+                        SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
+                    )
+            )
+            .andThen(
+                AutoBalance(robotContainer.drivetrain)
+                    .deadlineWith(
                         SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
                     )
             )
-            .andThen(AutoBalance(robotContainer.drivetrain))
     }
 
     private fun pathBlue(robotContainer: RobotContainer): Command {
@@ -80,7 +87,8 @@ class AutoPlaceAndBalance(
                 { IOLevel.High },
                 { GamePiece.cone },
                 true
-            ))
+            )
+            )
             .andThen(
                 SetSubsystemPosition(
                     robotContainer,
@@ -103,13 +111,17 @@ class AutoPlaceAndBalance(
                     .andThen(
                         MoveToPosition(robotContainer.drivetrain, 16.52 - (13.34 - 0.5), 2.72, 0.0,
                             maxPosSpeed = 3.7
-                        )
-                            .withTimeout(4.0)
+                        ).withTimeout(4.0)
                     )
-                    .alongWith(
+                    .deadlineWith(
+                        SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
+                    )
+            )
+            .andThen(
+                AutoBalance(robotContainer.drivetrain)
+                    .deadlineWith(
                         SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
                     )
             )
-            .andThen(AutoBalance(robotContainer.drivetrain))
     }
 }
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt b/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt
index 23c8cfb3..d4fd521f 100644
--- a/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt
+++ b/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt
@@ -32,6 +32,7 @@ val Pose2d.flipped: Pose2d
 //fun safePrint(vararg message: String, separator: String = "") {
 //    message.joinToString {  }
 //}
+
 open class MoveToPosition(
     private val drivetrain: Drivetrain,
     /**
@@ -46,7 +47,7 @@ open class MoveToPosition(
      * The desired velocity of the robot (in meters per second)
      */
     private val velocity: Transform2d = Transform2d(),
-    private val toleranceppos: Double = 0.02,
+    private val toleranceppos: Double = 0.075,
     private val tolerancepvel: Double = 0.1,
     private val tolerancerpos: Double = 0.01,
     private val tolerancervel: Double = 0.1,
@@ -143,6 +144,12 @@ open class MoveToPosition(
         rPIDController.reset(drivetrain.estimatedPose2d.rotation.radians, drivetrain.estimatedVelocity.rotation.radians)
 
         visualization.pose = pose(xPIDController, yPIDController, rPIDController)
+
+        val current = drivetrain.estimatedPose2d
+        val desired = pose(xPIDController, yPIDController, rPIDController)
+        if (desired.translation.getDistance(current.translation) > 8.0) {
+            print("Unable to path")
+        }
     }
 
     // on command start and every time the command is executed, calculate the
diff --git a/src/main/kotlin/frc/robot/constants/Constants.kt b/src/main/kotlin/frc/robot/constants/Constants.kt
index c6d54652..8946d302 100644
--- a/src/main/kotlin/frc/robot/constants/Constants.kt
+++ b/src/main/kotlin/frc/robot/constants/Constants.kt
@@ -66,7 +66,7 @@ object Constants {
     const val driveKV = 2.2335
     const val driveKA = 0.34271
 
-    const val ANGLE_P = 4.1487 //fixme pid
+    const val ANGLE_P = 4.1487
     const val ANGLE_I = 0.0
     const val ANGLE_D = .14002
 
diff --git a/src/main/kotlin/frc/robot/constants/Field2dLayout.kt b/src/main/kotlin/frc/robot/constants/Field2dLayout.kt
index 500f79a8..278f35b8 100644
--- a/src/main/kotlin/frc/robot/constants/Field2dLayout.kt
+++ b/src/main/kotlin/frc/robot/constants/Field2dLayout.kt
@@ -2,7 +2,6 @@ package frc.robot.constants
 
 import edu.wpi.first.math.geometry.Translation2d
 
-@Suppress("unused") //TODO: Remove this suppression
 object Field2dLayout {
     const val xCenter = 8.28
 
diff --git a/src/main/kotlin/frc/robot/constants/FieldConstants.kt b/src/main/kotlin/frc/robot/constants/FieldConstants.kt
index f0511e2e..fbd42bb5 100644
--- a/src/main/kotlin/frc/robot/constants/FieldConstants.kt
+++ b/src/main/kotlin/frc/robot/constants/FieldConstants.kt
@@ -2,9 +2,8 @@ package frc.robot.constants
 
 import edu.wpi.first.math.util.Units
 
-@Suppress("unused") //TODO: Remove this suppression
 object FieldConstants {
-    val heightLimit = Units.feetToMeters(8.5) //FIXME: is this correct
+    val heightLimit = Units.feetToMeters(6.5)
     const val width = 3.048
     const val length = 5.486
 }
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/constants/drivetrain.kt b/src/main/kotlin/frc/robot/constants/drivetrain.kt
index 5abf2d76..f4383d09 100644
--- a/src/main/kotlin/frc/robot/constants/drivetrain.kt
+++ b/src/main/kotlin/frc/robot/constants/drivetrain.kt
@@ -4,14 +4,14 @@ import kotlin.math.PI
 
 object drivetrain {
     const val maxAcceleration = 48.0
-    const val maxAngularAcceleration = 3.0 * 2 * PI
+    const val maxAngularAcceleration = 5.0 * 2 * PI
 
     const val maxVelocity = 4.0
     const val maxAngularVelocity = 2.0 * 2 * PI
 
-    const val maxAutonomousVelocity = maxVelocity
-    const val maxAutonomousAngularVelocity = maxAngularVelocity
+    const val maxAutonomousVelocity = maxVelocity * .75
+    const val maxAutonomousAngularVelocity = maxAngularVelocity * .75
 
-    const val maxAutonomousAcceleration = maxAcceleration * .75
+    const val maxAutonomousAcceleration = maxAcceleration * .85
     const val maxAutonomousAngularAcceleration = maxAngularAcceleration * .75
 }
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/constants/intake.kt b/src/main/kotlin/frc/robot/constants/intake.kt
index 3022874e..cacc6724 100644
--- a/src/main/kotlin/frc/robot/constants/intake.kt
+++ b/src/main/kotlin/frc/robot/constants/intake.kt
@@ -25,7 +25,7 @@ object deployMotor {
     const val maxAcceleration = 18.0
     const val kP = 4.0
     const val kI = 0.0
-    const val kD = 0.01
+    const val kD = 0.05
 
     const val kS = 0.0
     const val kG = 0.0
@@ -45,7 +45,7 @@ object deployMotor {
         const val maxAcceleration = 32.0
         const val kP = 3.05
         const val kI = 0.0
-        const val kD = 0.01
+        const val kD = 0.035
 
         const val kS = 0.0
         const val kG = 0.0
diff --git a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
index 83535ba9..67f46e29 100644
--- a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
+++ b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
@@ -11,7 +11,7 @@ class ChrisControlScheme(
     override val xbox = CommandXboxController(xboxNum)
 
     override val speedMutiplier: Double
-        get() = (1.0 - (0.8*(round(xbox.rightTriggerAxis * 3)/3.0)))
+        get() = (1.0 - (0.3 * (round(xbox.rightTriggerAxis * 3) / 3.0)))
             .coerceIn(0.0, 1.0)
 
     override val rotation: Double
@@ -31,5 +31,8 @@ class ChrisControlScheme(
     override val decreaseEncoderAngle: Trigger = xbox.povLeft()
     override val increaseEncoderAngle: Trigger = xbox.povRight()
     override val lockSwerveModulesCircle: Trigger = xbox.rightBumper()
-    //override val snapTo180: Trigger = xbox.rightTrigger()
-}
\ No newline at end of file
+
+    override val snapTo180: Trigger = xbox.leftTrigger()
+    override val zeroIntake: Trigger = xbox.povUp()
+//    override val balanceIOLevel: Trigger = xbox.povDown()
+}
diff --git a/src/main/kotlin/frc/robot/controls/ControlScheme.kt b/src/main/kotlin/frc/robot/controls/ControlScheme.kt
index d8d21e70..f49771b9 100644
--- a/src/main/kotlin/frc/robot/controls/ControlScheme.kt
+++ b/src/main/kotlin/frc/robot/controls/ControlScheme.kt
@@ -7,7 +7,7 @@ abstract class ControlScheme {
     abstract val xbox: CommandXboxController?
 
     open val speedMutiplier: Double
-        get() = 0.0
+        get() = 1.0
 
     open val rotation: Double
         get() = 0.0
@@ -28,8 +28,6 @@ abstract class ControlScheme {
 
     open val spinIntakeIn: Trigger = Trigger { false }
     open val spinIntakeOut: Trigger = Trigger { false }
-    open val stopIntake: Trigger = Trigger { false }
-
 
     open val moveToClosestHPSAxis: Trigger = Trigger { false }
     open val moveToClosestScoreStationAxis: Trigger = Trigger { false }
@@ -47,8 +45,6 @@ abstract class ControlScheme {
 
     open val alignClosestHPS: Trigger = Trigger { false }
 
-    open val overrideAlignmentSafety: Trigger = Trigger { false }
-
     open val selectGridUp: Trigger = Trigger { false }
     open val selectGridDown: Trigger = Trigger { false }
     open val selectGridLeft: Trigger = Trigger { false }
@@ -60,11 +56,13 @@ abstract class ControlScheme {
 
     open val lockSwerveModulesCircle: Trigger = Trigger { false }
 
-    //open val snapTo180: Trigger = Trigger { false }
+    open val snapTo180: Trigger = Trigger { false }
 
     open val intakeGroundIntake: Trigger = Trigger { false }
     open val intakeEject: Trigger = Trigger { false }
     open val shootToLTwo: Trigger = Trigger { false }
     open val shootToLThree: Trigger = Trigger { false }
+    open val zeroIntake: Trigger = Trigger { false }
+//    open val balanceIOLevel: Trigger = Trigger { false }
 
 }
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
index bfaf5dfe..da30f710 100644
--- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
+++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
@@ -10,8 +10,8 @@ import edu.wpi.first.math.geometry.Transform2d
 import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics
-import edu.wpi.first.math.kinematics.SwerveModuleState
 import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard.getTab
 import edu.wpi.first.wpilibj.smartdashboard.Field2d
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
@@ -23,7 +23,7 @@ import frc.robot.commands.drivetrain.DriverCommand
 import frc.robot.constants.Constants
 import frc.robot.constants.drivetrain
 import frc.robot.controls.ControlScheme
-import java.lang.Math.PI
+import kotlin.math.PI
 
 class Drivetrain(
     controlScheme: ControlScheme,
@@ -45,15 +45,15 @@ class Drivetrain(
     private val rotEntry = swerveTab.add("xBox rot", 0)
         .entry
     val invertx = swerveTab.add("invert x", false)
-        .withWidget("Toggle Button")
+        .withWidget(BuiltInWidgets.kToggleButton)
         .entry
     val inverty = swerveTab.add("invert y", false)
-        .withWidget("Toggle Button")
+        .withWidget(BuiltInWidgets.kToggleButton)
         .entry
     val invertrot = swerveTab.add("invert rot", false)
-        .withWidget("Toggle Button")
+        .withWidget(BuiltInWidgets.kToggleButton)
         .entry
-    val frontLeft = SwerveModule( // front right
+    private val frontLeft = SwerveModule( // front right
         Constants.FLDriveMotorId,
         Constants.FLTurnMotorId,
         Constants.FLTurnEncoderId,
@@ -64,7 +64,7 @@ class Drivetrain(
             Constants.MODULE_DISTANCE_Y / 2
         )
     )
-    val frontRight = SwerveModule( // backleft
+    private val frontRight = SwerveModule( // backleft
         Constants.FRDriveMotorId,
         Constants.FRTurnMotorId,
         Constants.FRTurnEncoderId,
@@ -75,7 +75,7 @@ class Drivetrain(
             -Constants.MODULE_DISTANCE_Y / 2
         )
     )
-    val backLeft = SwerveModule(
+    private val backLeft = SwerveModule(
         Constants.BLDriveMotorId,
         Constants.BLTurnMotorId,
         Constants.BLTurnEncoderId,
@@ -86,7 +86,7 @@ class Drivetrain(
         ),
         angleZero = Constants.BLZeroAngle,
     )
-    val backRight = SwerveModule(
+    private val backRight = SwerveModule(
         Constants.BRDriveMotorId,
         Constants.BRTurnMotorId,
         Constants.BRTurnEncoderId,
@@ -99,7 +99,7 @@ class Drivetrain(
         angleZero = Constants.BRZeroAngle,
     )
     val modules = listOf(frontLeft, frontRight, backLeft, backRight)
-    val kinematics = SwerveDriveKinematics(
+    private val kinematics = SwerveDriveKinematics(
         *modules.map { it.location }.toTypedArray()
     )
 
@@ -107,8 +107,6 @@ class Drivetrain(
         configFactoryDefault()
     }
 
-    private val f2d = Field2d()
-
     val Idrc = getTab("drivetrain")
 
     // pose shuffleboard stuff (using the field 2d widget)
@@ -120,12 +118,13 @@ class Drivetrain(
         }.toTypedArray(),
         Pose2d(),
         VecBuilder.fill(0.1, 0.1, 0.1),
-        VecBuilder.fill(1.8, 1.8, 1.8)
+        VecBuilder.fill(1.8, 1.8, 3.1)
     )
 
     @Deprecated("Use estimatedPose2d instead, this is only for internal drivetrain use")
     var simEstimatedPose2d: Pose2d = Pose2d()
 
+    @Suppress("DEPRECATION")
     inline val estimatedPose2d: Pose2d
         get() = if (!Game.sim) {
             poseEstimator.estimatedPosition
@@ -167,17 +166,17 @@ class Drivetrain(
         this.robotPose = estimatedPose2d
     }
 
-    val gyroPitchWidget = Idrc.add("Gyro Pitch", 0.0)
+    private val gyroPitchWidget = Idrc.add("Gyro Pitch", 0.0)
         .withWidget("Gryo")
         .withProperties(mapOf("min" to -180.0, "max" to 180.0))
         .withSize(2, 2)
         .entry
-    val gyroYawWidget = Idrc.add("Gyro Yaw", 0.0)
+    private val gyroYawWidget = Idrc.add("Gyro Yaw", 0.0)
         .withWidget("Gryo")
         .withProperties(mapOf("min" to -180.0, "max" to 180.0))
         .withSize(2, 2)
         .entry
-    val gyroRollWidget = Idrc.add("Gyro Roll", 0.0)
+    private val gyroRollWidget = Idrc.add("Gyro Roll", 0.0)
         .withWidget("Gryo")
         .withProperties(mapOf("min" to -180.0, "max" to 180.0))
         .withSize(2, 2)
@@ -227,28 +226,10 @@ class Drivetrain(
         SmartDashboard.putNumber("uptime", gyro.upTime.toDouble())
         SmartDashboard.putNumber("posex", poseEstimator.estimatedPosition.translation.x)
         SmartDashboard.putNumber("posey", poseEstimator.estimatedPosition.translation.y)
-//        cameraWrappers[0].getEstimatedGlobalPose(poseEstimator.estimatedPosition).ifPresent {
-//            SmartDashboard.putNumber("poseyCamera", it.estimatedPose.translation.y)
-//            SmartDashboard.putNumber("posexCamera", it.estimatedPose.translation.x)
-//        }
-
-
-//        gyroEntry.setDouble(gyro.yaw)
-//        odometry.update(
-//            Rotation2d.fromDegrees(gyro.yaw),
-//            modules.map { it.position.toSwerveModulePosition() }.toTypedArray()
-//        )
 
         if (Game.sim) {
             val vel = estimatedVelocity
-//            simEstimatedPose2d =
-//                Pose2d(
-//                    Translation2d(
-//                        simEstimatedPose2d.x + (vel.translation.x * 0.02 * 0.01) + simQueuedForce.translation.x,
-//                        simEstimatedPose2d.y + (vel.translation.y * 0.02 * 0.01) + simQueuedForce.translation.y
-//                    ),
-//                    Rotation2d(simEstimatedPose2d.rotation.radians + (vel.rotation.radians * 0.02 * 0.01) + simQueuedForce.rotation.radians)
-//                )
+            @Suppress("DEPRECATION")
             simEstimatedPose2d = simEstimatedPose2d + (vel * 0.02 * 0.01) + simQueuedForce
             simQueuedForce = Transform2d()
         }
@@ -295,12 +276,19 @@ class Drivetrain(
             chassisSpeedsField,
             rotAxis
         )
+
+        val currentChassisSpeeds = kinematics.toChassisSpeeds(*swerveModuleStates)
+
         SwerveDriveKinematics.desaturateWheelSpeeds(
+            /* moduleStates = */ swerveModuleStates,
+            /* currentChassisSpeed = */ currentChassisSpeeds,
+            /*
             swerveModuleStates,
             chassisSpeedsField,
-            4.0,
-            2.0,
-            2 * PI
+             */
+            /* attainableMaxModuleSpeedMetersPerSecond = */ 4.0,
+            /* attainableMaxTranslationalSpeedMetersPerSecond = */ 4.0,
+            /* attainableMaxRotationalVelocityRadiansPerSecond = */ PI,
         )
 
         if (Game.real) {
@@ -311,10 +299,6 @@ class Drivetrain(
             xSpeedEntry.setDouble(chassisSpeeds.vxMetersPerSecond)
             ySpeedEntry.setDouble(chassisSpeeds.vyMetersPerSecond)
             rotEntry.setDouble(chassisSpeeds.omegaRadiansPerSecond)
-            //fixme: move the following to swerve module periodic
-            modules.forEachIndexed { i, module ->
-                module.stateEntry.setDouble(swerveModuleStates[i].speedMetersPerSecond)
-            }
         } else {
             val newChassisSpeeds = kinematics.toChassisSpeeds(*swerveModuleStates)
             // add the chassis speeds to the sim pose with dt = 0.02
@@ -356,39 +340,6 @@ class Drivetrain(
         }
     }
 
-    inline var swerveModuleStates: List<SwerveModuleState>
-        get() = this.modules.map { it.currentPosition }
-        set(value) = value.forEachIndexed { i, it ->
-            modules[i].setpoint = it
-        }
-    var powerSaveMode: Int = 0
-        set(value) {
-            field = value
-            if (value == 0) {
-                modules.forEach { it.powerSaveMode = false }
-            } else {
-                modules.forEachIndexed { i, module ->
-                    module.powerSaveMode = i < value
-                    // only enable power save mode for the first n modules
-                }
-            }
-        }
-
-    /**
-     * Zeroes the heading of the robot
-     */
-    fun zeroHeading() {
-        poseEstimator.resetPosition(
-            Rotation2d.fromDegrees(gyro.yaw),
-            modules.map { it.swerveModulePosition }
-                .toTypedArray(),
-            Pose2d(
-                poseEstimator.estimatedPosition.translation,
-                Rotation2d()
-            )
-        )
-    }
-
     inline val canTrustPose: Boolean
         get() = cameraWrappers.any { it.canTrustPose } || Game.sim
 
diff --git a/src/main/kotlin/frc/robot/subsystems/Intake.kt b/src/main/kotlin/frc/robot/subsystems/Intake.kt
index 264b9f14..7a30f07e 100644
--- a/src/main/kotlin/frc/robot/subsystems/Intake.kt
+++ b/src/main/kotlin/frc/robot/subsystems/Intake.kt
@@ -7,6 +7,7 @@ import com.revrobotics.SparkMaxAbsoluteEncoder.Type.kDutyCycle
 import edu.wpi.first.math.controller.ProfiledPIDController
 import edu.wpi.first.math.trajectory.TrapezoidProfile
 import edu.wpi.first.math.util.Units.rotationsToRadians
+import edu.wpi.first.networktables.DoubleEntry
 import edu.wpi.first.wpilibj.DigitalInput
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
@@ -135,9 +136,15 @@ class Intake(
     val deployPosition: Double
         get() = rotationsToRadians(deployEncoder.position)
 
+    val deployVelocity: Double
+        get() = rotationsToRadians(deployEncoder.velocity)
+
     val modePosition: Double
         get() = rotationsToRadians(modeEncoder.position)
 
+    val modeVelocity: Double
+        get() = rotationsToRadians(modeEncoder.velocity)
+
     var modeVoltage = 0.0 // voltage while zeroing
 
     val modeCurrent
@@ -165,9 +172,11 @@ class Intake(
 
     fun setModeAngle(angle: Double) {
         modePositionSetpoint = angle
+        modePID.reset(modePosition, modeVelocity)
     }
 
     fun setDeployAngle(angle: Double) {
         deployPositionSetpoint = angle
+        deployPID.reset(deployPosition, deployVelocity)
     }
 }
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/subsystems/SwerveModule.kt b/src/main/kotlin/frc/robot/subsystems/SwerveModule.kt
index 948c8b61..52774b01 100644
--- a/src/main/kotlin/frc/robot/subsystems/SwerveModule.kt
+++ b/src/main/kotlin/frc/robot/subsystems/SwerveModule.kt
@@ -1,6 +1,9 @@
 package frc.robot.subsystems
 
-import com.ctre.phoenix.motorcontrol.*
+import com.ctre.phoenix.motorcontrol.NeutralMode
+import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration
+import com.ctre.phoenix.motorcontrol.StatusFrame
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX
 import com.ctre.phoenix.sensors.AbsoluteSensorRange
 import com.ctre.phoenix.sensors.CANCoder
@@ -222,6 +225,8 @@ class SwerveModule(
         move()
         SmartDashboard.putNumber("$mname Encoder", turnEncoder.absolutePosition)
 
+        this.stateEntry.setDouble(setpoint.speedMetersPerSecond)
+
         // simulate motor velocity based on motor percent output
 //        val dt = Timer.getFPGATimestamp() - lastPeriodicTime
 //        val vel = (driveMotor.selectedSensorVelocity + (driveMotor.motorOutputVoltage * 2048.0 / 12.0) * 0.02).toInt()