diff --git a/src/main/kotlin/frc/team449/ControllerBindings.kt b/src/main/kotlin/frc/team449/ControllerBindings.kt
index 8741836..8c80c9c 100644
--- a/src/main/kotlin/frc/team449/ControllerBindings.kt
+++ b/src/main/kotlin/frc/team449/ControllerBindings.kt
@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.ConditionalCommand
 import edu.wpi.first.wpilibj2.command.InstantCommand
+import edu.wpi.first.wpilibj2.command.PrintCommand
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import frc.team449.commands.driveAlign.SimpleReefAlign
@@ -42,6 +43,13 @@ class ControllerBindings(
     premove_l4()
 
     stow()
+
+    mechanismController.pov(0).onTrue(
+      robot.wrist.setPosition(PI/2)
+    )
+    mechanismController.pov(180).onTrue(
+      robot.wrist.setPosition(0.0)
+    )
   }
 
   private fun nonRobotBindings() {
diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt
index 29133f5..238db9c 100644
--- a/src/main/kotlin/frc/team449/RobotLoop.kt
+++ b/src/main/kotlin/frc/team449/RobotLoop.kt
@@ -5,6 +5,10 @@ import dev.doglog.DogLog
 import dev.doglog.DogLogOptions
 import edu.wpi.first.hal.FRCNetComm
 import edu.wpi.first.hal.HAL
+import edu.wpi.first.math.geometry.Pose3d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Rotation3d
+import edu.wpi.first.math.geometry.Transform3d
 import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.*
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
@@ -25,6 +29,7 @@ import frc.team449.system.encoder.QuadCalibration
 import org.littletonrobotics.urcl.URCL
 import kotlin.jvm.optionals.getOrDefault
 import kotlin.jvm.optionals.getOrNull
+import kotlin.math.*
 
 /** The main class of the robot, constructs all the subsystems
  * and initializes default commands . */
@@ -38,6 +43,10 @@ class RobotLoop : TimedRobot() {
   private var routineMap = hashMapOf<String, Command>()
   private val controllerBinder = ControllerBindings(robot.driveController, robot.mechController, robot)
 
+  private var componentStorage: Array<Pose3d>? = null
+  private var wristMove: Pose3d? = null
+
+
   override fun robotInit() {
     // Yes this should be a print statement, it's useful to know that robotInit started.
     println("Started robotInit.")
@@ -161,9 +170,53 @@ class RobotLoop : TimedRobot() {
 
   override fun testPeriodic() {}
 
-  override fun simulationInit() {}
+  override fun simulationInit() {
+    // set position
+//    componentStorage = arrayOf(
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(0.61, 0.0, 0.327,
+//        Rotation3d(0.0, 0.0, 0.0)))
+//    componentStorage = arrayOf(Pose3d(),
+//      Pose3d(),
+//      Pose3d(),
+//      Pose3d(),
+//      Pose3d(0.0, 0.0, 0.0,
+//        Rotation3d(0.0, 0.0, 0.0)))
+    robot.poseSubsystem.enableVisionFusion = false
+  }
 
   override fun simulationPeriodic() {
+    // sim pose calculator
+
+//    componentStorage[4]
+    val pivotPos = -robot.pivot.positionSupplier.get()
+
+    componentStorage = arrayOf(
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+//      Pose3d(0.569, 0.0, 0.366,
+//        Rotation3d(0.0, -robot.wrist.positionSupplier.get(), 0.0))
+      Pose3d(sqrt(0.569.pow(2) + 0.366.pow(2))*cos(-pivotPos + atan(0.569)),
+        0.0,
+        sqrt(0.569.pow(2) + 0.366.pow(2))*sin(-pivotPos + atan(0.366)),
+        Rotation3d(0.0, -robot.wrist.positionSupplier.get(), 0.0))
+    )
+    //sqrt(x0^2+y0^2)*<cos, sin>(theta + atan(y0, x0))
+    DogLog.log("SimComponent", componentStorage)
+
     robot.drive as SwerveSim
 
     VisionConstants.ESTIMATORS.forEach {
diff --git a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt
index 3c4a3ac..fecb45d 100644
--- a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt
+++ b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt
@@ -157,12 +157,12 @@ class SwerveOrthogonalCommand(
       rotRamp.calculate(
         min(
           MathUtil.applyDeadband(
-            abs(controller.rightX).pow(SwerveConstants.ROT_FILTER_ORDER),
+            abs(controller.getRawAxis(2)).pow(SwerveConstants.ROT_FILTER_ORDER),
             RobotConstants.ROTATION_DEADBAND,
             1.0
           ),
           1.0
-        ) * -sign(controller.rightX) * drive.maxRotSpeed
+        ) * -sign(controller.getRawAxis(2)) * drive.maxRotSpeed
       )
     } else {
       if (checkSnapToAngleTolerance()) headingLock = false
diff --git a/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt
index 39cf4fa..b18f37f 100644
--- a/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt
+++ b/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt
@@ -38,7 +38,8 @@ class SuperstructureManager(
       )
   }
 
-  companion object {
+
+    companion object {
     fun createSuperstructureManager(robot: Robot): SuperstructureManager {
       return SuperstructureManager(
         robot.elevator,
diff --git a/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt b/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt
index e7287b5..a196e92 100644
--- a/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt
+++ b/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt
@@ -119,7 +119,6 @@ class ReefOnlyEstimator(
       ) {
         lowestAmbiguityScore = targetPoseAmbiguity
         lowestAmbiguityTarget = target
-        print("getting here")
       }
     }