Skip to content

Localizer dev - Mishawaka with vision #8

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

Closed
Anunaya08 wants to merge 16 commits into from
Closed

Localizer dev - Mishawaka with vision #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
88b748f
fixed names for all the red auto paths
cindyguo636 Feb 24, 2026
2edbdeb
Merge branch 'alpha' of https://github.com/cindyguo636/Rowdy26 into a…
cindyguo636 Feb 24, 2026
2e939a7
Paths!
Aj9262 Feb 24, 2026
466f986
Merge branch 'alpha' of https://github.com/frc461/Rowdy26 into alpha
Aj9262 Feb 24, 2026
8d36bc4
More Paths! (Almost Done)
Aj9262 Feb 24, 2026
6a0058d
Finished paths
Aj9262 Feb 24, 2026
7b867ac
Fixed AJ's Paths
Anunaya08 Feb 25, 2026
0ab2fd3
changed localizer constants
jooj99 Feb 26, 2026
4581567
defined defaultNetworkTable
jooj99 Feb 27, 2026
d85eef4
fixed localizer periodic
jooj99 Feb 27, 2026
99c42da
changed camera measurement constants
jooj99 Feb 27, 2026
982b0c0
smartdashboard for fieldodom
jooj99 Feb 28, 2026
408c935
removed unused constants
jooj99 Feb 28, 2026
eea3600
Merge alpha into localizer-dev
Copilot Apr 3, 2026
f6d17a5
Merge pull request #6 from frc461/copilot/merge-alpha-into-localizer-dev
Anunaya08 Apr 3, 2026
a1cadd4
Merge pull request #7 from frc461/alpha
Anunaya08 Apr 3, 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
4 changes: 2 additions & 2 deletions src/main/deploy/pathplanner/autos/Left Trench shoot and Human Player.auto
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
{
"type": "path",
"data": {
"pathName": "LT to Human Player"
"pathName": "Red LT to Human Player"
}
},
{
Expand All @@ -37,7 +37,7 @@
{
"type": "path",
"data": {
"pathName": "Human Player to Tower"
"pathName": "Red Human Player to Tower"
}
},
{
Expand Down
70 changes: 31 additions & 39 deletions src/main/java/frc/robot/constants/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,62 +7,54 @@
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.NetworkTableInstance;

public final class Constants {

public static NetworkTableInstance NT_INSTANCE;

public final class VisionConstants {


public static String CAMERA_FR_NAME;
public static double CAMERA_FR_YAW;
public static double CAMERA_FR_PITCH;
public static double CAMERA_FR_ROLL;
public static double CAMERA_FR_FORWARD;
public static double CAMERA_FR_LEFT;
public static double CAMERA_FR_UP;

public static String CAMERA_FL_NAME;
public static double CAMERA_FL_YAW;
public static double CAMERA_FL_PITCH;
public static double CAMERA_FL_ROLL;
public static double CAMERA_FL_FORWARD;
public static double CAMERA_FL_LEFT;
public static double CAMERA_FL_UP;

public static String CAMERA_BR_NAME;
public static double CAMERA_BR_YAW;
public static double CAMERA_BR_PITCH;
public static double CAMERA_BR_ROLL;
public static double CAMERA_BR_FORWARD;
public static double CAMERA_BR_LEFT;
public static double CAMERA_BR_UP;
public static final NetworkTableInstance NT_INSTANCE = NetworkTableInstance.getDefault();

Comment on lines 16 to +19
Copy link

Copilot AI Apr 3, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

VisionConstants is declared as a non-static inner class, but it now contains non-compile-time static members (e.g., NT_INSTANCE, Matrix values). Java forbids static members in non-static inner classes, so this should be public static final class VisionConstants (and similarly for other nested constants classes that hold static non-constants).

Copilot uses AI. Check for mistakes.
public static final String CAMERA_FR_NAME = "CAMERA_FR";
public static final double CAMERA_FR_YAW = 0;
public static final double CAMERA_FR_PITCH = 28.812;
public static final double CAMERA_FR_ROLL = 0;
public static final double CAMERA_FR_FORWARD = Units.inchesToMeters(15.03);
public static final double CAMERA_FR_LEFT = Units.inchesToMeters(-1.475);
public static final double CAMERA_FR_UP = Units.inchesToMeters(20);

public static final String CAMERA_FL_NAME = "CAMERA_FL";
public static final double CAMERA_FL_YAW = 45;
public static final double CAMERA_FL_PITCH = 14.864;
public static final double CAMERA_FL_ROLL = 0;
public static final double CAMERA_FL_FORWARD = Units.inchesToMeters(9.893);
public static final double CAMERA_FL_LEFT = Units.inchesToMeters(-7.310);
public static final double CAMERA_FL_UP = Units.inchesToMeters(20);

public static final String CAMERA_BR_NAME = "CAMERA_BR";
public static final double CAMERA_BR_YAW = 0;
public static final double CAMERA_BR_PITCH = 0;
public static final double CAMERA_BR_ROLL = 0;
public static final double CAMERA_BR_FORWARD = 0;
public static final double CAMERA_BR_LEFT = 0;
public static final double CAMERA_BR_UP = 0;



// Trust Constants

// The standard deviations of our vision estimated poses, which affect correction rate
// (Fake values. Experiment and determine estimation noise on an actual robot.)
public static double MAX_VALID_DIST;
public static Matrix<N3, N1> kSINGLE_TAG_STD_DEVS;
public static Matrix<N3, N1> kMULTI_TAG_STD_DEVS;
public static Matrix<N3, N1> ODOM_STD_DEV;
public static final double MAX_VALID_DIST = 5.0; //Meters
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
public static final Matrix<N3, N1> ODOM_STD_DEV = VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(0.01));


}

public final class SwerveConstants {
public static Translation2d k_frontLeftLocation;
public static Translation2d k_frontRightLocation;
public static Translation2d k_backLeftLocation;
public static Translation2d k_backRightLocation;
}



public final class LauncherConstants {
public static double SHOOTER_SIZE_IN = 7.0;

Expand All @@ -77,4 +69,4 @@ public final class LauncherConstants {
public static double TOWER_HOOD_ANGLE = 1.25;
public static double TOWER_RPM = -2250.0;
}
}
}
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/subsystems/drivetrain/Swerve.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.constants.TunerConstants;
import frc.robot.constants.TunerConstants.TunerSwerveDrivetrain;
//import frc.robot.subsystems.localizer.Localizer;
import frc.robot.subsystems.localizer.Localizer;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
Expand Down Expand Up @@ -60,7 +60,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem {

private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();

//private Localizer localizer;
private Localizer localizer;

/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
Expand Down Expand Up @@ -157,7 +157,7 @@ public Swerve(
if (Utils.isSimulation()) {
startSimThread();
}
//localizer = new Localizer(this);
localizer = new Localizer(this);
// Configure AutoBuilder last
configureAutoBuilder();
}
Expand Down Expand Up @@ -217,7 +217,7 @@ public Swerve(
if (Utils.isSimulation()) {
startSimThread();
}
//localizer = new Localizer(this);
localizer = new Localizer(this);
configureAutoBuilder();
}

Expand Down Expand Up @@ -251,7 +251,7 @@ public Swerve(
if (Utils.isSimulation()) {
startSimThread();
}
//localizer = new Localizer(this);
localizer = new Localizer(this);
configureAutoBuilder();
}

Expand Down Expand Up @@ -309,7 +309,7 @@ public void periodic() {
});
}

//localizer.periodic();
localizer.periodic();
}

private void startSimThread() {
Expand Down
151 changes: 75 additions & 76 deletions src/main/java/frc/robot/subsystems/localizer/Localizer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,92 +1,91 @@
// package frc.robot.subsystems.localizer;

// import java.util.List;
// import java.util.Optional;


// import org.photonvision.EstimatedRobotPose;
// import org.photonvision.PhotonCamera;
// import org.photonvision.PhotonPoseEstimator;
// import org.photonvision.PhotonUtils;
// import org.photonvision.targeting.PhotonPipelineResult;
// import org.photonvision.targeting.PhotonTrackedTarget;

// import edu.wpi.first.apriltag.AprilTagFieldLayout;
// import edu.wpi.first.apriltag.AprilTagFields;
// import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
// import edu.wpi.first.math.geometry.*;
// import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
// import edu.wpi.first.math.kinematics.SwerveModulePosition;
// import edu.wpi.first.math.numbers.N1;
// import edu.wpi.first.math.numbers.N3;
// import edu.wpi.first.networktables.NetworkTable;
// import edu.wpi.first.networktables.NetworkTableInstance;
// import edu.wpi.first.networktables.StructPublisher;
// import edu.wpi.first.wpilibj.smartdashboard.Field2d;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj2.command.SubsystemBase;
// import edu.wpi.first.math.Matrix;
// import frc.robot.Robot;
// import frc.robot.constants.Constants;
// import frc.robot.subsystems.drivetrain.Swerve;
// import frc.robot.util.vision.Vision.BW;
// import frc.robot.util.vision.Vision.BW.BWCamera;
// import frc.robot.util.vision.Vision;

// public class Localizer extends SubsystemBase {

// private final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

// private final Vision vision;
// private final SwerveDrivePoseEstimator poseEstimator;
package frc.robot.subsystems.localizer;

import java.util.List;
import java.util.Optional;


import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonUtils;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.math.Matrix;
import frc.robot.Robot;
import frc.robot.constants.Constants;
import frc.robot.subsystems.drivetrain.Swerve;
import frc.robot.util.vision.Vision.BW;
import frc.robot.util.vision.Vision.BW.BWCamera;
import frc.robot.util.vision.Vision;

public class Localizer extends SubsystemBase {

private final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

Comment on lines +3 to +39
Copy link

Copilot AI Apr 3, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This file includes a large set of imports that are currently unused (e.g., PhotonVision types like EstimatedRobotPose, PhotonPoseEstimator, PhotonUtils, etc.). Please remove unused imports (and any unused helper fields) to keep the file focused and avoid potential lint/tooling failures.

Suggested change
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonUtils;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.math.Matrix;
import frc.robot.Robot;
import frc.robot.constants.Constants;
import frc.robot.subsystems.drivetrain.Swerve;
import frc.robot.util.vision.Vision.BW;
import frc.robot.util.vision.Vision.BW.BWCamera;
import frc.robot.util.vision.Vision;
public class Localizer extends SubsystemBase {
private final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
import frc.robot.subsystems.drivetrain.Swerve;
import frc.robot.util.vision.Vision;
public class Localizer extends SubsystemBase {

Copilot uses AI. Check for mistakes.
private final Vision vision;
private final SwerveDrivePoseEstimator poseEstimator;

// private final Swerve swerve;
private final Swerve swerve;

// public Field2d fieldOdom;
public Field2d fieldOdom;

// private Pose2d currentPose;
private Pose2d currentPose;

public Localizer(Swerve swerve) {

// public Localizer(Swerve swerve) {
currentPose = new Pose2d();
fieldOdom = new Field2d();

// currentPose = new Pose2d();
// fieldOdom = new Field2d();
this.swerve = swerve;

// this.swerve = swerve;

// poseEstimator = new SwerveDrivePoseEstimator(
// this.swerve.getKinematics(),
// this.swerve.getState().RawHeading,
// this.swerve.getState().ModulePositions,
// this.swerve.getState().Pose,
// Constants.VisionConstants.ODOM_STD_DEV,
// Constants.VisionConstants.kMULTI_TAG_STD_DEVS
// );
poseEstimator = new SwerveDrivePoseEstimator(
this.swerve.getKinematics(),
this.swerve.getState().RawHeading,
this.swerve.getState().ModulePositions,
this.swerve.getState().Pose,
Constants.VisionConstants.ODOM_STD_DEV,
Constants.VisionConstants.kMultiTagStdDevs
);

// vision = new Vision((pose, timestamp, stdDevs) -> {
// poseEstimator.addVisionMeasurement(pose, timestamp, stdDevs);
// });

// //TEMP, unsure if correct.
// SmartDashboard.putData("Field2d Pose", fieldOdom);
vision = new Vision((pose, timestamp, stdDevs) -> {
poseEstimator.addVisionMeasurement(pose, timestamp, stdDevs);
});
//SmartDashboard.putData("Field2d Pose", fieldOdom);

// }
}

// public void resetPose(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d newPose) {
// poseEstimator.resetPosition(gyroAngle, modulePositions, newPose);
// }
public void resetPose(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d newPose) {
poseEstimator.resetPosition(gyroAngle, modulePositions, newPose);
}

// public void updateOdometry(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
// poseEstimator.update(gyroAngle, modulePositions);
// }
public void updateOdometry(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
poseEstimator.update(gyroAngle, modulePositions);
}

Copy link

Copilot AI Apr 3, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

periodic() overrides SubsystemBase.periodic(), but it’s missing an @Override annotation. Adding @Override here will make the override explicit and catch signature mistakes at compile time (consistent with other subsystems in this repo).

Suggested change
@Override

Copilot uses AI. Check for mistakes.
// public void periodic() {
// currentPose = poseEstimator.getEstimatedPosition();
// vision.getEstimatedGlobalPoses(currentPose);
public void periodic() {
poseEstimator.update(this.swerve.getState().RawHeading, this.swerve.getState().ModulePositions);
vision.getEstimatedGlobalPoses(poseEstimator.getEstimatedPosition());
currentPose = poseEstimator.getEstimatedPosition();
Comment on lines +82 to +83
Copy link

Copilot AI Apr 3, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

vision.getEstimatedGlobalPoses(poseEstimator.getEstimatedPosition()) passes a reference pose, but Vision#getEstimatedGlobalPoses(Pose2d) currently does not use its referencePose parameter at all. Either wire the reference pose into the PhotonPoseEstimator (e.g., set the estimator's reference pose before estimating) or remove/rename the parameter/call to avoid a misleading API.

Suggested change
vision.getEstimatedGlobalPoses(poseEstimator.getEstimatedPosition());
currentPose = poseEstimator.getEstimatedPosition();
currentPose = poseEstimator.getEstimatedPosition();
// Vision#getEstimatedGlobalPoses(Pose2d) currently ignores its Pose2d argument,
// so avoid passing the estimator pose as though it were used as a reference pose.
vision.getEstimatedGlobalPoses(new Pose2d());

Copilot uses AI. Check for mistakes.
fieldOdom.setRobotPose(currentPose);
SmartDashboard.putData("Field2d Pose", fieldOdom);
Comment on lines +83 to +85
Copy link

Copilot AI Apr 3, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

SmartDashboard.putData("Field2d Pose", fieldOdom) is being called every periodic(). putData typically only needs to be called once (e.g., in the constructor); in periodic() you should only update the Field2d pose. Re-publishing every loop can add unnecessary NetworkTables overhead.

Copilot uses AI. Check for mistakes.

// fieldOdom.setRobotPose(currentPose);
// }
}


// }
}

Loading