From d8cd3d28cd1e9655e424867ea5c42f6f99ba1a06 Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Thu, 30 Mar 2023 22:13:13 -0400
Subject: [PATCH 01/13] small cleanup

---
 src/main/kotlin/frc/robot/constants/Constants.kt      | 2 +-
 src/main/kotlin/frc/robot/constants/Field2dLayout.kt  | 1 -
 src/main/kotlin/frc/robot/constants/FieldConstants.kt | 3 +--
 src/main/kotlin/frc/robot/constants/intake.kt         | 4 ++--
 src/main/kotlin/frc/robot/subsystems/Drivetrain.kt    | 4 ----
 src/main/kotlin/frc/robot/subsystems/SwerveModule.kt  | 7 ++++++-
 6 files changed, 10 insertions(+), 11 deletions(-)

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/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/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
index bfaf5dfe..096c27fc 100644
--- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
+++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
@@ -311,10 +311,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
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()

From 28c6545406a6572422e4003b3ed3fb735c098873 Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Thu, 30 Mar 2023 22:21:41 -0400
Subject: [PATCH 02/13] tried to fix nearest 180 code

---
 .../commands/drivetrain/DriverCommand.kt      |  3 ++-
 .../robot/commands/drivetrain/Nearest180.kt   | 26 +++++++++++--------
 2 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
index b2dc372f..3863aa5a 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
@@ -4,6 +4,7 @@ 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
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.CommandBase
 import frc.kyberlib.command.Game
@@ -44,7 +45,7 @@ class DriverCommand(
                         * controlScheme.speedMutiplier,
                 if(nearestStation()) {
                     rotationPid.calculate(drivetrain.estimatedPose2d.rotation.radians,
-                         Nearest180.findNearest180(drivetrain)
+                        Units.degreesToRadians(findNearest180(drivetrain.estimatedPose2d.rotation.degrees))
                     ) // desired radians
                 }
                 else{
diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
index 9a6535d7..d1c3d428 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
@@ -1,14 +1,18 @@
 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 > 180) {
+        nearest180 - 360
+    } else if (nearest180 < -180) {
+        nearest180 + 360
+    } else {
+        nearest180
     }
-}
\ No newline at end of file
+}

From 09d33d2c2c919fdf7ec9a8abf60ee4eea388d902 Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Thu, 30 Mar 2023 22:23:00 -0400
Subject: [PATCH 03/13] tried to fix nearest 180 code

---
 .../kotlin/frc/robot/commands/drivetrain/DriverCommand.kt     | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
index 3863aa5a..e28c45c0 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
@@ -4,7 +4,7 @@ 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
+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
@@ -45,7 +45,7 @@ class DriverCommand(
                         * controlScheme.speedMutiplier,
                 if(nearestStation()) {
                     rotationPid.calculate(drivetrain.estimatedPose2d.rotation.radians,
-                        Units.degreesToRadians(findNearest180(drivetrain.estimatedPose2d.rotation.degrees))
+                        degreesToRadians(findNearest180(drivetrain.estimatedPose2d.rotation.degrees))
                     ) // desired radians
                 }
                 else{

From c638564f78a666c0c38742d87ba1e9e9a3114e36 Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Thu, 30 Mar 2023 22:38:13 -0400
Subject: [PATCH 04/13] some small tweaks

---
 src/main/kotlin/frc/robot/RobotContainer.kt    | 18 ++++++++++++------
 .../kotlin/frc/robot/constants/drivetrain.kt   |  8 ++++----
 .../frc/robot/controls/ChrisControlScheme.kt   |  4 ++--
 .../kotlin/frc/robot/controls/ControlScheme.kt |  6 +-----
 4 files changed, 19 insertions(+), 17 deletions(-)

diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt
index 7572b4f1..4cc820f6 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
@@ -314,8 +316,8 @@ class RobotContainer {
                 shootToLThree
                     .whileTrue(ShootToLTwo(intake, this@RobotContainer))
 
-//                snapTo180
-//                    .whileTrue(RotateTo180(this@RobotContainer))
+                snapTo180
+                    .whileTrue(RotateTo180(this@RobotContainer))
             }
         }
     }
@@ -472,7 +474,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 +543,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/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/controls/ChrisControlScheme.kt b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
index 83535ba9..f7f2cbd3 100644
--- a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
+++ b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
@@ -31,5 +31,5 @@ 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.rightTrigger()
+}
diff --git a/src/main/kotlin/frc/robot/controls/ControlScheme.kt b/src/main/kotlin/frc/robot/controls/ControlScheme.kt
index d8d21e70..e4b00d1c 100644
--- a/src/main/kotlin/frc/robot/controls/ControlScheme.kt
+++ b/src/main/kotlin/frc/robot/controls/ControlScheme.kt
@@ -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,7 +56,7 @@ 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 }

From 0d5905232ba7896f4fea4e312974aee684df96fd Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Thu, 30 Mar 2023 23:25:51 -0400
Subject: [PATCH 05/13] the desaturate wheel speeds in drivetrain.drive() was
 wildly wrong and was not implemented correctly. It was being passed the
 intended chassis speeds instead of the current chassis speeds and had
 illogical values for max translational and rotational velocity.

---
 .../kotlin/frc/robot/subsystems/Drivetrain.kt     | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
index 096c27fc..43ae6535 100644
--- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
+++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
@@ -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,
@@ -295,12 +295,15 @@ class Drivetrain(
             chassisSpeedsField,
             rotAxis
         )
+
+        val currentChassisSpeeds = kinematics.toChassisSpeeds(*swerveModuleStates)
+
         SwerveDriveKinematics.desaturateWheelSpeeds(
-            swerveModuleStates,
-            chassisSpeedsField,
-            4.0,
-            2.0,
-            2 * PI
+            /* moduleStates = */ swerveModuleStates,
+            /* currentChassisSpeed = */ currentChassisSpeeds,
+            /* attainableMaxModuleSpeedMetersPerSecond = */ 4.0,
+            /* attainableMaxTranslationalSpeedMetersPerSecond = */ 4.0,
+            /* attainableMaxRotationalVelocityRadiansPerSecond = */ PI,
         )
 
         if (Game.real) {

From 2b74ceb67b87f478986227d05614bbae2d27d68b Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Thu, 30 Mar 2023 23:55:43 -0400
Subject: [PATCH 06/13] changed the control scheme and eject command so that
 when you press button it deploys intake, and when you let go it throws the
 object

---
 .../kotlin/frc/robot/subsystems/Drivetrain.kt | 76 +++----------------
 1 file changed, 12 insertions(+), 64 deletions(-)

diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
index 43ae6535..a8b5d280 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
@@ -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()
     )
 
@@ -167,17 +167,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 +227,9 @@ 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)
-//                )
             simEstimatedPose2d = simEstimatedPose2d + (vel * 0.02 * 0.01) + simQueuedForce
             simQueuedForce = Transform2d()
         }
@@ -355,39 +336,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
 

From e526f9a087ff8ceac25745694f921fc06e93b4f3 Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Thu, 30 Mar 2023 23:55:43 -0400
Subject: [PATCH 07/13] cleaned up the drivetrain subsystem

---
 .../kotlin/frc/robot/subsystems/Drivetrain.kt | 76 +++----------------
 1 file changed, 12 insertions(+), 64 deletions(-)

diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
index 43ae6535..a8b5d280 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
@@ -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()
     )
 
@@ -167,17 +167,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 +227,9 @@ 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)
-//                )
             simEstimatedPose2d = simEstimatedPose2d + (vel * 0.02 * 0.01) + simQueuedForce
             simQueuedForce = Transform2d()
         }
@@ -355,39 +336,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
 

From c8292aeb849fd8d5937adc57f24bd54edebca3af Mon Sep 17 00:00:00 2001
From: KP <86213869+Kanishk-Pandey@users.noreply.github.com>
Date: Fri, 31 Mar 2023 08:15:37 -0400
Subject: [PATCH 08/13] removed redundant stuff

---
 .../robot/commands/pathing/NoVisionAuto.kt    | 43 -------------------
 1 file changed, 43 deletions(-)

diff --git a/src/main/kotlin/frc/robot/commands/pathing/NoVisionAuto.kt b/src/main/kotlin/frc/robot/commands/pathing/NoVisionAuto.kt
index aea9c14a..25db8254 100644
--- a/src/main/kotlin/frc/robot/commands/pathing/NoVisionAuto.kt
+++ b/src/main/kotlin/frc/robot/commands/pathing/NoVisionAuto.kt
@@ -41,47 +41,4 @@ class NoVisionAuto(
             )
             .withTimeout(8.0)
     }
-
-
-    fun pathBlue(robotContainer: RobotContainer): Command {
-        return SetSubsystemPosition(
-            robotContainer,
-            { IOLevel.High },
-            { GamePiece.cone },
-            true
-        )
-            .andThen(
-                Throw(
-                    robotContainer.manipulator,
-                    { GamePiece.cone },
-                    { PlacementLevel.Level3 }
-                )
-                    .withTimeout(1.5))
-            .andThen(
-                DriveCommand(
-                    robotContainer.drivetrain,
-                    { 1.0 },
-                    { 0.0 },
-                    { 0.0 },
-                    false,
-                    { Translation2d(0.0, 0.0) }
-                )
-                    .withTimeout(4.0)
-                    .alongWith(
-                        SetSubsystemPosition(
-                            robotContainer,
-                            { IOLevel.Idle },
-                            { GamePiece.cone },
-                            true
-                        )
-                            .andThen(
-                                ZeroElevatorAndIdle(
-                                    robotContainer.elevator,
-                                    robotContainer.arm
-                                )
-                            )
-                    )
-                    .alongWith(IdleIntake(robotContainer.intake, { GamePiece.cube }))
-            )
-    }
 }
\ No newline at end of file

From ed6b3a3195fee0cad6dea11ef63cc255c7e80d22 Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Sun, 2 Apr 2023 11:31:21 -0400
Subject: [PATCH 09/13] I fixed snap to rotation!

---
 .../robot/commands/drivetrain/DriverCommand.kt    | 15 ++++++++++++++-
 .../frc/robot/commands/drivetrain/Nearest180.kt   |  9 ++++++---
 .../frc/robot/commands/drivetrain/RotateTo180.kt  |  3 ++-
 .../frc/robot/controls/ChrisControlScheme.kt      |  2 +-
 4 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
index e28c45c0..1b77444c 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
@@ -8,7 +8,10 @@ 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.maxAutonomousAngularAcceleration
+import frc.robot.constants.drivetrain.maxAutonomousAngularVelocity
 import frc.robot.controls.ControlScheme
 import frc.robot.subsystems.Drivetrain
 import frc.robot.subsystems.slewLimited
@@ -20,15 +23,25 @@ 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
diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
index d1c3d428..757b6a44 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
@@ -7,12 +7,15 @@ fun findNearest180(
     // 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 > 180) {
+    var nearest180 = currentAngle % 360
+    println("nearest180 = $nearest180")
+    nearest180 = if (nearest180 > 180) {
         nearest180 - 360
     } else if (nearest180 < -180) {
         nearest180 + 360
     } else {
         nearest180
     }
-}
+    return if (nearest180 < 90 && nearest180 > -90) 0.0
+    else 180.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/controls/ChrisControlScheme.kt b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
index f7f2cbd3..3bbe3638 100644
--- a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
+++ b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
@@ -31,5 +31,5 @@ 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()
+    override val snapTo180: Trigger = xbox.rightTrigger()
 }

From e2df04f87c9e1ab310178353e878ac00213054b3 Mon Sep 17 00:00:00 2001
From: a1cd <71281043+a1cd@users.noreply.github.com>
Date: Sun, 2 Apr 2023 11:34:31 -0400
Subject: [PATCH 10/13] changed rotation pose estimator standard deviations

---
 src/main/kotlin/frc/robot/subsystems/Drivetrain.kt | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
index a8b5d280..3dc07b45 100644
--- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
+++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
@@ -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
@@ -230,6 +229,7 @@ class Drivetrain(
 
         if (Game.sim) {
             val vel = estimatedVelocity
+            @Suppress("DEPRECATION")
             simEstimatedPose2d = simEstimatedPose2d + (vel * 0.02 * 0.01) + simQueuedForce
             simQueuedForce = Transform2d()
         }

From b2310baf718ca6f0cd6476a65aeb03a80f7fe877 Mon Sep 17 00:00:00 2001
From: Kanishk-Pandey <kanishkpandey0407@gmail.com>
Date: Sun, 2 Apr 2023 11:48:43 -0400
Subject: [PATCH 11/13] competition changes

Signed-off-by: Kanishk-Pandey <kanishkpandey0407@gmail.com>
---
 src/main/kotlin/frc/robot/Robot.kt            |  2 +-
 src/main/kotlin/frc/robot/RobotContainer.kt   | 20 ++++++++--
 .../commands/alltogether/PlacementValues.kt   |  8 ++--
 .../alltogether/SetSubsystemPosition.kt       |  2 +
 .../frc/robot/commands/balance/AutoBalance.kt |  2 +-
 .../robot/commands/drivetrain/DriveCommand.kt |  4 +-
 .../commands/drivetrain/DriverCommand.kt      |  8 ++--
 .../robot/commands/drivetrain/Nearest180.kt   |  8 ++--
 .../robot/commands/intake/ManualZeroIntake.kt | 33 +++++++++++++++
 .../robot/commands/intake/ZeroModeMotor.kt    | 15 +++----
 .../commands/pathing/AutoPlaceAndBalance.kt   | 40 ++++++++++++-------
 .../robot/commands/pathing/MoveToPosition.kt  |  9 ++++-
 .../frc/robot/controls/ChrisControlScheme.kt  |  9 +++--
 .../frc/robot/controls/ControlScheme.kt       |  4 +-
 .../kotlin/frc/robot/subsystems/Drivetrain.kt |  4 ++
 15 files changed, 122 insertions(+), 46 deletions(-)
 create mode 100644 src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt

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 4cc820f6..a7ae6f54 100644
--- a/src/main/kotlin/frc/robot/RobotContainer.kt
+++ b/src/main/kotlin/frc/robot/RobotContainer.kt
@@ -12,6 +12,8 @@ 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.PowerDistribution
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets
 import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab
@@ -32,10 +34,7 @@ import frc.robot.commands.alltogether.SetSubsystemPosition
 import frc.robot.commands.drivetrain.DriveCommand
 import frc.robot.commands.drivetrain.RotateTo180
 import frc.robot.commands.elevator.ZeroElevatorAndIdle
-import frc.robot.commands.intake.DeployIntake
-import frc.robot.commands.intake.IdleIntake
-import frc.robot.commands.intake.IntakeEject
-import frc.robot.commands.intake.ShootToLTwo
+import frc.robot.commands.intake.*
 import frc.robot.commands.manipulator.ManipulatorIO
 import frc.robot.commands.manipulator.SetManipulatorSpeed
 import frc.robot.commands.manipulator.Throw
@@ -76,6 +75,7 @@ class RobotContainer {
     val intake = Intake(arm, { wantedObject })
     val elevator = Elevator(this@RobotContainer, arm, this.intake)
 
+    val pdh = PowerDistribution(1, PowerDistribution.ModuleType.kRev)
 
     init {
         arrayOf(controlScheme0, controlScheme1).forEachIndexed { i, it ->
@@ -318,6 +318,12 @@ class RobotContainer {
 
                 snapTo180
                     .whileTrue(RotateTo180(this@RobotContainer))
+
+                zeroIntake
+                    .whileTrue(ZeroModeMotor(this@RobotContainer))
+
+//                balanceIOLevel
+//                    .whileTrue(SetSubsystemPosition(this@RobotContainer, { IOLevel.Balance }, { GamePiece.cone }, true))
             }
         }
     }
@@ -545,6 +551,10 @@ class RobotContainer {
     // shuffleboard auto chooser
     private val autoChooserTab: ShuffleboardTab = Shuffleboard.getTab("Autonomous")
 
+    val cameraTurnOffChooser = autoChooserTab.add("PowerToggle", true)
+        .withWidget(BuiltInWidgets.kToggleButton)
+        .entry
+
     @Suppress("unused")
     val autoChooserWidget: ComplexWidget = autoChooserTab.add("Autonomous", autoChooser)
     val armFieldPosition: FieldObject2d = drivetrain.field2d.getObject("arm")
@@ -564,6 +574,8 @@ class RobotContainer {
         leds.update()
         smartDashboardSelector.update()
 
+        pdh.switchableChannel = cameraTurnOffChooser.getBoolean(true)
+
         // send subsystems to SmartDashboard
         SmartDashboard.putData("Drivetrain/sendable", drivetrain)
         SmartDashboard.putData("elevator/sendable", elevator)
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/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 e28c45c0..88d0e8ec 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt
@@ -8,7 +8,9 @@ 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.controls.ControlScheme
 import frc.robot.subsystems.Drivetrain
 import frc.robot.subsystems.slewLimited
@@ -20,7 +22,7 @@ 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(2*PI, 4*PI)).apply {
         enableContinuousInput(-PI, PI)
     }
 
@@ -34,7 +36,7 @@ class DriverCommand(
             else -> Game.alliance.xMul
         }
         val vec = Translation2d(-controlScheme.forward, -controlScheme.strafe)
-            .times(3.5)
+            .times(maxVelocity)
         drivetrain.drive(
             ChassisSpeeds(
                 vec.x * Constants.powerPercent *
@@ -52,7 +54,7 @@ class DriverCommand(
                     -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 d1c3d428..9477e42a 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
@@ -8,11 +8,9 @@ fun findNearest180(
     // 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 > 180) {
-        nearest180 - 360
-    } else if (nearest180 < -180) {
-        nearest180 + 360
+    return if (nearest180 >= 90 || nearest180<=-90) {
+        180.0
     } else {
-        nearest180
+        0.0
     }
 }
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/controls/ChrisControlScheme.kt b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt
index f7f2cbd3..efcbc6d8 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()
-}
+
+    //override val snapTo180: Trigger = xbox.rightTrigger()
+    override val zeroIntake: Trigger = xbox.povUp()
+//    override val balanceIOLevel: Trigger = xbox.povDown()
+}
\ No newline at end of file
diff --git a/src/main/kotlin/frc/robot/controls/ControlScheme.kt b/src/main/kotlin/frc/robot/controls/ControlScheme.kt
index e4b00d1c..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
@@ -62,5 +62,7 @@ abstract class ControlScheme {
     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 a8b5d280..4c410d26 100644
--- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
+++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt
@@ -282,6 +282,10 @@ class Drivetrain(
         SwerveDriveKinematics.desaturateWheelSpeeds(
             /* moduleStates = */ swerveModuleStates,
             /* currentChassisSpeed = */ currentChassisSpeeds,
+            /*
+            swerveModuleStates,
+            chassisSpeedsField,
+             */
             /* attainableMaxModuleSpeedMetersPerSecond = */ 4.0,
             /* attainableMaxTranslationalSpeedMetersPerSecond = */ 4.0,
             /* attainableMaxRotationalVelocityRadiansPerSecond = */ PI,

From 1d72e91088c1a7fadb8a79ed555ab44ca1f442b7 Mon Sep 17 00:00:00 2001
From: Kanishk-Pandey <kanishkpandey0407@gmail.com>
Date: Sun, 2 Apr 2023 22:29:34 -0400
Subject: [PATCH 12/13] Thats it folks!

Signed-off-by: Kanishk-Pandey <kanishkpandey0407@gmail.com>
---
 .glass/glass.json | 32 ++++++++++++++++++++++++--------
 1 file changed, 24 insertions(+), 8 deletions(-)

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
+      }
     }
   }
 }

From 73220da2828aaee1b647f7f288d513595340133c Mon Sep 17 00:00:00 2001
From: KP <86213869+Kanishk-Pandey@users.noreply.github.com>
Date: Thu, 11 Apr 2024 10:32:29 -0400
Subject: [PATCH 13/13] its been a while but these are changes that I never
 added

---
 src/main/kotlin/frc/robot/RobotContainer.kt   |  5 ++++-
 .../commands/drivetrain/BackupSnapTo180.kt    | 22 +++++++++++++++++++
 .../robot/commands/drivetrain/Nearest180.kt   |  8 +++----
 .../kotlin/frc/robot/subsystems/Intake.kt     |  9 ++++++++
 4 files changed, 38 insertions(+), 6 deletions(-)
 create mode 100644 src/main/kotlin/frc/robot/commands/drivetrain/BackupSnapTo180.kt

diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt
index 4cc820f6..c83b50b3 100644
--- a/src/main/kotlin/frc/robot/RobotContainer.kt
+++ b/src/main/kotlin/frc/robot/RobotContainer.kt
@@ -43,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
@@ -317,7 +318,9 @@ class RobotContainer {
                     .whileTrue(ShootToLTwo(intake, this@RobotContainer))
 
                 snapTo180
-                    .whileTrue(RotateTo180(this@RobotContainer))
+                    .whileTrue(
+                        MoveToPosition(drivetrain, drivetrain.estimatedPose2d, 180.0)
+                    )
             }
         }
     }
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/Nearest180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
index d1c3d428..5b09cfd3 100644
--- a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
+++ b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt
@@ -8,11 +8,9 @@ fun findNearest180(
     // 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 > 180) {
-        nearest180 - 360
-    } else if (nearest180 < -180) {
-        nearest180 + 360
+    return if (nearest180 >=90 || nearest180 <= -90) {
+        180.0
     } else {
-        nearest180
+        0.0
     }
 }
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