From 02d033ab8685ad99e5f3c0d6b3a6b351f585bd2c Mon Sep 17 00:00:00 2001
From: Sidney Trzepacz <74916637+Sid220@users.noreply.github.com>
Date: Wed, 27 Mar 2024 18:37:44 -0400
Subject: [PATCH] OTF Amp working?

---
 src/main/java/frc/robot/commands/otf/OTFAmp.java | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/commands/otf/OTFAmp.java b/src/main/java/frc/robot/commands/otf/OTFAmp.java
index a4c7a070..13f01f0f 100644
--- a/src/main/java/frc/robot/commands/otf/OTFAmp.java
+++ b/src/main/java/frc/robot/commands/otf/OTFAmp.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -36,10 +37,8 @@ public static OTFAmp getInstance() {
 
   public Timer timer = new Timer();
   public double ttl = 3;
-  private Pose2d ampPose = new Pose2d(1.8, 7.67, Rotation2d.fromRadians(0));
-  private Transform2d preAmpPoseOffset =
-      new Transform2d(new Translation2d(0, -0.25), new Rotation2d());
-  private Pose2d preAmpPose = ampPose.plus(preAmpPoseOffset);
+  private Pose2d ampPose = new Pose2d(1.8, 7.67, Rotation2d.fromDegrees(90));
+  private Pose2d preAmpPose =  new Pose2d(1.8, 7.67 - Units.inchesToMeters(2), Rotation2d.fromDegrees(90));
 
   @Getter
   public ErrorTracker tracker =