Skip to content

Streamline Camera and CAN instantiation #135

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
tbowers7 merged 2 commits into from
Feb 2, 2026
Merged

Streamline Camera and CAN instantiation #135

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4a7418f
Streamline Camera and CAN instantiation
tbowers7 Jan 31, 2026
a1f564a
Merge branch 'develop' into from_2486
tbowers7 Feb 2, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
122 changes: 59 additions & 63 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,8 +164,10 @@ public static final class PowerConstants {
/************************************************************************* */
/** List of Robot CAN Busses ********************************************* */
public static final class CANBuses {
public static final String DRIVE = "DriveTrain";
public static final String RIO = "";
public static final String DRIVE = "DriveTrain";

public static final String[] ALL = {RIO, DRIVE};
}

/************************************************************************* */
Expand All @@ -178,30 +180,30 @@ public static class RobotDevices {

// Front Left
public static final RobotDeviceId FL_DRIVE =
new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 18);
new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 16);
public static final RobotDeviceId FL_ROTATION =
new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 19);
new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 17);
public static final RobotDeviceId FL_CANCODER =
new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, null);
// Front Right
public static final RobotDeviceId FR_DRIVE =
new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 17);
new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 13);
public static final RobotDeviceId FR_ROTATION =
new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 16);
new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 12);
public static final RobotDeviceId FR_CANCODER =
new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, null);
// Back Left
public static final RobotDeviceId BL_DRIVE =
new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 1);
new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 2);
public static final RobotDeviceId BL_ROTATION =
new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 0);
new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 3);
public static final RobotDeviceId BL_CANCODER =
new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, null);
// Back Right
public static final RobotDeviceId BR_DRIVE =
new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 2);
new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 7);
public static final RobotDeviceId BR_ROTATION =
new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 3);
new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 6);
public static final RobotDeviceId BR_CANCODER =
new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null);
// Pigeon
Expand Down Expand Up @@ -317,7 +319,7 @@ public static final class DrivebaseConstants {
public static final double kDriveD = 0.03;
public static final double kDriveV = 0.83;
public static final double kDriveA = 0.0;
public static final double kDriveS = 0.21;
public static final double kDriveS = 2.00;
public static final double kDriveT =
SwerveConstants.kDriveGearRatio / DCMotor.getKrakenX60Foc(1).KtNMPerAmp;
public static final double kSteerP = 400.0;
Expand Down Expand Up @@ -439,61 +441,55 @@ public static class VisionConstants {

/************************************************************************* */
/** Vision Camera Posses ************************************************* */
public static class Cameras {
// Camera names, must match names configured on coprocessor
public static String camera0Name = "camera_0";
public static String camera1Name = "camera_1";
// ... And more, if needed

// Robot to camera transforms
public static final class Cameras {
public record CameraConfig(
String name,
Transform3d robotToCamera,
double stdDevFactor,
SimCameraProperties simProps) {}

// Camera Configuration Records
// (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead)
// Example Cameras are mounted in the back corners, 18" up from the floor, facing sideways
public static Transform3d robotToCamera0 =
new Transform3d(
Inches.of(-13.0),
Inches.of(13.0),
Inches.of(12.0),
new Rotation3d(0.0, 0.0, Math.PI / 2));
public static Transform3d robotToCamera1 =
new Transform3d(
Inches.of(-13.0),
Inches.of(-13.0),
Inches.of(12.0),
new Rotation3d(0.0, 0.0, -Math.PI / 2));

// Standard deviation multipliers for each camera
// (Adjust to trust some cameras more than others)
public static double[] cameraStdDevFactors =
new double[] {
1.0, // Camera 0
1.0 // Camera 1
};
}

/************************************************************************* */
/** Simulation Camera Properties ***************************************** */
public static class SimCameras {
public static final SimCameraProperties kSimCamera1Props =
new SimCameraProperties() {
{
setCalibration(1280, 800, Rotation2d.fromDegrees(120));
setCalibError(0.25, 0.08);
setFPS(30);
setAvgLatencyMs(20);
setLatencyStdDevMs(5);
}
};

public static final SimCameraProperties kSimCamera2Props =
new SimCameraProperties() {
{
setCalibration(1280, 800, Rotation2d.fromDegrees(120));
setCalibError(0.25, 0.08);
setFPS(30);
setAvgLatencyMs(20);
setLatencyStdDevMs(5);
}
};
public static final CameraConfig[] ALL = {
new CameraConfig(
"camera_0",
new Transform3d(
Inches.of(-13.0),
Inches.of(13.0),
Inches.of(12.0),
new Rotation3d(0.0, 0.0, Math.PI / 2)),
1.0,
new SimCameraProperties() {
{
setCalibration(1280, 800, Rotation2d.fromDegrees(120));
setCalibError(0.25, 0.08);
setFPS(30);
setAvgLatencyMs(20);
setLatencyStdDevMs(5);
}
}),
//
new CameraConfig(
"camera_1",
new Transform3d(
Inches.of(-13.0),
Inches.of(-13.0),
Inches.of(12.0),
new Rotation3d(0.0, 0.0, -Math.PI / 2)),
1.0,
new SimCameraProperties() {
{
setCalibration(1280, 800, Rotation2d.fromDegrees(120));
setCalibError(0.25, 0.08);
setFPS(30);
setAvgLatencyMs(20);
setLatencyStdDevMs(5);
}
}),

// ... And more, if needed
};
}

/************************************************************************* */
Expand Down
55 changes: 26 additions & 29 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,6 @@
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;
Expand All @@ -38,7 +35,6 @@
import org.littletonrobotics.urcl.URCL;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;

/**
Expand Down Expand Up @@ -285,31 +281,32 @@ public void testPeriodic() {}
/** This function is called once when the robot is first started up. */
@Override
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);
// ---------------- SIM-ONLY: vision simulation world + camera sims ----------------
// 1) Create the vision simulation world
visionSim = new VisionSystemSim("CameraSweepWorld");

// 2) Add AprilTags (field layout)
visionSim.addAprilTags(FieldConstants.aprilTagLayout);

// 3) Build PhotonCameraSim objects from Constants camera configs
final Constants.Cameras.CameraConfig[] camConfigs = Constants.Cameras.ALL;

PhotonCameraSim[] simCams = new PhotonCameraSim[camConfigs.length];

for (int i = 0; i < camConfigs.length; i++) {
final var cfg = camConfigs[i];

// Name must match the VisionIOPhotonVisionSim name
PhotonCamera photonCam = new PhotonCamera(cfg.name());

// 2026 API: wrap camera with sim properties from Constants
PhotonCameraSim camSim = new PhotonCameraSim(photonCam, cfg.simProps());

// Register camera with the sim using the robot-to-camera transform
visionSim.addCamera(camSim, cfg.robotToCamera());

simCams[i] = camSim;
}
}

/** This function is called periodically whilst in simulation. */
Expand Down
Loading