Finish simulation point to hub

src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java

@@ -4,6 +4,9 @@
 
 package frc4388.robot.commands.Swerve;
 
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.AutoLogOutput;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.commands.PID;
@@ -11,6 +14,7 @@ import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 public class StayInPosition extends PID {
     SwerveDrive drive;
+    Translation2d driveTranslation = new Translation2d();
 
     public StayInPosition(SwerveDrive drive) {
         super(0.3, 0.0, 0.0, 0.0, 1);
@@ -20,24 +24,23 @@ public class StayInPosition extends PID {
 
     public void goToTargetPose(Pose2d targetPose) {
         Pose2d currentPose = drive.getCurrentPose();
-
         double translationX = targetPose.getX() - currentPose.getX();
         double translationY = targetPose.getY() - currentPose.getY();
-
-        double magnitude = Math.sqrt(translationX * translationX + translationY * translationY);
-        if (magnitude > 1.0) {
-            translationX /= magnitude;
-            translationY /= magnitude;
+        if (translationX > 0.8){
+          translationX = 0.8;
         }
-
-        Translation2d driveTranslation;
-        if (magnitude < 0.05) {
+        if (translationY > 0.8){
+          translationY = 0.8;
+        }
+        if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) {
             driveTranslation = new Translation2d();
         } else {
             driveTranslation = new Translation2d(translationX, translationY);
         }
 
         drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
+        // If above doesn't work
+        //drive.driveFieldAngle(driveTranslation, targetPose.getRotation());
     }
 
     @Override

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 159;
-  public static final String GIT_SHA = "ebab028818fc0362daa4d7eb1010131819edd1cb";
-  public static final String GIT_DATE = "2026-03-14 18:29:15 MDT";
+  public static final int GIT_REVISION = 160;
+  public static final String GIT_SHA = "8c8ac261397954f1b5a118cb96d4e5f60d70da81";
+  public static final String GIT_DATE = "2026-03-14 18:57:04 MDT";
   public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-14 18:46:53 MDT";
-  public static final long BUILD_UNIX_TIME = 1773535613285L;
+  public static final String BUILD_DATE = "2026-03-14 19:59:14 MDT";
+  public static final long BUILD_UNIX_TIME = 1773539954157L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java

@@ -24,27 +24,59 @@ public class SimpleSwerveSim implements SwerveIO {
     public SimpleSwerveSim() {
     }
 
-    @Override
     public synchronized void setControl(SwerveRequest ctrl) {
         if (ctrl == null) return;
 
+        // Handle FieldCentricFacingAngle — compute omega from target direction
+        if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) {
+        vx = facingAngle.VelocityX;
+        vy = facingAngle.VelocityY;
+
+        // Simple P controller to rotate toward target
+        double currentAngle = pose.getRotation().getRadians();
+        double targetAngle  = facingAngle.TargetDirection.getRadians();
+        double error = targetAngle - currentAngle;
+
+            // Wrap error to [-pi, pi]
+            error = Math.atan2(Math.sin(error), Math.cos(error));
+
+            double kP = 5.0; // tune this — matches PathPlanner's rotation PID
+            omega = error * kP;
+            return;
+        }
+
+        // Handle FieldCentric (normal driving with explicit rotational rate)
+        if (ctrl instanceof SwerveRequest.FieldCentric fc) {
+            vx = fc.VelocityX;
+            vy = fc.VelocityY;
+            omega = fc.RotationalRate;
+            return;
+        }
+
+        if (ctrl instanceof SwerveRequest.RobotCentric rc) {
+        // rotate velocity into field frame
+            double cos = pose.getRotation().getCos();
+            double sin = pose.getRotation().getSin();
+            double vxRobot = rc.VelocityX;
+            double vyRobot = rc.VelocityY;
+            vx = vxRobot * cos - vyRobot * sin;
+            vy = vxRobot * sin + vyRobot * cos;
+            omega = rc.RotationalRate;
+            return;
+        }
+
+        // Handle brake
+        if (ctrl instanceof SwerveRequest.SwerveDriveBrake) {
+            vx = 0; vy = 0; omega = 0;
+            return;
+        }
+
+        // Fallback: your original reflection approach
         ChassisSpeeds cs = tryGetSpeedsField(ctrl);
         if (cs != null) {
             vx = cs.vxMetersPerSecond;
             vy = cs.vyMetersPerSecond;
             omega = cs.omegaRadiansPerSecond;
-            return;
-        }
-
-        try {
-            Class<?> cls = ctrl.getClass();
-            double vxF = tryGetDoubleField(ctrl, cls, "VelocityX", "velocityX", "velocityx", "VelX");
-            double vyF = tryGetDoubleField(ctrl, cls, "VelocityY", "velocityY", "velocityy", "VelY");
-            double rotF = tryGetDoubleField(ctrl, cls, "RotationalRate", "rotationalRate", "rotationalrate", "omega", "Omega");
-            vx = vxF;
-            vy = vyF;
-            omega = rotF;
-        } catch (Exception e) {
         }
     }
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -297,11 +297,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) {
         
-        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
-        leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
-
-        rotTarget = heading.getDegrees();
-
+       rotTarget = heading.getDegrees();
         var ctrl = new SwerveRequest.FieldCentricFacingAngle()
             .withVelocityX(leftStick.getX() * speedAdjust)
             .withVelocityY(leftStick.getY() * speedAdjust)
@@ -310,9 +306,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             SwerveDriveConstants.PIDConstants.AIM_kP.get(),
             SwerveDriveConstants.PIDConstants.AIM_kI.get(),
             SwerveDriveConstants.PIDConstants.AIM_kD.get()
-            // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP,
-            // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI,
-            // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD
         );
         io.setControl(ctrl);
     }