From a9b9e4210f6e2e57bbfbbf39240383f60978c597 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 09:27:43 -0700 Subject: [PATCH 01/10] WIP: Simulation mostly working Had the drive (strafe) part working a minute ago, but now it's not working anymore. Still need to get the turn (spin) part working. --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/Robot.java | 46 +++++++- src/main/java/frc/robot/RobotContainer.java | 5 + .../frc/robot/subsystems/drive/Drive.java | 101 +++++++++++------- .../frc/robot/subsystems/drive/Module.java | 5 + .../frc/robot/subsystems/drive/ModuleIO.java | 3 + .../robot/subsystems/drive/ModuleIOSim.java | 66 +++++++----- .../java/frc/robot/subsystems/imu/ImuIO.java | 4 + .../frc/robot/subsystems/imu/ImuIOSim.java | 38 ++++--- .../frc/robot/subsystems/vision/Vision.java | 5 + 11 files changed, 196 insertions(+), 83 deletions(-) diff --git a/build.gradle b/build.gradle index b64a52d..4df38fa 100644 --- a/build.gradle +++ b/build.gradle @@ -64,7 +64,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9b81736..081e1e9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,7 +56,7 @@ public final class Constants { * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT, * etc.) and the operating modes of the code (REAL, SIM, or REPLAY) */ - private static RobotType robotType = RobotType.COMPBOT; + private static RobotType robotType = RobotType.SIMBOT; // Define swerve, auto, and vision types being used // NOTE: Only PHOENIX6 swerve base has been tested at this point!!! @@ -66,7 +66,7 @@ public final class Constants { private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.MANUAL; // MANUAL, PATHPLANNER, CHOREO - private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE + private static VisionType visionType = VisionType.PHOTON; // PHOTON, LIMELIGHT, NONE /** Enumerate the robot types (name your robots here) */ public static enum RobotType { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 13c74fa..6896baf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,6 +18,9 @@ package frc.robot; import com.revrobotics.util.StatusLogger; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.Timer; @@ -31,6 +34,10 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.littletonrobotics.urcl.URCL; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -43,6 +50,9 @@ public class Robot extends LoggedRobot { private RobotContainer m_robotContainer; private Timer m_disabledTimer; + // 1️⃣ Define simulation fields here + private VisionSystemSim visionSim; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -108,7 +118,9 @@ public Robot() { @Override public void robotPeriodic() { // Switch thread to high priority to improve loop timing - Threads.setCurrentThreadPriority(true, 99); + if (isReal()) { + Threads.setCurrentThreadPriority(true, 99); + } // Run all virtual subsystems each time through the loop VirtualSubsystem.periodicAll(); @@ -242,9 +254,37 @@ public void testPeriodic() {} /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() {} + public void simulationInit() { + visionSim = new VisionSystemSim("main"); + + // Load AprilTag field layout + AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Register AprilTags with vision simulation + visionSim.addAprilTags(fieldLayout); + + // Simulated Camera Properties + SimCameraProperties cameraProp = new SimCameraProperties(); + // A 1280 x 800 camera with a 100 degree diagonal FOV. + cameraProp.setCalibration(1280, 800, Rotation2d.fromDegrees(100)); + // Approximate detection noise with average and standard deviation error in pixels. + cameraProp.setCalibError(0.25, 0.08); + // Set the camera image capture framerate (Note: this is limited by robot loop rate). + cameraProp.setFPS(20); + // The average and standard deviation in milliseconds of image data latency. + cameraProp.setAvgLatencyMs(35); + cameraProp.setLatencyStdDevMs(5); + + // Define Cameras and add to simulation + PhotonCamera camera0 = new PhotonCamera("frontCam"); + PhotonCamera camera1 = new PhotonCamera("backCam"); + visionSim.addCamera(new PhotonCameraSim(camera0, cameraProp), Constants.Cameras.robotToCamera0); + visionSim.addCamera(new PhotonCameraSim(camera1, cameraProp), Constants.Cameras.robotToCamera1); + } /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { // 3️⃣ Update sim each sim tick + visionSim.update(m_robotContainer.getDrivebase().getPose()); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 56b1daa..1d36de0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -402,6 +402,11 @@ public void updateAlerts() { } } + /** Drivetrain getter method */ + public Drive getDrivebase() { + return m_drivebase; + } + /** * Set up the SysID routines from AdvantageKit * diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 27b6eb1..170bcf5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -89,50 +89,62 @@ public class Drive extends SubsystemBase { public Drive(ImuIO imuIO) { this.imuIO = imuIO; - switch (Constants.getSwerveType()) { - case PHOENIX6: - // This one is easy because it's all CTRE all the time - for (int i = 0; i < 4; i++) { - modules[i] = new Module(new ModuleIOTalonFX(i), i); - } - break; - - case YAGSL: - // Then parse the module(s) - Byte modType = RBSIParsing.parseModuleType(); - for (int i = 0; i < 4; i++) { - switch (modType) { - case 0b00000000: // ALL-CTRE - if (kImuType == "navx" || kImuType == "navx_spi") { - modules[i] = new Module(new ModuleIOTalonFX(i), i); - } else { - throw new RuntimeException( - "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!"); - } - case 0b00010000: // Blended Talon Drive / NEO Steer - modules[i] = new Module(new ModuleIOBlended(i), i); - break; - case 0b01010000: // NEO motors + CANcoder - modules[i] = new Module(new ModuleIOSparkCANcoder(i), i); - break; - case 0b01010100: // NEO motors + analog encoder - modules[i] = new Module(new ModuleIOSpark(i), i); - break; - default: - throw new RuntimeException("Invalid swerve module combination"); + if (Constants.getMode() == Mode.REAL) { + + // Case out the swerve types because Az-RBSI supports a lot + switch (Constants.getSwerveType()) { + case PHOENIX6: + // This one is easy because it's all CTRE all the time + for (int i = 0; i < 4; i++) { + modules[i] = new Module(new ModuleIOTalonFX(i), i); } - } + break; + + case YAGSL: + // Then parse the module(s) + Byte modType = RBSIParsing.parseModuleType(); + for (int i = 0; i < 4; i++) { + switch (modType) { + case 0b00000000: // ALL-CTRE + if (kImuType == "navx" || kImuType == "navx_spi") { + modules[i] = new Module(new ModuleIOTalonFX(i), i); + } else { + throw new RuntimeException( + "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!"); + } + case 0b00010000: // Blended Talon Drive / NEO Steer + modules[i] = new Module(new ModuleIOBlended(i), i); + break; + case 0b01010000: // NEO motors + CANcoder + modules[i] = new Module(new ModuleIOSparkCANcoder(i), i); + break; + case 0b01010100: // NEO motors + analog encoder + modules[i] = new Module(new ModuleIOSpark(i), i); + break; + default: + throw new RuntimeException("Invalid swerve module combination"); + } + } + break; - default: - throw new RuntimeException("Invalid Swerve Drive Type"); + default: + throw new RuntimeException("Invalid Swerve Drive Type"); + } + // Start odometry thread (for the real robot) + + PhoenixOdometryThread.getInstance().start(); + + } else { + + // If SIM, just order up some SIM modules! + for (int i = 0; i < 4; i++) { + modules[i] = new Module(new ModuleIOSim(), i); + } } // Usage reporting for swerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); - // Start odometry thread - PhoenixOdometryThread.getInstance().start(); - // Configure Autonomous Path Building for PathPlanner based on `AutoType` switch (Constants.getAutoType()) { case PATHPLANNER: @@ -240,6 +252,21 @@ public void periodic() { gyroDisconnectedAlert.set(!imuInputs.connected && Constants.getMode() != Mode.SIM); } + /** Simulation Periodic Method */ + @Override + public void simulationPeriodic() { + // 1) Advance module physics + for (Module module : modules) { + module.simulationPeriodic(); + } + + // 2) Compute chassis speeds from modules (already updated) + ChassisSpeeds speeds = getChassisSpeeds(); + + // 3) Advance IMU using resulting motion + imuIO.simulationPeriodic(speeds); + } + /** Drive Base Action Functions ****************************************** */ /** diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 8b647bb..4bd9017 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -128,6 +128,11 @@ public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } + /** Forwards the simulation periodic call to the IO layer */ + public void simulationPeriodic() { + io.simulationPeriodic(); + } + /** Returns the module position in radians. */ public double getWheelRadiusCharacterizationPosition() { return inputs.drivePositionRad; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 2b7e9da..1005577 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -60,4 +60,7 @@ public default void setDrivePID(double kP, double kI, double kD) {} /** Set P, I, and D gains for closed-loop control on turn motor */ public default void setTurnPID(double kP, double kI, double kD) {} + + /** Simulation-only update hook */ + default void simulationPeriodic() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 5dfa59b..3348b29 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -9,9 +9,6 @@ package frc.robot.subsystems.drive; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -20,6 +17,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; /** * Physics sim implementation of module IO. The sim models are configured using a set of module @@ -49,19 +47,17 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - public ModuleIOSim( - SwerveModuleConstants - constants) { + public ModuleIOSim() { // Create drive and turn sim models driveSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - DRIVE_GEARBOX, constants.DriveInertia, constants.DriveMotorGearRatio), + DRIVE_GEARBOX, SwerveConstants.kDriveInertia, SwerveConstants.kDriveGearRatio), DRIVE_GEARBOX); turnSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), + TURN_GEARBOX, SwerveConstants.kSteerInertia, SwerveConstants.kSteerGearRatio), TURN_GEARBOX); // Enable wrapping for turn PID @@ -69,45 +65,59 @@ public ModuleIOSim( } @Override - public void updateInputs(ModuleIOInputs inputs) { - // Run closed-loop control + public void simulationPeriodic() { + // Closed-loop control if (driveClosedLoop) { driveAppliedVolts = driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); } else { driveController.reset(); } + if (turnClosedLoop) { + // TODO: turn PID has no feedforward or inertia compensation; fix turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); } else { turnController.reset(); } - // Update simulation state + // Apply voltages driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); - driveSim.update(0.02); - turnSim.update(0.02); - // Update drive inputs + // Advance physics + driveSim.update(Constants.loopPeriodSecs); + turnSim.update(Constants.loopPeriodSecs); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Convert rotor → mechanism + double mechanismPositionRad = driveSim.getAngularPositionRad(); + double mechanismVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + + // Drive inputs inputs.driveConnected = true; - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.drivePositionRad = mechanismPositionRad; + inputs.driveVelocityRadPerSec = mechanismVelocityRadPerSec; inputs.driveAppliedVolts = driveAppliedVolts; inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); - // Update turn inputs + // Turn inputs + double steerPositionRad = turnSim.getAngularPositionRad(); + inputs.turnConnected = true; inputs.turnEncoderConnected = true; - inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAbsolutePosition = new Rotation2d(steerPositionRad); + inputs.turnPosition = new Rotation2d(steerPositionRad); + inputs.turnVelocityRadPerSec = + turnSim.getAngularVelocityRadPerSec() / SwerveConstants.kSteerGearRatio; inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); - // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) + // Odometry (single sample per loop is fine) inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryDrivePositionsRad = new double[] {mechanismPositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @@ -124,10 +134,16 @@ public void setTurnOpenLoop(double output) { } @Override - public void setDriveVelocity(double velocityRadPerSec) { + public void setDriveVelocity(double wheelRadPerSec) { driveClosedLoop = true; - driveFFVolts = DRIVE_KS * Math.signum(velocityRadPerSec) + DRIVE_KV * velocityRadPerSec; - driveController.setSetpoint(velocityRadPerSec); + + // Convert wheel → rotor + double rotorRadPerSec = wheelRadPerSec * SwerveConstants.kDriveGearRatio; + + // Feedforward should also be in rotor units + driveFFVolts = DRIVE_KS * Math.signum(rotorRadPerSec) + DRIVE_KV * rotorRadPerSec; + + driveController.setSetpoint(rotorRadPerSec); } @Override diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 4434b24..3570f35 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -21,6 +21,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; @@ -56,4 +57,7 @@ default void updateInputs(ImuIOInputs inputs) {} /** Zero the yaw to a known field-relative angle */ default void zeroYaw(Rotation2d yaw) {} + + /** Simulation-only update hook */ + default void simulationPeriodic(ChassisSpeeds speeds) {} } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index b0eb800..b47bb8b 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -17,18 +17,22 @@ package frc.robot.subsystems.imu; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.RadiansPerSecond; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import java.util.LinkedList; import java.util.Queue; +import org.littletonrobotics.junction.Logger; /** Simulated IMU for full robot simulation & replay logging */ public class ImuIOSim implements ImuIO { - private double yawDeg = 0.0; - private double yawRateDps = 0.0; + private Rotation2d yaw = Rotation2d.kZero; + private double yawRateRadPerSec = 0.0; private Translation3d linearAccel = Translation3d.kZero; private final Queue odomTimestamps = new LinkedList<>(); @@ -40,18 +44,27 @@ public ImuIOSim(double loopPeriodSecs) { this.loopPeriodSecs = loopPeriodSecs; } + @Override + public void simulationPeriodic(ChassisSpeeds speeds) { + yawRateRadPerSec = speeds.omegaRadiansPerSecond; + yaw = yaw.plus(new Rotation2d(yawRateRadPerSec * loopPeriodSecs)); + + Logger.recordOutput("IMU/Yaw", yaw); + Logger.recordOutput("IMU/YawRateDps", Units.radiansToDegrees(yawRateRadPerSec)); + } + @Override public void updateInputs(ImuIOInputs inputs) { // Populate raw IMU readings inputs.connected = true; - inputs.yawPosition = Rotation2d.fromDegrees(yawDeg); - inputs.yawVelocityRadPerSec = RadiansPerSecond.of(yawRateDps); + inputs.yawPosition = yaw; + inputs.yawVelocityRadPerSec = RadiansPerSecond.of(yawRateRadPerSec); inputs.linearAccel = linearAccel; // Maintain odometry history for latency/logging - double now = System.currentTimeMillis() / 1000.0; + double now = Timer.getFPGATimestamp(); odomTimestamps.add(now); - odomYaws.add(yawDeg); + odomYaws.add(yaw.getDegrees()); while (odomTimestamps.size() > 50) odomTimestamps.poll(); while (odomYaws.size() > 50) odomYaws.poll(); @@ -64,24 +77,19 @@ public void updateInputs(ImuIOInputs inputs) { @Override public void zeroYaw(Rotation2d yaw) { - yawDeg = yaw.getDegrees(); + this.yaw = yaw; } // --- Simulation helpers to update the IMU state --- public void setYawDeg(double deg) { - yawDeg = deg; + yaw = Rotation2d.fromDegrees(deg); } public void setYawRateDps(double dps) { - yawRateDps = dps; + yawRateRadPerSec = Units.degreesToRadians(dps); } public void setLinearAccel(Translation3d accelMps2) { linearAccel = accelMps2; } - - /** Optional: integrate yaw from yawRate over loop period */ - public void integrateYaw() { - yawDeg += yawRateDps * loopPeriodSecs; - } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index cb55b42..6b71f49 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.Cameras; import frc.robot.FieldConstants; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; @@ -53,6 +54,10 @@ public Vision(VisionConsumer consumer, VisionIO... io) { new Alert( "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); } + + // Log the robot-to-camera transformations + Logger.recordOutput("Vision/RobotToCamera0", Cameras.robotToCamera0); + Logger.recordOutput("Vision/RobotToCamera1", Cameras.robotToCamera1); } /** From dc668de24b0d9a67c30066fd9edfdb0390d207e5 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 09:48:21 -0700 Subject: [PATCH 02/10] WIP: Driving in SIM, but rotation is scrambling --- .../frc/robot/subsystems/drive/Drive.java | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 170bcf5..3dcff19 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -50,6 +50,7 @@ import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; +import java.util.Arrays; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -253,18 +254,30 @@ public void periodic() { } /** Simulation Periodic Method */ - @Override public void simulationPeriodic() { - // 1) Advance module physics + // --- 1️⃣ Advance module physics first --- for (Module module : modules) { module.simulationPeriodic(); } - // 2) Compute chassis speeds from modules (already updated) - ChassisSpeeds speeds = getChassisSpeeds(); + // --- 2️⃣ Compute chassis speeds from updated module states --- + // This uses the module states (velocities + angles) to get robot-relative motion + SwerveModuleState[] moduleStates = + Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(moduleStates); + + // --- 3️⃣ Update simulated IMU --- + // Feed it the chassis angular velocity so yaw stays consistent with module rotations + imuIO.simulationPeriodic(chassisSpeeds); + + // --- 4️⃣ Update odometry --- + SwerveModulePosition[] modulePositions = + Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); + m_PoseEstimator.update(imuInputs.yawPosition, modulePositions); - // 3) Advance IMU using resulting motion - imuIO.simulationPeriodic(speeds); + // --- 5️⃣ Logging for AdvantageScope --- + Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); + Logger.recordOutput("Drive/ChassisSpeeds", chassisSpeeds); } /** Drive Base Action Functions ****************************************** */ From 970ea5e20704a37b28bc2d22a8150b3aaaa649b4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 14:17:57 -0700 Subject: [PATCH 03/10] More simulation work --- .../frc/robot/subsystems/drive/Drive.java | 82 +++++++++---------- .../frc/robot/subsystems/imu/ImuIOSim.java | 9 ++ 2 files changed, 50 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3dcff19..2ed5ee4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -28,7 +28,6 @@ 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.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -47,6 +46,7 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.subsystems.imu.ImuIO; +import frc.robot.subsystems.imu.ImuIOSim; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; @@ -67,7 +67,7 @@ public class Drive extends SubsystemBase { new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Rotation2d rawGyroRotation = Rotation2d.kZero; + private Rotation2d rawGyroRotation = imuInputs.yawPosition; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { new SwerveModulePosition(), @@ -210,36 +210,36 @@ public void periodic() { // Update the IMU inputs — logging happens automatically imuIO.updateInputs(imuInputs); - // Feed historical samples into odometry - double[] sampleTimestamps = modules[0].getOdometryTimestamps(); - int sampleCount = sampleTimestamps.length; - - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; - moduleDeltas[moduleIndex] = - new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } + // Feed historical samples into odometry if REAL robot + if (Constants.getMode() != Mode.SIM) { + double[] sampleTimestamps = modules[0].getOdometryTimestamps(); + int sampleCount = sampleTimestamps.length; + + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } - // Update gyro angle for odometry - if (imuInputs.connected && imuInputs.odometryYawPositions.length > i) { - rawGyroRotation = imuInputs.odometryYawPositions[i]; - } else { - // Use the angle delta from the kinematics and module deltas - Twist2d twist = kinematics.toTwist2d(moduleDeltas); - rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); - } + // Update gyro angle for odometry + Rotation2d yaw = + imuInputs.connected && imuInputs.odometryYawPositions.length > i + ? imuInputs.odometryYawPositions[i] + : imuInputs.yawPosition; - // Apply to pose estimator - m_PoseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + // Apply to pose estimator + m_PoseEstimator.updateWithTime(sampleTimestamps[i], yaw, modulePositions); + } + Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); } // Module periodic updates @@ -254,28 +254,28 @@ public void periodic() { } /** Simulation Periodic Method */ + @Override public void simulationPeriodic() { - // --- 1️⃣ Advance module physics first --- - for (Module module : modules) { - module.simulationPeriodic(); - } + // Advance module physics + for (Module module : modules) module.simulationPeriodic(); - // --- 2️⃣ Compute chassis speeds from updated module states --- - // This uses the module states (velocities + angles) to get robot-relative motion + // Compute chassis speeds SwerveModuleState[] moduleStates = Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(moduleStates); - // --- 3️⃣ Update simulated IMU --- - // Feed it the chassis angular velocity so yaw stays consistent with module rotations + // Update simulated IMU imuIO.simulationPeriodic(chassisSpeeds); - // --- 4️⃣ Update odometry --- + // Update odometry (physics-step only) SwerveModulePosition[] modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); - m_PoseEstimator.update(imuInputs.yawPosition, modulePositions); - // --- 5️⃣ Logging for AdvantageScope --- + Rotation2d yaw = (imuIO instanceof ImuIOSim sim) ? sim.getYaw() : imuInputs.yawPosition; + + m_PoseEstimator.update(yaw, modulePositions); + + // Logging Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); Logger.recordOutput("Drive/ChassisSpeeds", chassisSpeeds); } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index b47bb8b..f45919b 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -80,6 +80,15 @@ public void zeroYaw(Rotation2d yaw) { this.yaw = yaw; } + /** + * YAW Getter function + * + * @return The current Rotation2d YAW + */ + public Rotation2d getYaw() { + return yaw; + } + // --- Simulation helpers to update the IMU state --- public void setYawDeg(double deg) { yaw = Rotation2d.fromDegrees(deg); From 228c9223212e0f59ae9c3dcd24815ef8e3982b10 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 15:36:42 -0700 Subject: [PATCH 04/10] Another stab at the SIM, including some drive physics --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 51 +++++--- .../subsystems/drive/DriveSimPhysics.java | 111 ++++++++++++++++++ .../java/frc/robot/subsystems/imu/ImuIO.java | 7 +- .../frc/robot/subsystems/imu/ImuIOSim.java | 69 +++++------ 5 files changed, 183 insertions(+), 59 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1d36de0..8d28cbd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -156,7 +156,7 @@ public RobotContainer() { case SIM: // Sim robot, instantiate physics sim IO implementations - m_imu = new ImuIOSim(Constants.loopPeriodSecs); + m_imu = new ImuIOSim(); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIOSim() {}); m_vision = @@ -169,7 +169,7 @@ public RobotContainer() { default: // Replayed robot, disable IO implementations - m_imu = new ImuIOSim(Constants.loopPeriodSecs); + m_imu = new ImuIOSim(); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 2ed5ee4..cdd0a23 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -46,7 +46,6 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.subsystems.imu.ImuIO; -import frc.robot.subsystems.imu.ImuIOSim; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; @@ -86,6 +85,8 @@ public class Drive extends SubsystemBase { new TrapezoidProfile.Constraints( DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); + private DriveSimPhysics simPhysics; + // Constructor public Drive(ImuIO imuIO) { this.imuIO = imuIO; @@ -141,6 +142,13 @@ public Drive(ImuIO imuIO) { for (int i = 0; i < 4; i++) { modules[i] = new Module(new ModuleIOSim(), i); } + + // Load the physics simulator + simPhysics = + new DriveSimPhysics( + kinematics, + 6.0, // kg·m² (swerve typical) + 120.0); // Nm } // Usage reporting for swerve template @@ -256,28 +264,37 @@ public void periodic() { /** Simulation Periodic Method */ @Override public void simulationPeriodic() { - // Advance module physics - for (Module module : modules) module.simulationPeriodic(); + final double dt = Constants.loopPeriodSecs; + + // 1️⃣ Advance module wheel physics + for (Module module : modules) { + module.simulationPeriodic(); + } - // Compute chassis speeds - SwerveModuleState[] moduleStates = + // 2️⃣ Observe module states + SwerveModuleState[] states = Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); - ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(moduleStates); - // Update simulated IMU - imuIO.simulationPeriodic(chassisSpeeds); + // 3️⃣ Integrate authoritative physics (linear + angular) + simPhysics.update(states, dt); - // Update odometry (physics-step only) + // 4️⃣ Feed IMU from physics (authoritative) + imuIO.simulationSetYaw(simPhysics.getYaw()); + imuIO.simulationSetOmega(simPhysics.getOmegaRadPerSec()); + + // 5️⃣ Feed PoseEstimator from authoritative physics + // SIM-only: ignore kinematics-based deltas; just trust DriveSimPhysics pose SwerveModulePosition[] modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); - Rotation2d yaw = (imuIO instanceof ImuIOSim sim) ? sim.getYaw() : imuInputs.yawPosition; + m_PoseEstimator.resetPosition(simPhysics.getYaw(), modulePositions, simPhysics.getPose()); - m_PoseEstimator.update(yaw, modulePositions); + // 6️⃣ Optional: inject vision measurement (e.g., photon vision) + // m_PoseEstimator.addVisionMeasurement(visionPose, visionTimestamp, visionStdDevs); - // Logging - Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); - Logger.recordOutput("Drive/ChassisSpeeds", chassisSpeeds); + // 7️⃣ Logging + Logger.recordOutput("Sim/Pose", simPhysics.getPose()); + Logger.recordOutput("Sim/Yaw", simPhysics.getYaw()); } /** Drive Base Action Functions ****************************************** */ @@ -400,12 +417,18 @@ public ChassisSpeeds getChassisSpeeds() { /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { + if (Constants.getMode() == Mode.SIM) { + return simPhysics.getPose(); + } return m_PoseEstimator.getEstimatedPosition(); } /** Returns the current odometry rotation. */ @AutoLogOutput(key = "Odometry/Yaw") public Rotation2d getHeading() { + if (Constants.getMode() == Mode.SIM) { + return simPhysics.getYaw(); + } return imuInputs.yawPosition; } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java new file mode 100644 index 0000000..ce5dfaf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -0,0 +1,111 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.MathUtil; +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 edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +/** + * Authoritative drivetrain physics model for SIM. + * + *

Responsibilities: - Integrates yaw with inertia + damping - Integrates translation in the + * correct frame - Produces the ONE true robot pose for SIM + */ +public class DriveSimPhysics { + + /* ---------------- Physical state ---------------- */ + private Pose2d pose = Pose2d.kZero; + private double omegaRadPerSec = 0.0; + private Translation2d prevVelocity = new Translation2d(0, 0); + private Translation2d linearAccel = new Translation2d(0, 0); + + /* ---------------- Robot constants ---------------- */ + private final double moiKgMetersSq; + private final double maxTorqueNm; + private static final double ANGULAR_DAMPING = 4.0; // Nm per rad/sec + + private final SwerveDriveKinematics kinematics; + + public DriveSimPhysics( + SwerveDriveKinematics kinematics, double moiKgMetersSq, double maxTorqueNm) { + this.kinematics = kinematics; + this.moiKgMetersSq = moiKgMetersSq; + this.maxTorqueNm = maxTorqueNm; + } + + /** Integrate physics from module states (SIM-only) */ + public void update(SwerveModuleState[] moduleStates, double dtSeconds) { + + // ------------------ LINEAR ------------------ + ChassisSpeeds chassis = kinematics.toChassisSpeeds(moduleStates); + Translation2d fieldVelocity = + new Translation2d(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond); + + // Finite-difference linear acceleration for IMU simulation + linearAccel = fieldVelocity.minus(prevVelocity).div(dtSeconds); + prevVelocity = fieldVelocity; + + Translation2d newTranslation = pose.getTranslation().plus(fieldVelocity.times(dtSeconds)); + + // ------------------ ANGULAR ------------------ + double commandedOmega = chassis.omegaRadiansPerSecond; + + // Convert commanded angular velocity to torque (simple capped model) + double torque = + MathUtil.clamp(commandedOmega * moiKgMetersSq / dtSeconds, -maxTorqueNm, maxTorqueNm); + + // Integrate angular velocity with damping + omegaRadPerSec += (torque / moiKgMetersSq - ANGULAR_DAMPING * omegaRadPerSec) * dtSeconds; + + // Integrate yaw + Rotation2d newYaw = pose.getRotation().plus(new Rotation2d(omegaRadPerSec * dtSeconds)); + + // ------------------ FINAL POSE ------------------ + pose = new Pose2d(newTranslation, newYaw); + } + + /* ================== Getters ================== */ + public Pose2d getPose() { + return pose; + } + + public Rotation2d getYaw() { + return pose.getRotation(); + } + + public double getOmegaRadPerSec() { + return omegaRadPerSec; + } + + public Translation2d getLinearAccel() { + return linearAccel; + } + + /* ================== Reset ================== */ + public void reset(Pose2d pose) { + this.pose = pose; + this.omegaRadPerSec = 0.0; + this.prevVelocity = new Translation2d(); + this.linearAccel = new Translation2d(); + } +} diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 3570f35..45d47c6 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -21,7 +21,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; @@ -58,6 +57,8 @@ default void updateInputs(ImuIOInputs inputs) {} /** Zero the yaw to a known field-relative angle */ default void zeroYaw(Rotation2d yaw) {} - /** Simulation-only update hook */ - default void simulationPeriodic(ChassisSpeeds speeds) {} + /** Simulation-only update hooks */ + default void simulationSetYaw(Rotation2d yaw) {} + + default void simulationSetOmega(double omegaRadPerSec) {} } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index f45919b..10a7b6d 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -21,8 +21,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import java.util.LinkedList; import java.util.Queue; @@ -31,74 +29,65 @@ /** Simulated IMU for full robot simulation & replay logging */ public class ImuIOSim implements ImuIO { + // --- AUTHORITATIVE SIM STATE --- private Rotation2d yaw = Rotation2d.kZero; private double yawRateRadPerSec = 0.0; private Translation3d linearAccel = Translation3d.kZero; + // --- ODOMETRY HISTORY FOR LOGGING/LATENCY --- private final Queue odomTimestamps = new LinkedList<>(); - private final Queue odomYaws = new LinkedList<>(); + private final Queue odomYaws = new LinkedList<>(); - private final double loopPeriodSecs; + public ImuIOSim() {} - public ImuIOSim(double loopPeriodSecs) { - this.loopPeriodSecs = loopPeriodSecs; + // ---------------- SIMULATION INPUTS (PUSH) ---------------- + + /** Set yaw from authoritative physics */ + public void simulationSetYaw(Rotation2d yaw) { + this.yaw = yaw; } - @Override - public void simulationPeriodic(ChassisSpeeds speeds) { - yawRateRadPerSec = speeds.omegaRadiansPerSecond; - yaw = yaw.plus(new Rotation2d(yawRateRadPerSec * loopPeriodSecs)); + /** Set angular velocity from authoritative physics */ + public void simulationSetOmega(double omegaRadPerSec) { + this.yawRateRadPerSec = omegaRadPerSec; + } - Logger.recordOutput("IMU/Yaw", yaw); - Logger.recordOutput("IMU/YawRateDps", Units.radiansToDegrees(yawRateRadPerSec)); + /** Set linear acceleration from physics (optional) */ + public void setLinearAccel(Translation3d accelMps2) { + this.linearAccel = accelMps2; } + // ---------------- IO UPDATE (PULL) ---------------- + + /** Populate the IMUIOInputs object with the current SIM state */ @Override public void updateInputs(ImuIOInputs inputs) { - // Populate raw IMU readings inputs.connected = true; + + // Authoritative physics inputs.yawPosition = yaw; inputs.yawVelocityRadPerSec = RadiansPerSecond.of(yawRateRadPerSec); inputs.linearAccel = linearAccel; - // Maintain odometry history for latency/logging + // Maintain odometry history for logging / latency purposes double now = Timer.getFPGATimestamp(); odomTimestamps.add(now); - odomYaws.add(yaw.getDegrees()); + odomYaws.add(yaw); while (odomTimestamps.size() > 50) odomTimestamps.poll(); while (odomYaws.size() > 50) odomYaws.poll(); - // Fill the provided inputs object inputs.odometryYawTimestamps = odomTimestamps.stream().mapToDouble(d -> d).toArray(); - inputs.odometryYawPositions = - odomYaws.stream().map(d -> Rotation2d.fromDegrees(d)).toArray(Rotation2d[]::new); + inputs.odometryYawPositions = odomYaws.toArray(Rotation2d[]::new); + + // Logging for SIM analysis + Logger.recordOutput("IMU/Yaw", yaw); + Logger.recordOutput("IMU/YawRateDps", Math.toDegrees(yawRateRadPerSec)); } @Override public void zeroYaw(Rotation2d yaw) { this.yaw = yaw; - } - - /** - * YAW Getter function - * - * @return The current Rotation2d YAW - */ - public Rotation2d getYaw() { - return yaw; - } - - // --- Simulation helpers to update the IMU state --- - public void setYawDeg(double deg) { - yaw = Rotation2d.fromDegrees(deg); - } - - public void setYawRateDps(double dps) { - yawRateRadPerSec = Units.degreesToRadians(dps); - } - - public void setLinearAccel(Translation3d accelMps2) { - linearAccel = accelMps2; + this.yawRateRadPerSec = 0.0; } } From 1189c8432e4022dc3f8b9973ee151fbe8b310bf0 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 16:03:17 -0700 Subject: [PATCH 05/10] WIP: Yet another stab at the simulation working --- .../frc/robot/subsystems/drive/Drive.java | 25 +++++++++++++------ .../subsystems/drive/DriveSimPhysics.java | 5 ++-- .../java/frc/robot/subsystems/imu/ImuIO.java | 2 ++ 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index cdd0a23..04c473d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -28,6 +28,7 @@ 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.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -271,30 +272,38 @@ public void simulationPeriodic() { module.simulationPeriodic(); } - // 2️⃣ Observe module states - SwerveModuleState[] states = + // 2️⃣ Observe module states from the modules (authoritative) + SwerveModuleState[] moduleStates = Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); // 3️⃣ Integrate authoritative physics (linear + angular) - simPhysics.update(states, dt); + simPhysics.update(moduleStates, dt); - // 4️⃣ Feed IMU from physics (authoritative) + // 4️⃣ Feed IMU from authoritative physics imuIO.simulationSetYaw(simPhysics.getYaw()); imuIO.simulationSetOmega(simPhysics.getOmegaRadPerSec()); + imuIO.setLinearAccel( + new Translation3d( + simPhysics.getLinearAccel().getX(), simPhysics.getLinearAccel().getY(), 0.0)); - // 5️⃣ Feed PoseEstimator from authoritative physics - // SIM-only: ignore kinematics-based deltas; just trust DriveSimPhysics pose + // 5️⃣ Feed PoseEstimator with authoritative yaw and module positions SwerveModulePosition[] modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); - m_PoseEstimator.resetPosition(simPhysics.getYaw(), modulePositions, simPhysics.getPose()); + // Reset PoseEstimator to match SIM pose perfectly + m_PoseEstimator.resetPosition( + simPhysics.getYaw(), // gyro reading (authoritative) + modulePositions, // wheel positions + simPhysics.getPose() // pose is authoritative + ); - // 6️⃣ Optional: inject vision measurement (e.g., photon vision) + // 6️⃣ Optional: inject vision measurement if available // m_PoseEstimator.addVisionMeasurement(visionPose, visionTimestamp, visionStdDevs); // 7️⃣ Logging Logger.recordOutput("Sim/Pose", simPhysics.getPose()); Logger.recordOutput("Sim/Yaw", simPhysics.getYaw()); + Logger.recordOutput("Sim/LinearAccel", simPhysics.getLinearAccel()); } /** Drive Base Action Functions ****************************************** */ diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java index ce5dfaf..b1174ad 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -61,16 +61,17 @@ public void update(SwerveModuleState[] moduleStates, double dtSeconds) { Translation2d fieldVelocity = new Translation2d(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond); - // Finite-difference linear acceleration for IMU simulation + // Compute finite-difference acceleration linearAccel = fieldVelocity.minus(prevVelocity).div(dtSeconds); prevVelocity = fieldVelocity; + // Integrate translation Translation2d newTranslation = pose.getTranslation().plus(fieldVelocity.times(dtSeconds)); // ------------------ ANGULAR ------------------ double commandedOmega = chassis.omegaRadiansPerSecond; - // Convert commanded angular velocity to torque (simple capped model) + // Convert to torque (simple model) and clamp double torque = MathUtil.clamp(commandedOmega * moiKgMetersSq / dtSeconds, -maxTorqueNm, maxTorqueNm); diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 45d47c6..9273f0c 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -61,4 +61,6 @@ default void zeroYaw(Rotation2d yaw) {} default void simulationSetYaw(Rotation2d yaw) {} default void simulationSetOmega(double omegaRadPerSec) {} + + default void setLinearAccel(Translation3d accelMps2) {} } From 4874b9bd03328ac5250022a1ce1b81a2114d4d47 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 16:22:29 -0700 Subject: [PATCH 06/10] Field-relative drive works in SIM --- .../frc/robot/subsystems/drive/Drive.java | 55 +++++++++++++++++-- .../subsystems/drive/DriveSimPhysics.java | 44 +++++++++------ 2 files changed, 78 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 04c473d..d66075b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -40,6 +40,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -272,11 +273,11 @@ public void simulationPeriodic() { module.simulationPeriodic(); } - // 2️⃣ Observe module states from the modules (authoritative) + // 2️⃣ Get module states from modules (authoritative) SwerveModuleState[] moduleStates = Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); - // 3️⃣ Integrate authoritative physics (linear + angular) + // 3️⃣ Update SIM physics (linear + angular) simPhysics.update(moduleStates, dt); // 4️⃣ Feed IMU from authoritative physics @@ -290,15 +291,19 @@ public void simulationPeriodic() { SwerveModulePosition[] modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); - // Reset PoseEstimator to match SIM pose perfectly m_PoseEstimator.resetPosition( simPhysics.getYaw(), // gyro reading (authoritative) modulePositions, // wheel positions simPhysics.getPose() // pose is authoritative ); - // 6️⃣ Optional: inject vision measurement if available - // m_PoseEstimator.addVisionMeasurement(visionPose, visionTimestamp, visionStdDevs); + // 6️⃣ Optional: inject vision measurement in SIM + if (simulatedVisionAvailable) { + Pose2d visionPose = getSimulatedVisionPose(); + double visionTimestamp = Timer.getFPGATimestamp(); + var visionStdDevs = getSimulatedVisionStdDevs(); + m_PoseEstimator.addVisionMeasurement(visionPose, visionTimestamp, visionStdDevs); + } // 7️⃣ Logging Logger.recordOutput("Sim/Pose", simPhysics.getPose()); @@ -562,4 +567,44 @@ public void followTrajectory(SwerveSample sample) { // Apply the generated speeds runVelocity(speeds); } + + // ---------------- SIM VISION ---------------- + + // Vision measurement enabled in simulation + private boolean simulatedVisionAvailable = true; + + // Maximum simulated noise in meters/radians + private static final double SIM_VISION_POS_NOISE_M = 0.02; // ±2cm + private static final double SIM_VISION_YAW_NOISE_RAD = Math.toRadians(2); // ±2 degrees + + /** + * Returns a simulated Pose2d for vision in field coordinates. Adds a small random jitter to + * simulate measurement error. + */ + private Pose2d getSimulatedVisionPose() { + Pose2d truePose = simPhysics.getPose(); // authoritative pose + + // Add small random noise + double dx = (Math.random() * 2 - 1) * SIM_VISION_POS_NOISE_M; + double dy = (Math.random() * 2 - 1) * SIM_VISION_POS_NOISE_M; + double dTheta = (Math.random() * 2 - 1) * SIM_VISION_YAW_NOISE_RAD; + + return new Pose2d( + truePose.getX() + dx, + truePose.getY() + dy, + truePose.getRotation().plus(new Rotation2d(dTheta))); + } + + /** + * Returns the standard deviations for the simulated vision measurement. These values are used by + * the PoseEstimator to weight vision updates. + */ + private edu.wpi.first.math.Matrix getSimulatedVisionStdDevs() { + edu.wpi.first.math.Matrix stdDevs = + new edu.wpi.first.math.Matrix<>(N3.instance, N1.instance); + stdDevs.set(0, 0, 0.02); // X standard deviation (meters) + stdDevs.set(1, 0, 0.02); // Y standard deviation (meters) + stdDevs.set(2, 0, Math.toRadians(2)); // rotation standard deviation (radians) + return stdDevs; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java index b1174ad..7bd4f7f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -21,28 +21,27 @@ 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 edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; /** * Authoritative drivetrain physics model for SIM. * - *

Responsibilities: - Integrates yaw with inertia + damping - Integrates translation in the - * correct frame - Produces the ONE true robot pose for SIM + *

Responsibilities: - Integrates yaw with inertia + friction-like damping - Integrates + * translation in the field frame - Produces ONE true robot pose for SIM */ public class DriveSimPhysics { /* ---------------- Physical state ---------------- */ private Pose2d pose = Pose2d.kZero; private double omegaRadPerSec = 0.0; - private Translation2d prevVelocity = new Translation2d(0, 0); private Translation2d linearAccel = new Translation2d(0, 0); + private Translation2d prevVelocity = new Translation2d(0, 0); /* ---------------- Robot constants ---------------- */ private final double moiKgMetersSq; private final double maxTorqueNm; - private static final double ANGULAR_DAMPING = 4.0; // Nm per rad/sec + private static final double ANGULAR_DAMPING = 12.0; // Increased friction-like damping private final SwerveDriveKinematics kinematics; @@ -53,30 +52,43 @@ public DriveSimPhysics( this.maxTorqueNm = maxTorqueNm; } - /** Integrate physics from module states (SIM-only) */ + /** + * Advance the physics model by one timestep. + * + * @param moduleStates Authoritative wheel states + * @param dtSeconds Loop period + */ public void update(SwerveModuleState[] moduleStates, double dtSeconds) { - // ------------------ LINEAR ------------------ - ChassisSpeeds chassis = kinematics.toChassisSpeeds(moduleStates); - Translation2d fieldVelocity = + // ------------------ CHASSIS VELOCITY ------------------ + var chassis = kinematics.toChassisSpeeds(moduleStates); + + // Robot-relative velocity + Translation2d robotVelocity = new Translation2d(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond); - // Compute finite-difference acceleration + // Rotate into field frame + Translation2d fieldVelocity = robotVelocity.rotateBy(pose.getRotation()); + + // Compute linear acceleration (finite difference) linearAccel = fieldVelocity.minus(prevVelocity).div(dtSeconds); prevVelocity = fieldVelocity; - // Integrate translation + // Integrate translation in field frame Translation2d newTranslation = pose.getTranslation().plus(fieldVelocity.times(dtSeconds)); // ------------------ ANGULAR ------------------ double commandedOmega = chassis.omegaRadiansPerSecond; - // Convert to torque (simple model) and clamp + // Simple torque model with damping double torque = MathUtil.clamp(commandedOmega * moiKgMetersSq / dtSeconds, -maxTorqueNm, maxTorqueNm); - // Integrate angular velocity with damping - omegaRadPerSec += (torque / moiKgMetersSq - ANGULAR_DAMPING * omegaRadPerSec) * dtSeconds; + // Stronger angular damping to simulate motor friction + torque -= omegaRadPerSec * ANGULAR_DAMPING; + + // Integrate angular velocity + omegaRadPerSec += (torque / moiKgMetersSq) * dtSeconds; // Integrate yaw Rotation2d newYaw = pose.getRotation().plus(new Rotation2d(omegaRadPerSec * dtSeconds)); @@ -106,7 +118,7 @@ public Translation2d getLinearAccel() { public void reset(Pose2d pose) { this.pose = pose; this.omegaRadPerSec = 0.0; - this.prevVelocity = new Translation2d(); - this.linearAccel = new Translation2d(); + this.prevVelocity = new Translation2d(0, 0); + this.linearAccel = new Translation2d(0, 0); } } From d22e7dd81830e9bc090816fc74d025eeb2cbe83b Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 16:33:07 -0700 Subject: [PATCH 07/10] Clean non-ASCII characters --- src/main/java/frc/robot/Robot.java | 7 +++--- .../frc/robot/subsystems/drive/Drive.java | 22 +++++++++---------- .../robot/subsystems/drive/ModuleIOSim.java | 4 ++-- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6896baf..474535f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -50,7 +50,7 @@ public class Robot extends LoggedRobot { private RobotContainer m_robotContainer; private Timer m_disabledTimer; - // 1️⃣ Define simulation fields here + // Define simulation fields here private VisionSystemSim visionSim; /** @@ -214,7 +214,7 @@ public void teleopPeriodic() { // For 2026 - REBUILT, the alliance will be provided as a single character // representing the color of the alliance whose goal will go inactive - // first (i.e. ‘R’ = red, ‘B’ = blue). This alliance’s goal will be + // first (i.e. 'R' = red, 'B' = blue). This alliance's goal will be // active in Shifts 2 and 4. // // https://docs.wpilib.org/en/stable/docs/yearly-overview/2026-game-data.html @@ -284,7 +284,8 @@ public void simulationInit() { /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() { // 3️⃣ Update sim each sim tick + public void simulationPeriodic() { + // Update sim each sim tick visionSim.update(m_robotContainer.getDrivebase().getPose()); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index d66075b..8e6aa5e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -149,7 +149,7 @@ public Drive(ImuIO imuIO) { simPhysics = new DriveSimPhysics( kinematics, - 6.0, // kg·m² (swerve typical) + 6.0, // kg m^2 (swerve typical) 120.0); // Nm } @@ -217,7 +217,7 @@ public void periodic() { } } - // Update the IMU inputs — logging happens automatically + // Update the IMU inputs -- logging happens automatically imuIO.updateInputs(imuInputs); // Feed historical samples into odometry if REAL robot @@ -268,26 +268,26 @@ public void periodic() { public void simulationPeriodic() { final double dt = Constants.loopPeriodSecs; - // 1️⃣ Advance module wheel physics + // 1) Advance module wheel physics for (Module module : modules) { module.simulationPeriodic(); } - // 2️⃣ Get module states from modules (authoritative) + // 2) Get module states from modules (authoritative) SwerveModuleState[] moduleStates = Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); - // 3️⃣ Update SIM physics (linear + angular) + // 3) Update SIM physics (linear + angular) simPhysics.update(moduleStates, dt); - // 4️⃣ Feed IMU from authoritative physics + // 4) Feed IMU from authoritative physics imuIO.simulationSetYaw(simPhysics.getYaw()); imuIO.simulationSetOmega(simPhysics.getOmegaRadPerSec()); imuIO.setLinearAccel( new Translation3d( simPhysics.getLinearAccel().getX(), simPhysics.getLinearAccel().getY(), 0.0)); - // 5️⃣ Feed PoseEstimator with authoritative yaw and module positions + // 5) Feed PoseEstimator with authoritative yaw and module positions SwerveModulePosition[] modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); @@ -297,7 +297,7 @@ public void simulationPeriodic() { simPhysics.getPose() // pose is authoritative ); - // 6️⃣ Optional: inject vision measurement in SIM + // 6) Optional: inject vision measurement in SIM if (simulatedVisionAvailable) { Pose2d visionPose = getSimulatedVisionPose(); double visionTimestamp = Timer.getFPGATimestamp(); @@ -305,7 +305,7 @@ public void simulationPeriodic() { m_PoseEstimator.addVisionMeasurement(visionPose, visionTimestamp, visionStdDevs); } - // 7️⃣ Logging + // 7) Logging Logger.recordOutput("Sim/Pose", simPhysics.getPose()); Logger.recordOutput("Sim/Yaw", simPhysics.getYaw()); Logger.recordOutput("Sim/LinearAccel", simPhysics.getLinearAccel()); @@ -574,8 +574,8 @@ public void followTrajectory(SwerveSample sample) { private boolean simulatedVisionAvailable = true; // Maximum simulated noise in meters/radians - private static final double SIM_VISION_POS_NOISE_M = 0.02; // ±2cm - private static final double SIM_VISION_YAW_NOISE_RAD = Math.toRadians(2); // ±2 degrees + private static final double SIM_VISION_POS_NOISE_M = 0.02; // +/- 2cm + private static final double SIM_VISION_YAW_NOISE_RAD = Math.toRadians(2); // +/- 2 degrees /** * Returns a simulated Pose2d for vision in field coordinates. Adds a small random jitter to diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 3348b29..8fb64fe 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -92,7 +92,7 @@ public void simulationPeriodic() { @Override public void updateInputs(ModuleIOInputs inputs) { - // Convert rotor → mechanism + // Convert rotor -> mechanism double mechanismPositionRad = driveSim.getAngularPositionRad(); double mechanismVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); @@ -137,7 +137,7 @@ public void setTurnOpenLoop(double output) { public void setDriveVelocity(double wheelRadPerSec) { driveClosedLoop = true; - // Convert wheel → rotor + // Convert wheel -> rotor double rotorRadPerSec = wheelRadPerSec * SwerveConstants.kDriveGearRatio; // Feedforward should also be in rotor units From b7d26edec38b56e3227ddb76478a2c958d34aa69 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 16:44:26 -0700 Subject: [PATCH 08/10] Clean physics --- src/main/java/frc/robot/Constants.java | 6 +++++- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/drive/Drive.java | 5 +++-- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 081e1e9..a65f5eb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -102,7 +102,7 @@ public static void disableHAL() { /** Physical Constants for Robot Operation ******************************* */ public static final class RobotConstants { - public static final Mass kRobotMass = Kilograms.of(100.); + public static final Mass kRobotMass = Pounds.of(100.); public static final Matter kChassis = new Matter(new Translation3d(0, 0, Inches.of(8).in(Meters)), kRobotMass.in(Kilograms)); // Robot moment of intertial; this can be obtained from a CAD model of your drivetrain. Usually, @@ -112,6 +112,10 @@ public static final class RobotConstants { // Wheel coefficient of friction public static final double kWheelCOF = 1.2; + // Maximum torque applied by wheel + // Kraken X60 stall torque ~7.09 Nm; MK4i L3 gear ratio 6.12:1 + public static final double kMaxWheelTorque = 43.4; // Nm + // Insert here the orientation (CCW == +) of the Rio and IMU from the robot // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference // frame. diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 474535f..83d5b19 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -284,7 +284,7 @@ public void simulationInit() { /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() { + public void simulationPeriodic() { // Update sim each sim tick visionSim.update(m_robotContainer.getDrivebase().getPose()); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8e6aa5e..b7beeb8 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -47,6 +47,7 @@ import frc.robot.Constants; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.RobotConstants; import frc.robot.subsystems.imu.ImuIO; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; @@ -149,8 +150,8 @@ public Drive(ImuIO imuIO) { simPhysics = new DriveSimPhysics( kinematics, - 6.0, // kg m^2 (swerve typical) - 120.0); // Nm + RobotConstants.kRobotMOI, // kg m^2 + RobotConstants.kMaxWheelTorque); // Nm } // Usage reporting for swerve template From 868d13de275c56b1696fa8e575917f2a797c1447 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 17 Jan 2026 21:27:43 -0700 Subject: [PATCH 09/10] Grid out camera phase space for placement optimization --- src/main/java/frc/robot/Constants.java | 45 ++++++- src/main/java/frc/robot/RobotContainer.java | 54 ++++++++ .../frc/robot/subsystems/drive/Drive.java | 25 ++++ .../subsystems/drive/DriveSimPhysics.java | 2 +- .../vision/CameraPlacementEvaluator.java | 90 +++++++++++++ .../vision/CameraSweepEvaluator.java | 126 ++++++++++++++++++ 6 files changed, 337 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java create mode 100644 src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a65f5eb..d9544dd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,7 +18,6 @@ package frc.robot; import static edu.wpi.first.units.Units.*; -import static frc.robot.util.RBSIEnum.*; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; @@ -38,7 +37,15 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.util.Alert; +import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.CTREPro; +import frc.robot.util.RBSIEnum.DriveStyle; +import frc.robot.util.RBSIEnum.Mode; +import frc.robot.util.RBSIEnum.MotorIdleMode; +import frc.robot.util.RBSIEnum.SwerveType; +import frc.robot.util.RBSIEnum.VisionType; import frc.robot.util.RobotDeviceId; +import org.photonvision.simulation.SimCameraProperties; import swervelib.math.Matter; /** @@ -114,7 +121,7 @@ public static final class RobotConstants { // Maximum torque applied by wheel // Kraken X60 stall torque ~7.09 Nm; MK4i L3 gear ratio 6.12:1 - public static final double kMaxWheelTorque = 43.4; // Nm + public static final double kMaxWheelTorque = 43.4; // Nm // Insert here the orientation (CCW == +) of the Rio and IMU from the robot // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference @@ -425,10 +432,14 @@ public static class Cameras { // Robot to camera transforms // (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead) + // Example Camera are mounted on the frame perimeter, 18" up from the floor, centered + // side-to-side public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + new Transform3d( + Inches.of(13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, 0.0)); public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + new Transform3d( + Inches.of(-13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, Math.PI)); // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) @@ -439,6 +450,32 @@ public static class Cameras { }; } + /************************************************************************* */ + /** Simulation Camera Properties ***************************************** */ + public static class SimCameras { + public static final SimCameraProperties kSimCamera1Props = + new SimCameraProperties() { + { + setCalibration(1280, 800, Rotation2d.fromDegrees(90)); + setCalibError(0.25, 0.08); + setFPS(30); + setAvgLatencyMs(20); + setLatencyStdDevMs(5); + } + }; + + public static final SimCameraProperties kSimCamera2Props = + new SimCameraProperties() { + { + setCalibration(1280, 800, Rotation2d.fromDegrees(90)); + setCalibError(0.25, 0.08); + setFPS(30); + setAvgLatencyMs(20); + setLatencyStdDevMs(5); + } + }; + } + /************************************************************************* */ /** Deploy Directoy Location Constants *********************************** */ public static final class DeployConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8d28cbd..c8099d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,17 +23,21 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.SimCameras; import frc.robot.FieldConstants.AprilTagLayoutType; import frc.robot.commands.AutopilotCommands; import frc.robot.commands.DriveCommands; @@ -47,6 +51,7 @@ import frc.robot.subsystems.imu.ImuIONavX; import frc.robot.subsystems.imu.ImuIOPigeon2; import frc.robot.subsystems.imu.ImuIOSim; +import frc.robot.subsystems.vision.CameraSweepEvaluator; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOLimelight; @@ -61,6 +66,9 @@ import frc.robot.util.RBSIPowerMonitor; import java.util.Set; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.VisionSystemSim; /** This is the location for defining robot hardware, commands, and controller button bindings. */ public class RobotContainer { @@ -72,6 +80,10 @@ public class RobotContainer { final CommandXboxController operatorController = new CommandXboxController(1); // Second Operator final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches + // These two are needed for the Sweep evaluator for camera FOV simulation + final CommandJoystick joystick3 = new CommandJoystick(3); // Joystick for CamersSweepEvaluator + private final CameraSweepEvaluator sweep; + /** Declare the robot subsystems here ************************************ */ // These are the "Active Subsystems" that the robot controls private final Drive m_drivebase; @@ -152,6 +164,7 @@ public RobotContainer() { default -> null; }; m_accel = new Accelerometer(m_imu); + sweep = null; break; case SIM: @@ -165,6 +178,27 @@ public RobotContainer() { new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, m_drivebase::getPose), new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, m_drivebase::getPose)); m_accel = new Accelerometer(m_imu); + + // CameraSweepEvaluator Construct + // 1) Create the vision simulation world + VisionSystemSim visionSim = new VisionSystemSim("CameraSweepWorld"); + // 2) Add AprilTags (field layout) + visionSim.addAprilTags(FieldConstants.aprilTagLayout); + // 3) Create two simulated cameras + // Create PhotonCamera objects (names must match VisionIOPhotonVisionSim) + PhotonCamera camera1 = new PhotonCamera(camera0Name); + PhotonCamera camera2 = new PhotonCamera(camera1Name); + + // Wrap them with simulation + properties (2026 API) + PhotonCameraSim cam1 = new PhotonCameraSim(camera1, SimCameras.kSimCamera1Props); + PhotonCameraSim cam2 = new PhotonCameraSim(camera2, SimCameras.kSimCamera2Props); + + // 4) Register cameras with the sim + visionSim.addCamera(cam1, Transform3d.kZero); + visionSim.addCamera(cam2, Transform3d.kZero); + // 5) Create the sweep evaluator + sweep = new CameraSweepEvaluator(visionSim, cam1, cam2); + break; default: @@ -175,6 +209,7 @@ public RobotContainer() { m_vision = new Vision(m_drivebase::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); m_accel = new Accelerometer(m_imu); + sweep = null; break; } @@ -343,6 +378,25 @@ private void configureBindings() { // Stop when command ended m_drivebase::stop, m_drivebase)); + + // Double-press the A button on Joystick3 to run the CameraSweepEvaluator + // Use WPILib's built-in double-press binding + joystick3 + .button(1) + .multiPress(2, 0.2) + .onTrue( + Commands.runOnce( + () -> { + try { + sweep.runFullSweep( + Filesystem.getOperatingDirectory() + .toPath() + .resolve("camera_sweep.csv") + .toString()); + } catch (Exception e) { + e.printStackTrace(); + } + })); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b7beeb8..91129b7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -466,6 +466,31 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } + /** + * Returns the measured chassis speeds in FIELD coordinates. + * + *

+X = field forward +Y = field left CCW+ = counterclockwise + */ + @AutoLogOutput(key = "SwerveChassisSpeeds/FieldMeasured") + public ChassisSpeeds getFieldRelativeSpeeds() { + // Robot-relative measured speeds from modules + ChassisSpeeds robotRelative = getChassisSpeeds(); + + // Convert to field-relative using authoritative yaw + return ChassisSpeeds.fromRobotRelativeSpeeds(robotRelative, getHeading()); + } + + /** + * Returns the FIELD-relative linear velocity of the robot's center. + * + *

+X = field forward +Y = field left + */ + @AutoLogOutput(key = "Drive/FieldLinearVelocity") + public Translation2d getFieldLinearVelocity() { + ChassisSpeeds fieldSpeeds = getFieldRelativeSpeeds(); + return new Translation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond); + } + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ public double getFFCharacterizationVelocity() { double output = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java index 7bd4f7f..e58a4b3 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024-2026 Az-FIRST +// Copyright (c) 2026 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java new file mode 100644 index 0000000..aa907ee --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java @@ -0,0 +1,90 @@ +// Copyright (c) 2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.drive.Drive; +import java.util.List; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; +import org.photonvision.targeting.PhotonPipelineResult; + +public class CameraPlacementEvaluator { + + private final Drive driveSim; + private final VisionSystemSim visionSim; + + public CameraPlacementEvaluator(Drive driveSim, VisionSystemSim visionSim) { + this.driveSim = driveSim; + this.visionSim = visionSim; + } + + public double evaluateCameraPose( + PhotonCameraSim simCam, Transform3d robotToCamera, SimCameraProperties props) { + + // Add or adjust camera transform in vision sim + visionSim.adjustCamera(simCam, robotToCamera); + + // Test field poses + Pose2d[] testPoses = + new Pose2d[] { + new Pose2d(1.0, 1.0, new Rotation2d(0)), + new Pose2d(3.0, 1.0, new Rotation2d(Math.PI / 2)), + new Pose2d(5.0, 3.0, new Rotation2d(Math.PI)), + new Pose2d(2.0, 4.0, new Rotation2d(-Math.PI / 2)), + }; + + double totalScore = 0.0; + double latencyMillis = 0.0; + + for (Pose2d pose : testPoses) { + + driveSim.resetPose(pose); + + for (int i = 0; i < 5; i++) { + driveSim.simulationPeriodic(); + visionSim.update(driveSim.getPose()); + Timer.delay(Constants.loopPeriodSecs); + } + + var maybeCamPose3d = visionSim.getCameraPose(simCam); + if (maybeCamPose3d.isEmpty()) { + continue; + } + Pose3d camFieldPose = maybeCamPose3d.get(); + + List allTargets = (List) visionSim.getVisionTargets(); + + PhotonPipelineResult result = simCam.process(latencyMillis, camFieldPose, allTargets); + + int tagsSeen = result.targets.size(); + + Pose2d estimatedPose = driveSim.getPose(); + double poseError = pose.getTranslation().getDistance(estimatedPose.getTranslation()); + + double score = tagsSeen - poseError; + totalScore += score; + } + + return totalScore / testPoses.length; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java new file mode 100644 index 0000000..5cb532d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java @@ -0,0 +1,126 @@ +// Copyright (c) 2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.io.FileWriter; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Set; +import org.littletonrobotics.junction.Logger; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; +import org.photonvision.targeting.PhotonPipelineResult; + +/** Sweeps two simulated cameras in a grid of poses and writes a CSV of performance. */ +public class CameraSweepEvaluator { + + private final VisionSystemSim visionSim; + private final PhotonCameraSim camSim1; + private final PhotonCameraSim camSim2; + + public CameraSweepEvaluator( + VisionSystemSim visionSim, PhotonCameraSim camSim1, PhotonCameraSim camSim2) { + this.visionSim = visionSim; + this.camSim1 = camSim1; + this.camSim2 = camSim2; + } + + /** + * Run a full sweep of candidate camera placements. + * + * @param outputCsvPath Path to write the CSV results + */ + public void runFullSweep(String outputCsvPath) throws IOException { + // Example field bounds (meters) -- tune these for your field size + double[] fieldX = {0.0, 2.0, 4.0, 6.0, 8.0}; // 0–8m along field length + double[] fieldY = {0.0, 1.0, 2.0, 3.0, 4.0}; // 0–4m along field width + double[] robotZ = {0.0}; // Usually floor height + double[] camYaw = {-30, 0, 30}; // Camera yaw + double[] camPitch = {-10, 0, 10}; // Camera pitch + + try (FileWriter writer = new FileWriter(outputCsvPath)) { + writer.write("robotX,robotY,robotZ," + "cam1Yaw,cam1Pitch,cam2Yaw,cam2Pitch,score\n"); + + // Sweep robot over the field + for (double rx : fieldX) { + for (double ry : fieldY) { + for (double rz : robotZ) { + Pose3d robotPose = + new Pose3d( + new Translation3d(rx, ry, rz), + new Rotation3d(0, 0, 0) // robot heading = 0 for simplicity + ); + + // Sweep camera rotations + for (double c1Yaw : camYaw) { + for (double c1Pitch : camPitch) { + Pose3d cam1Pose = + new Pose3d( + robotPose.getTranslation(), + new Rotation3d(Math.toRadians(c1Pitch), 0, Math.toRadians(c1Yaw))); + + for (double c2Yaw : camYaw) { + for (double c2Pitch : camPitch) { + Pose3d cam2Pose = + new Pose3d( + robotPose.getTranslation(), + new Rotation3d(Math.toRadians(c2Pitch), 0, Math.toRadians(c2Yaw))); + + // Get all vision targets + Set simTargets = visionSim.getVisionTargets(); + List targetList = new ArrayList<>(simTargets); + + // Simulate camera processing + PhotonPipelineResult res1 = camSim1.process(0, cam1Pose, targetList); + PhotonPipelineResult res2 = camSim2.process(0, cam2Pose, targetList); + + // Score + double score = res1.getTargets().size() + res2.getTargets().size(); + if (res1.getTargets().size() >= 2) score += 2.0; + if (res2.getTargets().size() >= 2) score += 2.0; + + // Penalize ambiguity safely with loops + for (var t : res1.getTargets()) score -= t.getPoseAmbiguity() * 2.0; + for (var t : res2.getTargets()) score -= t.getPoseAmbiguity() * 2.0; + + // Write CSV row + writer.write( + String.format( + "%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.1f,%.1f\n", + rx, ry, rz, c1Yaw, c1Pitch, c2Yaw, c2Pitch, score)); + + Logger.recordOutput("CameraSweep/Score", score); + Logger.recordOutput("CameraSweep/Cam1Pose", cam1Pose); + Logger.recordOutput("CameraSweep/Cam2Pose", cam2Pose); + } + } + } + } + } + } + } + + writer.flush(); + } + } +} From 35568f7fbfd0886bc6458f16e7380b84cbb1f8f8 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 19 Jan 2026 16:05:59 -0700 Subject: [PATCH 10/10] Reset ``Constants.java`` for release --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d9544dd..1ebb197 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,7 +63,7 @@ public final class Constants { * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT, * etc.) and the operating modes of the code (REAL, SIM, or REPLAY) */ - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.COMPBOT; // Define swerve, auto, and vision types being used // NOTE: Only PHOENIX6 swerve base has been tested at this point!!! @@ -71,9 +71,9 @@ public final class Constants { // under strict caveat emptor -- and submit any error and bugfixes // via GitHub issues. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL - private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED + private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.MANUAL; // MANUAL, PATHPLANNER, CHOREO - private static VisionType visionType = VisionType.PHOTON; // PHOTON, LIMELIGHT, NONE + private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE /** Enumerate the robot types (name your robots here) */ public static enum RobotType {