diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 0f4ec98..fc42833 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/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 = 153; - public static final String GIT_SHA = "c4b8e2972e7be654bd544019760a174afc4871ae"; - public static final String GIT_DATE = "2026-03-12 00:08:36 MDT"; + public static final int GIT_REVISION = 155; + public static final String GIT_SHA = "af3646ff4378382f3bfcce6ab7828823bb76be21"; + public static final String GIT_DATE = "2026-03-12 18:06:12 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-12 18:03:03 MDT"; - public static final long BUILD_UNIX_TIME = 1773360183045L; + public static final String BUILD_DATE = "2026-03-12 18:11:48 MDT"; + public static final long BUILD_UNIX_TIME = 1773360708923L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index a79794c..825569e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -10,21 +10,12 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; -/** - * Minimal swerve simulator implementation for SIM mode. - * - Accepts SwerveRequest controls (tries to extract ChassisSpeeds via reflection) - * - Integrates chassis speeds into a Pose2d each updateInputs call - * - Provides reset/tare/vision correction hooks - * - * Lightweight: does not simulate voltages or module dynamics. Suitable to make - * the robot move in the simulator and provide pose/velocity feedback. - */ public class SimpleSwerveSim implements SwerveIO { private Pose2d pose = new Pose2d(); private Pose2d lastPose = pose; - private double vx = 0.0; // m/s (robot-relative) - private double vy = 0.0; // m/s (robot-relative) - private double omega = 0.0; // rad/s (robot-relative) + private double vx = 0.0; + private double vy = 0.0; + private double omega = 0.0; private long lastTimeNs = System.nanoTime(); @@ -35,7 +26,6 @@ public class SimpleSwerveSim implements SwerveIO { public synchronized void setControl(SwerveRequest ctrl) { if (ctrl == null) return; - // Try to extract a ChassisSpeeds field first (common approach) ChassisSpeeds cs = tryGetSpeedsField(ctrl); if (cs != null) { vx = cs.vxMetersPerSecond; @@ -44,7 +34,6 @@ public class SimpleSwerveSim implements SwerveIO { return; } - // Fallbacks: try to pull numeric fields by common names try { Class cls = ctrl.getClass(); double vxF = tryGetDoubleField(ctrl, cls, "VelocityX", "velocityX", "velocityx", "VelX"); @@ -54,7 +43,6 @@ public class SimpleSwerveSim implements SwerveIO { vy = vyF; omega = rotF; } catch (Exception e) { - // Silently ignore reflection failures and keep previous speeds. } } @@ -67,11 +55,8 @@ public class SimpleSwerveSim implements SwerveIO { return (ChassisSpeeds) o; } } catch (NoSuchFieldException nsf) { - // ignore } catch (IllegalAccessException iae) { - // ignore } catch (SecurityException se) { - // ignore } return null; } @@ -86,11 +71,8 @@ public class SimpleSwerveSim implements SwerveIO { return ((Number) val).doubleValue(); } } catch (NoSuchFieldException nsf) { - // try next name } catch (IllegalAccessException iae) { - // try next name } catch (Throwable t) { - // ignore other reflection issues } } return 0.0; @@ -104,35 +86,22 @@ public class SimpleSwerveSim implements SwerveIO { double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9); lastTimeNs = now; - // Save previous pose lastPose = pose; - // Compute robot-relative motion over dt - double dxRobot = vx * dt; - double dyRobot = vy * dt; + double dxField = vx * dt; + double dyField = vy * dt; double dTheta = omega * dt; - // Transform robot-relative delta into field frame using current rotation - Rotation2d r = pose.getRotation(); - double cos = r.getCos(); - double sin = r.getSin(); - - double dxField = dxRobot * cos - dyRobot * sin; - double dyField = dxRobot * sin + dyRobot * cos; - Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField)); - Rotation2d newRot = r.plus(Rotation2d.fromRadians(dTheta)); + Rotation2d newRot = pose.getRotation().plus(Rotation2d.fromRadians(dTheta)); pose = new Pose2d(newTrans, newRot); - // Populate provided SwerveState for callers state.lastPose = lastPose; state.currentPose = pose; state.speeds = new ChassisSpeeds(vx, vy, omega); state.odometryRate = dt; } - - @Override public synchronized void resetPose(Pose2d p) { if (p == null) return; @@ -161,8 +130,17 @@ public class SimpleSwerveSim implements SwerveIO { } } + public synchronized void pointAt(Translation2d target) { + if (target == null) return; + Translation2d toTarget = target.minus(pose.getTranslation()); + if (toTarget.getNorm() < 1e-9) return; + Rotation2d desired = toTarget.getAngle(); + pose = new Pose2d(pose.getTranslation(), desired); + lastPose = pose; + omega = 0.0; + } + @Override public synchronized void setLimits(double limitInAmps) { - // No-op for this simple sim } -} \ No newline at end of file +}