+
--------
### Software Requirements
@@ -32,22 +39,22 @@ The Az-RBSI requires the [2026 WPILib Installer](
https://github.com/wpilibsuite/allwpilib/releases) (VSCode and associated
tools), 2026 firmware installed on all hardware (motors, encoders, power
distribution, etc.), the [2026 NI FRC Game Tools](
-https://github.com/wpilibsuite/2026Beta)
+https://www.ni.com/en/support/downloads/drivers/download.frc-game-tools.html)
(Driver Station and associated tools), and the [2026 CTRE Phoenix Tuner X](
https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/index.html). Take a
-moment to update all software and firmware before attempting to load your new
-robot project.
+moment to update all software and firmware to the latest versions before
+attempting to load your new robot project.
Please note that you need these _minimum_ versions of the following components:
-* WPILib ` v2026.1.1-beta-1`
+* WPILib ` v2026.2.1`
* RoboRIO image `FRC_roboRIO_2026_v1.2`
--------
### Setting up your new project
-When your new robot code respository is created, it will have a single commit
+When your new robot code repository is created, it will have a single commit
that contains the entire Az-RBSI template for the current release. (See the
[Az-RBSI Releases page](https://github.com/AZ-First/Az-RBSI/releases) for more
information about the latest release.)
@@ -101,6 +108,12 @@ method are encouraged to submit bug reports and code fixes to the [Az-RBSI
repository](https://github.com/AZ-First/Az-RBSI).
+--------
+
+### Getting Started with your Robot Code
+
+See the Az-RBSI [Getting Started Guide](RBSI-GSG.md) for next steps.
+
--------
### Updating your project based on the latest released version of Az-RBSI
diff --git a/RBSI-GSG.md b/doc/RBSI-GSG.md
similarity index 69%
rename from RBSI-GSG.md
rename to doc/RBSI-GSG.md
index 7c41bd8..11d93d4 100644
--- a/RBSI-GSG.md
+++ b/doc/RBSI-GSG.md
@@ -41,10 +41,16 @@ modifications to extant RBSI code will be done to files within the
### Tuning constants for optimal performance
-4. HHHH
-
+4. Over the course of your robot project, you will need to tune PID parameters
+ for both your drivebase and any mechanisms you build to play the game.
+ AdvantageKit includes detailed instructions for how to tune the various
+ portions of your drivetrain, and we **STRONGLY RECOMMEND** you work through
+ these steps **BEFORE** running your robot.
+ * [Tuning for drivebase with CTRE components](https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template#tuning)
+ * [Tuning for drivebase with REV components](https://docs.advantagekit.org/getting-started/template-projects/spark-swerve-template/#tuning)
+ Similar tuning can be done with subsystem components (flywheel, intake, etc.).
5. Power monitoring by subsystem is included in the Az-RBSI. In order to
properly match subsystems to ports on your Power Distribution Module,
@@ -55,12 +61,17 @@ modifications to extant RBSI code will be done to files within the
instantiation](
https://github.com/AZ-First/Az-RBSI/blob/38f6391cb70c4caa90502710f591682815064677/src/main/java/frc/robot/RobotContainer.java#L154-L157) in the `RobotContainer.java` file.
-6. All of the constants for needed for tuning your robot should be in the
- `Constants.java` file in the `src/main/java/frc/robot` directory. This file
- should be thoroughly edited to match the particulars of your robot. Be sure
- to work through each section of this file and include the proper values for
- your robot.
-
+6. In the `Constants.java` file, the classes following `RobotDevices` contain
+ individual containers for robot subsystems and interaction methods. The
+ `OperatorConstants` class determines how the OPERATOR interacts with the
+ robot. `DriveBaseConstants` and `FlywheelConstants` (and additional classes
+ you add for your own mechanisms) contain human-scale conversions and limits
+ for the subsystem (_e.g._, maximum speed, gear ratios, PID constants, etc.).
+ `AutoConstants` contains the values needed for your autonomous period method
+ of choice (currently supported are MANUAL -- you write your own code;
+ PATHPLANNER, and CHOREO). The next two are related to robot vision, where
+ the vision system constants are contained in `VisionConstants`, and the
+ physical properties (location, FOV, etc.) of the cameras are in `Cameras`.
--------
diff --git a/doc/dependency_enable.png b/doc/dependency_enable.png
new file mode 100644
index 0000000..81691d2
Binary files /dev/null and b/doc/dependency_enable.png differ
diff --git a/src/main/deploy/apriltags/2026-none.json b/src/main/deploy/apriltags/2026-none.json
new file mode 100644
index 0000000..4763fda
--- /dev/null
+++ b/src/main/deploy/apriltags/2026-none.json
@@ -0,0 +1,7 @@
+{
+ "tags": [],
+ "field": {
+ "length": 16.4592,
+ "width": 8.2296
+ }
+}
diff --git a/src/main/deploy/apriltags/2026-official.json b/src/main/deploy/apriltags/2026-official.json
new file mode 100644
index 0000000..8c5a52d
--- /dev/null
+++ b/src/main/deploy/apriltags/2026-official.json
@@ -0,0 +1,584 @@
+{
+ "tags": [
+ {
+ "ID": 1,
+ "pose": {
+ "translation": {
+ "x": 11.8779798,
+ "y": 7.4247756,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 2,
+ "pose": {
+ "translation": {
+ "x": 11.9154194,
+ "y": 4.638039999999999,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.7071067811865476,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 3,
+ "pose": {
+ "translation": {
+ "x": 11.3118646,
+ "y": 4.3902376,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 4,
+ "pose": {
+ "translation": {
+ "x": 11.3118646,
+ "y": 4.0346376,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 5,
+ "pose": {
+ "translation": {
+ "x": 11.9154194,
+ "y": 3.4312351999999997,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 6,
+ "pose": {
+ "translation": {
+ "x": 11.8779798,
+ "y": 0.6444996,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 7,
+ "pose": {
+ "translation": {
+ "x": 11.9528844,
+ "y": 0.6444996,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 8,
+ "pose": {
+ "translation": {
+ "x": 12.2710194,
+ "y": 3.4312351999999997,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 9,
+ "pose": {
+ "translation": {
+ "x": 12.519177399999998,
+ "y": 3.6790375999999996,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 10,
+ "pose": {
+ "translation": {
+ "x": 12.519177399999998,
+ "y": 4.0346376,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 11,
+ "pose": {
+ "translation": {
+ "x": 12.2710194,
+ "y": 4.638039999999999,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.7071067811865476,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 12,
+ "pose": {
+ "translation": {
+ "x": 11.9528844,
+ "y": 7.4247756,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 13,
+ "pose": {
+ "translation": {
+ "x": 16.5333172,
+ "y": 7.4033126,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 14,
+ "pose": {
+ "translation": {
+ "x": 16.5333172,
+ "y": 6.9715126,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 15,
+ "pose": {
+ "translation": {
+ "x": 16.5329616,
+ "y": 4.3235626,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 16,
+ "pose": {
+ "translation": {
+ "x": 16.5329616,
+ "y": 3.8917626,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 17,
+ "pose": {
+ "translation": {
+ "x": 4.6630844,
+ "y": 0.6444996,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 18,
+ "pose": {
+ "translation": {
+ "x": 4.6256194,
+ "y": 3.4312351999999997,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 19,
+ "pose": {
+ "translation": {
+ "x": 5.229174199999999,
+ "y": 3.6790375999999996,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 20,
+ "pose": {
+ "translation": {
+ "x": 5.229174199999999,
+ "y": 4.0346376,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 21,
+ "pose": {
+ "translation": {
+ "x": 4.6256194,
+ "y": 4.638039999999999,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.7071067811865476,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 22,
+ "pose": {
+ "translation": {
+ "x": 4.6630844,
+ "y": 7.4247756,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 23,
+ "pose": {
+ "translation": {
+ "x": 4.5881798,
+ "y": 7.4247756,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 24,
+ "pose": {
+ "translation": {
+ "x": 4.2700194,
+ "y": 4.638039999999999,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.7071067811865476,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 25,
+ "pose": {
+ "translation": {
+ "x": 4.0218614,
+ "y": 4.3902376,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 26,
+ "pose": {
+ "translation": {
+ "x": 4.0218614,
+ "y": 4.0346376,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 27,
+ "pose": {
+ "translation": {
+ "x": 4.2700194,
+ "y": 3.4312351999999997,
+ "z": 1.12395
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 28,
+ "pose": {
+ "translation": {
+ "x": 4.5881798,
+ "y": 0.6444996,
+ "z": 0.889
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 29,
+ "pose": {
+ "translation": {
+ "x": 0.0077469999999999995,
+ "y": 0.6659626,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 30,
+ "pose": {
+ "translation": {
+ "x": 0.0077469999999999995,
+ "y": 1.0977626,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 31,
+ "pose": {
+ "translation": {
+ "x": 0.0080772,
+ "y": 3.7457125999999996,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 32,
+ "pose": {
+ "translation": {
+ "x": 0.0080772,
+ "y": 4.1775126,
+ "z": 0.55245
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ }
+ ],
+ "field": {
+ "length": 16.541,
+ "width": 8.069
+ }
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 9b81736..1ebb197 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;
/**
@@ -64,7 +71,7 @@ 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.NONE; // PHOTON, LIMELIGHT, NONE
@@ -102,7 +109,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 +119,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.
@@ -421,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)
@@ -435,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/Robot.java b/src/main/java/frc/robot/Robot.java
index ccfc193..83d5b19 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;
+ // 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();
@@ -153,6 +165,10 @@ public void autonomousInit() {
// TODO: Make sure Gyro inits here with whatever is in the path planning thingie
switch (Constants.getAutoType()) {
+ case MANUAL:
+ CommandScheduler.getInstance().schedule(m_robotContainer.getManualAuto());
+ break;
+
case PATHPLANNER:
m_autoCommandPathPlanner = m_robotContainer.getAutonomousCommandPathPlanner();
// schedule the autonomous command
@@ -160,6 +176,7 @@ public void autonomousInit() {
CommandScheduler.getInstance().schedule(m_autoCommandPathPlanner);
}
break;
+
case CHOREO:
m_robotContainer.getAutonomousCommandChoreo();
break;
@@ -186,6 +203,9 @@ public void teleopInit() {
CommandScheduler.getInstance().cancelAll();
}
m_robotContainer.setMotorBrake(true);
+
+ // In case this got set in sequential practice sessions or whatever
+ FieldState.wonAuto = null;
}
/** This function is called periodically during operator control. */
@@ -194,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
@@ -234,9 +254,38 @@ 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() {
+ // 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 faf29dd..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,11 +164,12 @@ public RobotContainer() {
default -> null;
};
m_accel = new Accelerometer(m_imu);
+ sweep = null;
break;
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 =
@@ -165,16 +178,38 @@ 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:
// 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 =
new Vision(m_drivebase::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
m_accel = new Accelerometer(m_imu);
+ sweep = null;
break;
}
@@ -343,6 +378,41 @@ 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();
+ }
+ }));
+ }
+
+ /**
+ * Use this to pass the MANUAL SHOOT FUEL command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getManualAuto() {
+ // NOTE:
+ //
+ // For teams not using PathPlanner, this auto may be used to simply shoot the pre-loaded fuel
+ // into the HUB during AUTO. Since shooters are beyond the scope of Az-RBSI, you will have to
+ // write your own command and call it here.
+
+ // Replace Commands.none() with your command that shoots fuel into the HUB.
+ return Commands.none();
}
/**
@@ -386,6 +456,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 0366120..91129b7 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -28,7 +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.Twist2d;
+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;
@@ -40,16 +40,19 @@
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;
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;
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;
@@ -66,7 +69,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(),
@@ -85,54 +88,75 @@ public class Drive extends SubsystemBase {
new TrapezoidProfile.Constraints(
DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel));
+ private DriveSimPhysics simPhysics;
+
// Constructor
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;
+ if (Constants.getMode() == Mode.REAL) {
- 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");
+ // 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);
+ }
+
+ // Load the physics simulator
+ simPhysics =
+ new DriveSimPhysics(
+ kinematics,
+ RobotConstants.kRobotMOI, // kg m^2
+ RobotConstants.kMaxWheelTorque); // Nm
}
// 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:
@@ -194,39 +218,39 @@ 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
- 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
@@ -240,6 +264,54 @@ public void periodic() {
gyroDisconnectedAlert.set(!imuInputs.connected && Constants.getMode() != Mode.SIM);
}
+ /** Simulation Periodic Method */
+ @Override
+ public void simulationPeriodic() {
+ final double dt = Constants.loopPeriodSecs;
+
+ // 1) Advance module wheel physics
+ for (Module module : modules) {
+ module.simulationPeriodic();
+ }
+
+ // 2) Get module states from modules (authoritative)
+ SwerveModuleState[] moduleStates =
+ Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
+
+ // 3) Update SIM physics (linear + angular)
+ simPhysics.update(moduleStates, dt);
+
+ // 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
+ SwerveModulePosition[] modulePositions =
+ Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
+
+ m_PoseEstimator.resetPosition(
+ simPhysics.getYaw(), // gyro reading (authoritative)
+ modulePositions, // wheel positions
+ simPhysics.getPose() // pose is authoritative
+ );
+
+ // 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());
+ Logger.recordOutput("Sim/Yaw", simPhysics.getYaw());
+ Logger.recordOutput("Sim/LinearAccel", simPhysics.getLinearAccel());
+ }
+
/** Drive Base Action Functions ****************************************** */
/**
@@ -304,17 +376,6 @@ public void runCharacterization(double output) {
}
}
- // /** Drive Forward Command Factory **************************************** */
- // // Example factory method
- // public Command driveForwardCommand(double distance) {
- // // This method composes and returns a complex command object
- // return Commands.sequence(
- // // Use internal methods and sensor data to define the command logic
- // new DriveToPositionCommand(this, distance),
- // new StopDrivetrainCommand(this)
- // );
- // }
-
/**
* Reset the heading ProfiledPIDController
*
@@ -353,6 +414,7 @@ private SwerveModuleState[] getModuleStates() {
}
/** Returns the module positions (turn angles and drive positions) for all of the modules. */
+ @AutoLogOutput(key = "SwerveStates/Positions")
private SwerveModulePosition[] getModulePositions() {
SwerveModulePosition[] states = new SwerveModulePosition[4];
for (int i = 0; i < 4; i++) {
@@ -370,12 +432,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() {
- imuIO.updateInputs(imuInputs);
+ if (Constants.getMode() == Mode.SIM) {
+ return simPhysics.getYaw();
+ }
return imuInputs.yawPosition;
}
@@ -398,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;
@@ -500,4 +593,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 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 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 = 12.0; // Increased friction-like damping
+
+ private final SwerveDriveKinematics kinematics;
+
+ public DriveSimPhysics(
+ SwerveDriveKinematics kinematics, double moiKgMetersSq, double maxTorqueNm) {
+ this.kinematics = kinematics;
+ this.moiKgMetersSq = moiKgMetersSq;
+ this.maxTorqueNm = maxTorqueNm;
+ }
+
+ /**
+ * Advance the physics model by one timestep.
+ *
+ * @param moduleStates Authoritative wheel states
+ * @param dtSeconds Loop period
+ */
+ public void update(SwerveModuleState[] moduleStates, double dtSeconds) {
+
+ // ------------------ CHASSIS VELOCITY ------------------
+ var chassis = kinematics.toChassisSpeeds(moduleStates);
+
+ // Robot-relative velocity
+ Translation2d robotVelocity =
+ new Translation2d(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond);
+
+ // 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 in field frame
+ Translation2d newTranslation = pose.getTranslation().plus(fieldVelocity.times(dtSeconds));
+
+ // ------------------ ANGULAR ------------------
+ double commandedOmega = chassis.omegaRadiansPerSecond;
+
+ // Simple torque model with damping
+ double torque =
+ MathUtil.clamp(commandedOmega * moiKgMetersSq / dtSeconds, -maxTorqueNm, maxTorqueNm);
+
+ // 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));
+
+ // ------------------ 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(0, 0);
+ this.linearAccel = new Translation2d(0, 0);
+ }
+}
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..8fb64fe 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