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..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 13c74fa..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(); @@ -202,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 @@ -242,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 56b1daa..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,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(); + } + })); } /** @@ -402,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 27b6eb1..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 ****************************************** */ /** @@ -360,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() { + if (Constants.getMode() == Mode.SIM) { + return simPhysics.getYaw(); + } return imuInputs.yawPosition; } @@ -388,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; @@ -490,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 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 new file mode 100644 index 0000000..e58a4b3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -0,0 +1,124 @@ +// 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.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.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +/** + * Authoritative drivetrain physics model 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 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 - 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..9273f0c 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -56,4 +56,11 @@ default void updateInputs(ImuIOInputs inputs) {} /** Zero the yaw to a known field-relative angle */ default void zeroYaw(Rotation2d yaw) {} + + /** Simulation-only update hooks */ + default void simulationSetYaw(Rotation2d yaw) {} + + default void simulationSetOmega(double omegaRadPerSec) {} + + default void setLinearAccel(Translation3d accelMps2) {} } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index b0eb800..10a7b6d 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -17,71 +17,77 @@ 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.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; + // --- 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; + } + + /** Set angular velocity from authoritative physics */ + public void simulationSetOmega(double omegaRadPerSec) { + this.yawRateRadPerSec = omegaRadPerSec; } + /** 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; - inputs.yawPosition = Rotation2d.fromDegrees(yawDeg); - inputs.yawVelocityRadPerSec = RadiansPerSecond.of(yawRateDps); + + // Authoritative physics + inputs.yawPosition = yaw; + inputs.yawVelocityRadPerSec = RadiansPerSecond.of(yawRateRadPerSec); inputs.linearAccel = linearAccel; - // Maintain odometry history for latency/logging - double now = System.currentTimeMillis() / 1000.0; + // Maintain odometry history for logging / latency purposes + double now = Timer.getFPGATimestamp(); odomTimestamps.add(now); - odomYaws.add(yawDeg); + 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) { - yawDeg = yaw.getDegrees(); - } - - // --- Simulation helpers to update the IMU state --- - public void setYawDeg(double deg) { - yawDeg = deg; - } - - public void setYawRateDps(double dps) { - yawRateDps = dps; - } - - public void setLinearAccel(Translation3d accelMps2) { - linearAccel = accelMps2; - } - - /** Optional: integrate yaw from yawRate over loop period */ - public void integrateYaw() { - yawDeg += yawRateDps * loopPeriodSecs; + this.yaw = yaw; + this.yawRateRadPerSec = 0.0; } } 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(); + } + } +} 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); } /**