Skip to content

Vision Redux #15

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
dlaflotte merged 8 commits into from
Jan 23, 2026
Merged

Vision Redux #15

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
3d71b62
changing how the photon classes work
Luddo183 Dec 7, 2025
585e9f3
adding gyro
Luddo183 Dec 8, 2025
ea039ed
timestamped lombok
Luddo183 Dec 8, 2025
e64e1a2
adding cameraparameters class
Luddo183 Dec 8, 2025
d33100e
Merge remote-tracking branch 'origin/master' into vision-redux
Luddo183 Jan 12, 2026
5ec6743
modifying parameters
Luddo183 Jan 12, 2026
8386098
testing
Luddo183 Jan 18, 2026
4763aa6
fixed vision offset command
Luddo183 Jan 21, 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
5 changes: 2 additions & 3 deletions build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2026.1.1"
id "edu.wpi.first.GradleRIO" version "2026.2.1"
id "com.peterabeles.gversion" version "1.10"
id "com.diffplug.spotless" version "6.12.0"
id "io.freefair.lombok" version "8.4"
Expand Down Expand Up @@ -207,15 +207,14 @@ spotless {
include "**/*.java"
exclude "**/build/**", "**/build-*/**"
exclude "src/main/java/org/team2342/frc/commands/DriveCommands.java"
exclude "src/main/java/org/team2342/frc/commands/DriveCommands.java"
exclude "src/main/java/org/team2342/frc/subsystems/vision/Vision.java"
exclude "src/main/java/org/team2342/frc/subsystems/vision/VisionIO.java"
exclude "src/main/java/org/team2342/frc/subsystems/vision/VisionIOSim.java"
exclude "src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhoton.java"
exclude "src/main/java/org/team2342/lib/util/Elastic.java"
}

licenseHeader("// Copyright (c) $currentYear Team 2342\n// https://github.com/FRCTeamPhoenix\n//\n// This source code is licensed under the MIT License.\n// See the LICENSE file in the root directory of this project.\n\n", "package ") // Your actual license header
licenseHeader("// Copyright (c) $currentYear Team 2342\n// https://github.com/FRCTeamPhoenix\n//\n// This source code is licensed under the MIT License.\n// See the LICENSE file in the root directory of this project.\n\n", "package ")
}
groovyGradle {
target fileTree(".") {
Expand Down
1 change: 1 addition & 0 deletions src/main/deploy/calibrations/left_arducam_800.json
View file
Open in desktop

Large diffs are not rendered by default.

25 changes: 10 additions & 15 deletions src/main/java/org/team2342/frc/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,10 @@
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import org.team2342.lib.util.CameraParameters;

public final class Constants {
public static final Mode CURRENT_MODE = Mode.REAL;
public static final Mode CURRENT_MODE = Mode.SIM;
public static final boolean TUNING = true;

public static enum Mode {
Expand All @@ -27,24 +28,18 @@ public static enum Mode {
}

public static final class VisionConstants {
public static final String LEFT_CAMERA_NAME = "left_arducam";
public static final String RIGHT_CAMERA_NAME = "right_arducam";
public static final String CAMERA_NAME = "left_arducam";

public static final Transform3d FRONT_LEFT_TRANSFORM =
public static final Transform3d CAMERA_TRANSFORM =
new Transform3d(
new Translation3d(
Units.inchesToMeters(6.2944),
Units.inchesToMeters(8.9822),
Units.inchesToMeters(12.125)),
new Rotation3d(0, 0.0, Units.degreesToRadians(-35)));
Units.inchesToMeters(7.883 + 0.5),
Units.inchesToMeters(-10.895 - 0.5),
Units.inchesToMeters(8)),
new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(90 - 61.475)));

public static final Transform3d FRONT_RIGHT_TRANSFORM =
new Transform3d(
new Translation3d(
Units.inchesToMeters(6.2944),
Units.inchesToMeters(-8.9822),
Units.inchesToMeters(12.125)),
new Rotation3d(0, 0.0, Units.degreesToRadians(35)));
public static final CameraParameters LEFT_PARAMETERS =
CameraParameters.loadFromName(CAMERA_NAME, 800, 600).withTransform(CAMERA_TRANSFORM);

// Basic filtering thresholds
public static final double MAX_AMBIGUITY = 0.1;
Expand Down
34 changes: 26 additions & 8 deletions src/main/java/org/team2342/frc/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
import lombok.Getter;
import org.littletonrobotics.junction.LoggedPowerDistribution;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.team2342.frc.Constants.CANConstants;
import org.team2342.frc.Constants.DriveConstants;
import org.team2342.frc.Constants.VisionConstants;
Expand All @@ -36,6 +37,7 @@
import org.team2342.frc.subsystems.drive.ModuleIOTalonFX;
import org.team2342.frc.subsystems.vision.Vision;
import org.team2342.frc.subsystems.vision.VisionIO;
import org.team2342.frc.subsystems.vision.VisionIOPhoton;
import org.team2342.frc.subsystems.vision.VisionIOSim;
import org.team2342.lib.util.AllianceUtils;
import org.team2342.lib.util.EnhancedXboxController;
Expand Down Expand Up @@ -63,7 +65,14 @@ public RobotContainer() {
new ModuleIOTalonFX(CANConstants.FR_IDS, DriveConstants.ENCODER_OFFSETS[1]),
new ModuleIOTalonFX(CANConstants.BL_IDS, DriveConstants.ENCODER_OFFSETS[2]),
new ModuleIOTalonFX(CANConstants.BR_IDS, DriveConstants.ENCODER_OFFSETS[3]));
vision = new Vision(drive::addVisionMeasurement, new VisionIO() {});
vision =
new Vision(
drive::addVisionMeasurement,
drive::getTimestampedHeading,
new VisionIOPhoton(
VisionConstants.LEFT_PARAMETERS,
PoseStrategy.CONSTRAINED_SOLVEPNP,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));

LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev);
break;
Expand All @@ -79,13 +88,11 @@ public RobotContainer() {
vision =
new Vision(
drive::addVisionMeasurement,
drive::getTimestampedHeading,
new VisionIOSim(
VisionConstants.RIGHT_CAMERA_NAME,
VisionConstants.FRONT_RIGHT_TRANSFORM,
drive::getRawOdometryPose),
new VisionIOSim(
VisionConstants.LEFT_CAMERA_NAME,
VisionConstants.FRONT_LEFT_TRANSFORM,
VisionConstants.LEFT_PARAMETERS,
PoseStrategy.CONSTRAINED_SOLVEPNP,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
drive::getRawOdometryPose));

break;
Expand All @@ -98,7 +105,12 @@ public RobotContainer() {
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {});
vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
vision =
new Vision(
drive::addVisionMeasurement,
drive::getTimestampedHeading,
new VisionIO() {},
new VisionIO() {});

break;
}
Expand All @@ -108,6 +120,12 @@ public RobotContainer() {
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
autoChooser.get();

SmartDashboard.putData(
"Calculate Vision Heading Offset",
Commands.runOnce(() -> drive.calculateVisionHeadingOffset())
.alongWith(Commands.print("Calculated Vision Offset"))
.ignoringDisable(true));

if (Constants.TUNING) setupDevelopmentRoutines();

configureBindings();
Expand Down
14 changes: 14 additions & 0 deletions src/main/java/org/team2342/frc/subsystems/drive/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -49,6 +50,7 @@
import org.team2342.lib.util.SwerveSetpointGenerator;
import org.team2342.lib.util.SwerveSetpointGenerator.ModuleLimits;
import org.team2342.lib.util.SwerveSetpointGenerator.SwerveSetpoint;
import org.team2342.lib.util.Timestamped;

public class Drive extends SubsystemBase {
private final GyroIO gyroIO;
Expand All @@ -68,6 +70,9 @@ public class Drive extends SubsystemBase {
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
private Rotation2d rawGyroRotation = new Rotation2d();

@AutoLogOutput(key = "Vision/Heading/Offset")
private Rotation2d visionHeadingOffset = new Rotation2d();

private SwerveModulePosition[] lastModulePositions = // For delta tracking
new SwerveModulePosition[] {
new SwerveModulePosition(),
Expand Down Expand Up @@ -357,6 +362,15 @@ public void addVisionMeasurement(
visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
}

public Timestamped<Rotation2d> getTimestampedHeading() {
return new Timestamped<Rotation2d>(
rawGyroRotation.minus(visionHeadingOffset), Timer.getFPGATimestamp());
}

public void calculateVisionHeadingOffset() {
visionHeadingOffset = rawGyroRotation.minus(getRotation());
}

/** Returns an array of module translations. */
public static Translation2d[] getModuleTranslations() {
return new Translation2d[] {
Expand Down
23 changes: 15 additions & 8 deletions src/main/java/org/team2342/frc/subsystems/vision/Vision.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,22 +19,31 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.LinkedList;
import java.util.List;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import org.team2342.frc.Constants.VisionConstants;
import org.team2342.frc.subsystems.vision.VisionIO.PoseObservationType;
import org.team2342.lib.logging.ExecutionLogger;
import org.team2342.lib.util.AllianceUtils;
import org.team2342.lib.util.Timestamped;

public class Vision extends SubsystemBase {
private final VisionConsumer consumer;
private final VisionIO[] io;
private final VisionIOInputsAutoLogged[] inputs;
private final Alert[] disconnectedAlerts;

public Vision(VisionConsumer consumer, VisionIO... io) {
private final Supplier<Timestamped<Rotation2d>> timestampedHeading;

public Vision(
VisionConsumer consumer,
Supplier<Timestamped<Rotation2d>> timestampedHeading,
VisionIO... io) {
this.consumer = consumer;
this.io = io;

this.timestampedHeading = timestampedHeading;

// Initialize inputs
this.inputs = new VisionIOInputsAutoLogged[io.length];
for (int i = 0; i < inputs.length; i++) {
Expand All @@ -61,8 +70,12 @@ public Rotation2d getTargetX(int cameraIndex) {

@Override
public void periodic() {
Timestamped<Rotation2d> heading = timestampedHeading.get();
Logger.recordOutput("Vision/Heading/Rotation", heading.get());
Logger.recordOutput("Vision/Heading/Timestamp", heading.getTimestamp());

for (int i = 0; i < io.length; i++) {
io[i].updateInputs(inputs[i]);
io[i].updateInputs(inputs[i], heading);
Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
}

Expand Down Expand Up @@ -175,12 +188,6 @@ public void periodic() {
ExecutionLogger.log("Vision");
}

public void toggleHeadingsFree() {
for (VisionIO vis : io) {
vis.toggleHeadingFree();
}
}

@FunctionalInterface
public static interface VisionConsumer {
public void accept(
Expand Down
5 changes: 2 additions & 3 deletions src/main/java/org/team2342/frc/subsystems/vision/VisionIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import org.littletonrobotics.junction.AutoLog;
import org.team2342.lib.util.Timestamped;

public interface VisionIO {
@AutoLog
Expand Down Expand Up @@ -40,7 +41,5 @@ public static enum PoseObservationType {
PHOTONVISION_CONSTRAINED
}

public default void updateInputs(VisionIOInputs inputs) {}

public default void toggleHeadingFree() {}
public default void updateInputs(VisionIOInputs inputs, Timestamped<Rotation2d> heading) {}
}
62 changes: 0 additions & 62 deletions src/main/java/org/team2342/frc/subsystems/vision/VisionIOConstrainedSim.java
View file
Open in desktop

This file was deleted.

Loading