From c4b8e2972e7be654bd544019760a174afc4871ae Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 12 Mar 2026 00:08:36 -0600 Subject: [PATCH] simulation added --- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 12 +- .../robot/constants/BuildConstants.java | 10 +- .../subsystems/swerve/SimpleSwerveSim.java | 209 ++++++++++++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 10 + 5 files changed, 235 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 697bfb7..00ab2c5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,7 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* RobotMap */ - public final RobotMap m_robotMap = new RobotMap(Mode.REAL); + public final RobotMap m_robotMap = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM); /*Limit Switch */ // public final DigitalInput m_armLimitSwitch = new DigitalInput(9); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9791edb..b3dc909 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -25,6 +25,7 @@ import frc4388.robot.subsystems.intake.IntakeReal; import frc4388.robot.subsystems.shooter.ShooterConstants; import frc4388.robot.subsystems.shooter.ShooterIO; import frc4388.robot.subsystems.shooter.ShooterReal; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; // import frc4388.robot.subsystems.elevator.ElevatorIO; // import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; @@ -133,8 +134,15 @@ public class RobotMap { FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); break; - // case SIM: - // break; + case SIM: + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; + + swerveDrivetrain = new SimpleSwerveSim() {}; + + shooterIO = new ShooterIO() {}; + intakeIO = new IntakeIO() {}; + break; default: leftCamera = new VisionIO() {}; rightCamera = new VisionIO() {}; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index c801a29..1a1f2b3 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 = 149; - public static final String GIT_SHA = "50b8312c48be14a559876064cdddbcd27bcb8ef7"; - public static final String GIT_DATE = "2026-03-10 08:39:05 MDT"; + public static final int GIT_REVISION = 152; + public static final String GIT_SHA = "4f361a805deb8cb48e62bc0a6c3a7406d6aa5f72"; + public static final String GIT_DATE = "2026-03-11 22:49:27 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-10 21:05:03 MDT"; - public static final long BUILD_UNIX_TIME = 1773198303540L; + public static final String BUILD_DATE = "2026-03-12 00:02:41 MDT"; + public static final long BUILD_UNIX_TIME = 1773295361724L; 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 new file mode 100644 index 0000000..78fcd01 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -0,0 +1,209 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +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 long lastTimeNs = System.nanoTime(); + + public SimpleSwerveSim() { + } + + @Override + 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; + vy = cs.vyMetersPerSecond; + omega = cs.omegaRadiansPerSecond; + return; + } + + // Fallbacks: try to pull numeric fields by common names + 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) { + // Silently ignore reflection failures and keep previous speeds. + } + } + + private ChassisSpeeds tryGetSpeedsField(SwerveRequest ctrl) { + try { + java.lang.reflect.Field f = ctrl.getClass().getDeclaredField("Speeds"); + f.setAccessible(true); + Object o = f.get(ctrl); + if (o instanceof ChassisSpeeds) { + return (ChassisSpeeds) o; + } + } catch (NoSuchFieldException nsf) { + // ignore + } catch (IllegalAccessException iae) { + // ignore + } catch (SecurityException se) { + // ignore + } + return null; + } + + private double tryGetDoubleField(Object obj, Class cls, String... names) { + for (String n : names) { + try { + java.lang.reflect.Field f = cls.getDeclaredField(n); + f.setAccessible(true); + Object val = f.get(obj); + if (val instanceof Number) { + 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; + } + + @Override + public synchronized void updateInputs(SwerveState state) { + if (state == null) return; + + long now = System.nanoTime(); + 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 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)); + 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; + } + + + public synchronized void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + if (leftStick == null || fieldPos == null) return; + + // current robot speed (robot-relative) + Translation2d robotSpeed = new Translation2d(vx, vy); + + // lead the target by robot motion over aimLeadTime + Translation2d fieldPosLead = fieldPos.plus(robotSpeed.times(aimLeadTime)); + + // compute angle from robot to lead point (field frame) + Rotation2d toLead = fieldPosLead.minus(pose.getTranslation()).getAngle(); + // Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle(); + + // compute shortest angle error (ang_error = desired - current) normalized to [-pi,pi] + double currentYaw = pose.getRotation().getRadians(); + double desiredYaw = toLead.getRadians(); + double error = desiredYaw - currentYaw; + // normalize + while (error > Math.PI) error -= 2 * Math.PI; + while (error < -Math.PI) error += 2 * Math.PI; + + // simple P controller for rotation + final double KP_ROT = 2.0; // tune as needed + final double MAX_OMEGA = 6.0; // rad/s cap + double commandedOmega = KP_ROT * error; + if (commandedOmega > MAX_OMEGA) commandedOmega = MAX_OMEGA; + if (commandedOmega < -MAX_OMEGA) commandedOmega = -MAX_OMEGA; + this.omega = commandedOmega; + + // apply translational command from leftStick (assume stick in [-1,1]) + final double STICK_SPEED_MPS = 3.0; // 3 m/s at full stick; tune as needed + this.vx = leftStick.getX() * STICK_SPEED_MPS; + this.vy = leftStick.getY() * STICK_SPEED_MPS; + } + + @Override + public synchronized void resetPose(Pose2d p) { + if (p == null) return; + pose = p; + lastPose = p; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void tareEverything() { + pose = new Pose2d(); + lastPose = pose; + vx = 0.0; + vy = 0.0; + omega = 0.0; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void addVisionMeasurement(List poses) { + if (poses == null || poses.isEmpty()) return; + Pose2d visPose = poses.get(poses.size() - 1).pose(); + if (visPose != null) { + pose = visPose; + lastPose = visPose; + } + } + + @Override + public synchronized void setLimits(double limitInAmps) { + // No-op for this simple sim + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index a79bfa0..3ee51e6 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -518,6 +518,16 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } io.addVisionMeasurement(vision.getPosesToAdd()); + io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); + + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); + if (state.speeds != null) { + this.chassisSpeeds = state.speeds; + } else { + this.chassisSpeeds = new ChassisSpeeds(); + } } // if(e.isPresent())