Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added all 4 cameras

Browse Source
This commit is contained in:
Team2890HawkCollective
2026-02-20 17:09:46 -05:00
parent a50d67d7f5
commit 178359341c
8 changed files with 657 additions and 655 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -55,7 +55,7 @@ public class IntakeSubsystem extends SubsystemBase {
}
public void stopIntakeMotor() {
intakeWheelsMotor.set(0);
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity);
}
public Command startIntakeMotorCommand() {

6
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View File

@@ -96,7 +96,7 @@ public class ShooterSubsystem extends SubsystemBase {
public void setShooterMotorsRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
//rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
}
public double getShooterMotorRPM() {
@@ -127,10 +127,12 @@ public class ShooterSubsystem extends SubsystemBase {
public void stopShooters() {
centerShooterMotor.set(0);
leftShooterMotor.set(0);
rightShooterMotor.set(0);
//rightShooterMotor.set(0);
indexerAndRampMotor.set(0);
}
public Command stopShooterCommand() {

358
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
View File

@@ -1,182 +1,176 @@
package frc.robot.subsystems;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;
import com.pathplanner.lib.path.GoalEndState;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.path.PathPoint;
import com.pathplanner.lib.path.RotationTarget;
import com.pathplanner.lib.path.Waypoint;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import frc.robot.Constants;
import frc.robot.LimelightHelpers;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc.robot.RobotContainer;
import frc.robot.Constants;
public class TargetingSubsystems extends SubsystemBase {
PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera");
Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
BACK_LEFT_CAMERA_OFFSETS);
PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001);
public TargetingSubsystems() {
photonAimPIDController.enableContinuousInput(-180, 180);
}
Pose2d currentRobotPose;
public List<Waypoint> rightClimbWaypoints;
public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) {
GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
currentRobotPose = swerveDrive.getPose();
rightClimbWaypoints = PathPlannerPath.waypointsFromPoses(
currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE);
PathPlannerPath goToClimbPath = new PathPlannerPath(rightClimbWaypoints, goToClimbConstraints, null,
goalEndState);
goToClimbPath.preventFlipping = true;
return swerveDrive.getAutonomousCommand("goToClimbPath");
}
public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
return new RunCommand(() -> {
currentRobotPose = swerveDrive.getPose();
Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
double xError = errorFromDesiredPose.getX();
double yError = errorFromDesiredPose.getY();
double angleError = errorFromDesiredPose.getRotation().getRadians();
PIDController xController = new PIDController(1.5, 0, 0);
PIDController yController = new PIDController(1.5, 0, 0);
PIDController angleController = new PIDController(3.0, 0, 0);
angleController.enableContinuousInput(-Math.PI, Math.PI);
double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
desiredPose.getRotation().getRadians());
swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
}, swerveDrive);
}
Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
double rot = 0.0;
var result = photonVision.getLatestResult();
if (result.hasTargets()) {
double yawError = result.getBestTarget().getYaw();
rot = photonAimPIDController.calculate(yawError, 0);
}
rot = MathUtil.clamp(rot, -3.0, 3.0);
swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
driverXbox.getLeftX() * -1), rot, true);
}, swerveDrive);
}
public PhotonPoseEstimator getPhotonPoseEstimator() {
return photonEstimator;
}
// static public NetworkTable table =
// NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
// static public NetworkTableEntry ty = table.getEntry("ty");
// static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
// how many degrees back is your limelight rotated from perfectly vertical?
static double limelightMountAngleDegrees = 25.0;
// distance from the center of the Limelight lens to the floor
static double limelightLensHeightInches = 27.5;
// distance from the target to the floor
static double goalHeightInches = 44;
static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
// calculate distance
static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
/ Math.tan(angleToGoalRadians);
public static double getDistanceFromAprilTag() {
angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
/ Math.tan(angleToGoalRadians);
return distanceFromLimelightToGoalInches;
}
public void updateRobotPose(SwerveSubsystem swerveDrive) {
Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
if (result.isPresent()) {
EstimatedRobotPose estimatedPose = result.get();
swerveDrive.getSwerveDrive()
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
}
}
@Override
public void periodic() {
/*
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
* photonVision.getLatestResult().getBestTarget().getYaw());
* Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
* photonVision.getLatestResult().getBestTarget().getPitch());
* Shuffleboard.getTab("Vision").add("Limelight TX Value",
* LimelightHelpers.getTX("limelight"));
* Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
* LimelightHelpers.getFiducialID("limelight"));
* Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
* Shuffleboard.getTab("Vision").addCamera("Photon",
* "Arducam_OV9281_USB_Camera",
* "http://photonvision.local:5800");
*/
}
}
package frc.robot.subsystems;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;
import com.pathplanner.lib.path.GoalEndState;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.path.PathPoint;
import com.pathplanner.lib.path.RotationTarget;
import com.pathplanner.lib.path.Waypoint;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import frc.robot.Constants;
import frc.robot.LimelightHelpers;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc.robot.RobotContainer;
import frc.robot.Constants;
public class TargetingSubsystems extends SubsystemBase {
PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001);
public TargetingSubsystems() {
photonAimPIDController.enableContinuousInput(-180, 180);
}
Pose2d currentRobotPose;
public List<Waypoint> pathWaypoints;
public Command pathPlanToPoseCommand(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
GoalEndState goalEndState = new GoalEndState(0, desiredPose.getRotation());
PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
currentRobotPose = swerveDrive.getPose();
pathWaypoints = PathPlannerPath.waypointsFromPoses(
currentRobotPose, desiredPose);
PathPlannerPath goToDesiredPose = new PathPlannerPath(pathWaypoints, pathConstraints, null,
goalEndState);
goToDesiredPose.preventFlipping = true;
return swerveDrive.getAutonomousCommand("goToDesiredPose");
}
public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
return new RunCommand(() -> {
currentRobotPose = swerveDrive.getPose();
Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
double xError = errorFromDesiredPose.getX();
double yError = errorFromDesiredPose.getY();
double angleError = errorFromDesiredPose.getRotation().getRadians();
PIDController xController = new PIDController(1.5, 0, 0);
PIDController yController = new PIDController(1.5, 0, 0);
PIDController angleController = new PIDController(3.0, 0, 0);
angleController.enableContinuousInput(-Math.PI, Math.PI);
double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
desiredPose.getRotation().getRadians());
swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
}, swerveDrive);
}
Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
double rot = 0.0;
var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult();
if (result.hasTargets()) {
double yawError = result.getBestTarget().getYaw();
rot = photonAimPIDController.calculate(yawError, 0);
}
rot = MathUtil.clamp(rot, -3.0, 3.0);
swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
driverXbox.getLeftX() * -1), rot, true);
}, swerveDrive);
}
public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) {
return poseEstimator;
}
/* // static public NetworkTable table =
// NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
// static public NetworkTableEntry ty = table.getEntry("ty");
// static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
// how many degrees back is your limelight rotated from perfectly vertical?
static double limelightMountAngleDegrees = 25.0;
// distance from the center of the Limelight lens to the floor
static double limelightLensHeightInches = 27.5;
// distance from the target to the floor
static double goalHeightInches = 44;
static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
// calculate distance
static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
/ Math.tan(angleToGoalRadians);
public static double getDistanceFromAprilTag() {
angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
/ Math.tan(angleToGoalRadians);
return distanceFromLimelightToGoalInches;
}
*/
public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) {
Optional<EstimatedRobotPose> result = poseEstimator.update(camera.getLatestResult());
if (result.isPresent()) {
EstimatedRobotPose estimatedPose = result.get();
swerveDrive.getSwerveDrive()
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
}
}
@Override
public void periodic() {
/*
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
* photonVision.getLatestResult().getBestTarget().getYaw());
* Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
* photonVision.getLatestResult().getBestTarget().getPitch());
* Shuffleboard.getTab("Vision").add("Limelight TX Value",
* LimelightHelpers.getTX("limelight"));
* Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
* LimelightHelpers.getFiducialID("limelight"));
* Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
* Shuffleboard.getTab("Vision").addCamera("Photon",
* "Arducam_OV9281_USB_Camera",
* "http://photonvision.local:5800");
*/
}
}

2
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View File

@@ -62,7 +62,7 @@ public class SwerveSubsystem extends SubsystemBase {
/**
* Enable vision odometry updates while driving.
*/
private final boolean visionDriveTest = false;
private final boolean visionDriveTest = true;
/**
* PhotonVision class to keep an accurate odometry.

431
src/main/java/frc/robot/subsystems/swervedrive/Vision.java
View File

@@ -40,59 +40,57 @@ import org.photonvision.targeting.PhotonTrackedTarget;
import swervelib.SwerveDrive;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from
* Example PhotonVision class to aid in the pursuit of accurate odometry. Taken
* from
* https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads
*/
public class Vision
{
public class Vision {
/**
* April Tag Field Layout of the year.
*/
public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
AprilTagFields.k2026RebuiltAndymark);
/**
* Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
* Ambiguity defined as a value between (0,1). Used in
* {@link Vision#filterPose}.
*/
private final double maximumAmbiguity = 0.25;
private final double maximumAmbiguity = 0.25;
/**
* Photon Vision Simulation
*/
public VisionSystemSim visionSim;
public VisionSystemSim visionSim;
/**
* Count of times that the odom thinks we're more than 10meters away from the april tag.
* Count of times that the odom thinks we're more than 10meters away from the
* april tag.
*/
private double longDistangePoseEstimationCount = 0;
private double longDistangePoseEstimationCount = 0;
/**
* Current pose from the pose estimator using wheel odometry.
*/
private Supplier<Pose2d> currentPose;
private Supplier<Pose2d> currentPose;
/**
* Field from {@link swervelib.SwerveDrive#field}
*/
private Field2d field2d;
private Field2d field2d;
/**
* Constructor for the Vision class.
*
* @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()}
* @param currentPose Current pose supplier, should reference
* {@link SwerveDrive#getPose()}
* @param field Current field, should be {@link SwerveDrive#field}
*/
public Vision(Supplier<Pose2d> currentPose, Field2d field)
{
public Vision(Supplier<Pose2d> currentPose, Field2d field) {
this.currentPose = currentPose;
this.field2d = field;
if (Robot.isSimulation())
{
if (Robot.isSimulation()) {
visionSim = new VisionSystemSim("Vision");
visionSim.addAprilTags(fieldLayout);
for (Cameras c : Cameras.values())
{
for (Cameras c : Cameras.values()) {
c.addToVisionSim(visionSim);
}
@@ -104,50 +102,48 @@ public class Vision
* Calculates a target pose relative to an AprilTag on the field.
*
* @param aprilTag The ID of the AprilTag.
* @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position
* @param robotOffset The offset {@link Transform2d} of the robot to apply to
* the pose for the robot to position
* itself correctly.
* @return The target pose of the AprilTag.
*/
public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
{
public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) {
Optional<Pose3d> aprilTagPose3d = fieldLayout.getTagPose(aprilTag);
if (aprilTagPose3d.isPresent())
{
if (aprilTagPose3d.isPresent()) {
return aprilTagPose3d.get().toPose2d().transformBy(robotOffset);
} else
{
} else {
throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString());
}
}
/**
* Update the pose estimation inside of {@link SwerveDrive} with all of the given poses.
* Update the pose estimation inside of {@link SwerveDrive} with all of the
* given poses.
*
* @param swerveDrive {@link SwerveDrive} instance.
*/
public void updatePoseEstimation(SwerveDrive swerveDrive)
{
if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent())
{
public void updatePoseEstimation(SwerveDrive swerveDrive) {
if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) {
/*
* In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting.
* In the maple-sim, odometry is simulated using encoder values, accounting for
* factors like skidding and drifting.
* As a result, the odometry may not always be 100% accurate.
* However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect.
* However, the vision system should be able to provide a reasonably accurate
* pose estimation, even when odometry is incorrect.
* (This is why teams implement vision system to correct odometry.)
* Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation.
* Therefore, we must ensure that the actual robot pose is provided in the
* simulator when updating the vision simulation during the simulation.
*/
visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
}
for (Cameras camera : Cameras.values())
{
for (Cameras camera : Cameras.values()) {
Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
if (poseEst.isPresent())
{
if (poseEst.isPresent()) {
var pose = poseEst.get();
swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(),
pose.timestampSeconds,
camera.curStdDevs);
pose.timestampSeconds,
camera.curStdDevs);
}
}
@@ -156,24 +152,22 @@ public class Vision
/**
* Generates the estimated robot pose. Returns empty if:
* <ul>
* <li> No Pose Estimates could be generated</li>
* <li> The generated pose estimate was considered not accurate</li>
* <li>No Pose Estimates could be generated</li>
* <li>The generated pose estimate was considered not accurate</li>
* </ul>
*
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and
* targets used to create the estimate
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
{
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera) {
Optional<EstimatedRobotPose> poseEst = camera.getEstimatedGlobalPose();
if (Robot.isSimulation())
{
if (Robot.isSimulation()) {
Field2d debugField = visionSim.getDebugField();
// Uncomment to enable outputting of vision targets in sim.
poseEst.ifPresentOrElse(
est ->
debugField
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
est -> debugField
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
debugField.getObject("VisionEstimation").setPoses();
});
@@ -181,46 +175,39 @@ public class Vision
return poseEst;
}
/**
* Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than
* Filter pose via the ambiguity and find best estimate between all of the
* camera's throwing out distances more than
* 10m for a short amount of time.
*
* @param pose Estimated robot pose.
* @return Could be empty if there isn't a good reading.
*/
@Deprecated(since = "2024", forRemoval = true)
private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose)
{
if (pose.isPresent())
{
private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose) {
if (pose.isPresent()) {
double bestTargetAmbiguity = 1; // 1 is max ambiguity
for (PhotonTrackedTarget target : pose.get().targetsUsed)
{
for (PhotonTrackedTarget target : pose.get().targetsUsed) {
double ambiguity = target.getPoseAmbiguity();
if (ambiguity != -1 && ambiguity < bestTargetAmbiguity)
{
if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) {
bestTargetAmbiguity = ambiguity;
}
}
//ambiguity to high dont use estimate
if (bestTargetAmbiguity > maximumAmbiguity)
{
// ambiguity to high dont use estimate
if (bestTargetAmbiguity > maximumAmbiguity) {
return Optional.empty();
}
//est pose is very far from recorded robot pose
if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1)
{
// est pose is very far from recorded robot pose
if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) {
longDistangePoseEstimationCount++;
//if it calculates that were 10 meter away for more than 10 times in a row its probably right
if (longDistangePoseEstimationCount < 10)
{
// if it calculates that were 10 meter away for more than 10 times in a row its
// probably right
if (longDistangePoseEstimationCount < 10) {
return Optional.empty();
}
} else
{
} else {
longDistangePoseEstimationCount = 0;
}
return pose;
@@ -228,15 +215,13 @@ public class Vision
return Optional.empty();
}
/**
* Get distance of the robot from the AprilTag pose.
*
* @param id AprilTag ID
* @return Distance
*/
public double getDistanceFromAprilTag(int id)
{
public double getDistanceFromAprilTag(int id) {
Optional<Pose3d> tag = fieldLayout.getTagPose(id);
return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
}
@@ -248,17 +233,12 @@ public class Vision
* @param camera Camera to check.
* @return Tracked target.
*/
public PhotonTrackedTarget getTargetFromId(int id, Cameras camera)
{
public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) {
PhotonTrackedTarget target = null;
for (PhotonPipelineResult result : camera.resultsList)
{
if (result.hasTargets())
{
for (PhotonTrackedTarget i : result.getTargets())
{
if (i.getFiducialId() == id)
{
for (PhotonPipelineResult result : camera.resultsList) {
if (result.hasTargets()) {
for (PhotonTrackedTarget i : result.getTargets()) {
if (i.getFiducialId() == id) {
return i;
}
}
@@ -273,54 +253,46 @@ public class Vision
*
* @return Vision Simulation
*/
public VisionSystemSim getVisionSim()
{
public VisionSystemSim getVisionSim() {
return visionSim;
}
/**
* Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost.
* Open up the photon vision camera streams on the localhost, assumes running
* photon vision on localhost.
*/
private void openSimCameraViews()
{
if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
{
// try
// {
// Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
// Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
// Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
// } catch (IOException | URISyntaxException e)
// {
// e.printStackTrace();
// }
private void openSimCameraViews() {
if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) {
// try
// {
// Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
// Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
// Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
// } catch (IOException | URISyntaxException e)
// {
// e.printStackTrace();
// }
}
}
/**
* Update the {@link Field2d} to include tracked targets/
*/
public void updateVisionField()
{
public void updateVisionField() {
List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
for (Cameras c : Cameras.values())
{
if (!c.resultsList.isEmpty())
{
for (Cameras c : Cameras.values()) {
if (!c.resultsList.isEmpty()) {
PhotonPipelineResult latest = c.resultsList.get(0);
if (latest.hasTargets())
{
if (latest.hasTargets()) {
targets.addAll(latest.targets);
}
}
}
List<Pose2d> poses = new ArrayList<>();
for (PhotonTrackedTarget target : targets)
{
if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
{
for (PhotonTrackedTarget target : targets) {
if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) {
Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
poses.add(targetPose);
}
@@ -332,95 +304,100 @@ public class Vision
/**
* Camera Enum to select each camera
*/
enum Cameras
{
enum Cameras {
/**
* Left Camera
*/
LEFT_CAM("left",
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(10.981),
Units.inchesToMeters(8.44)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(10.981),
Units.inchesToMeters(8.44)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
/**
* Right Camera
*/
RIGHT_CAM("right",
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(-10.981),
Units.inchesToMeters(8.44)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(-10.981),
Units.inchesToMeters(8.44)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
/**
* Center Camera
*/
CENTER_CAM("center",
new Rotation3d(0, Units.degreesToRadians(18), 0),
new Translation3d(Units.inchesToMeters(-4.628),
Units.inchesToMeters(-10.687),
Units.inchesToMeters(16.129)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
new Rotation3d(0, Units.degreesToRadians(18), 0),
new Translation3d(Units.inchesToMeters(-4.628),
Units.inchesToMeters(-10.687),
Units.inchesToMeters(16.129)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
/**
* Latency alert to use when high latency is detected.
*/
public final Alert latencyAlert;
public final Alert latencyAlert;
/**
* Camera instance for comms.
*/
public final PhotonCamera camera;
public final PhotonCamera camera;
/**
* Pose estimator for camera.
*/
public final PhotonPoseEstimator poseEstimator;
public final PhotonPoseEstimator poseEstimator;
/**
* Standard Deviation for single tag readings for pose estimation.
*/
private final Matrix<N3, N1> singleTagStdDevs;
private final Matrix<N3, N1> singleTagStdDevs;
/**
* Standard deviation for multi-tag readings for pose estimation.
*/
private final Matrix<N3, N1> multiTagStdDevs;
private final Matrix<N3, N1> multiTagStdDevs;
/**
* Transform of the camera rotation and translation relative to the center of the robot
* Transform of the camera rotation and translation relative to the center of
* the robot
*/
private final Transform3d robotToCamTransform;
private final Transform3d robotToCamTransform;
/**
* Current standard deviations used.
*/
public Matrix<N3, N1> curStdDevs;
public Matrix<N3, N1> curStdDevs;
/**
* Estimated robot pose.
*/
public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
/**
* Simulated camera instance which only exists during simulations.
*/
public PhotonCameraSim cameraSim;
public PhotonCameraSim cameraSim;
/**
* Results list to be updated periodically and cached to avoid unnecessary queries.
* Results list to be updated periodically and cached to avoid unnecessary
* queries.
*/
public List<PhotonPipelineResult> resultsList = new ArrayList<>();
public List<PhotonPipelineResult> resultsList = new ArrayList<>();
/**
* Last read from the camera timestamp to prevent lag due to slow data fetches.
*/
private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
/**
* Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine
* Construct a Photon Camera class with help. Standard deviations are fake
* values, experiment and determine
* estimation noise on an actual robot.
*
* @param name Name of the PhotonVision camera found in the PV UI.
* @param name Name of the PhotonVision camera found in the PV
* UI.
* @param robotToCamRotation {@link Rotation3d} of the camera.
* @param robotToCamTranslation {@link Translation3d} relative to the center of the robot.
* @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera.
* @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera.
* @param robotToCamTranslation {@link Translation3d} relative to the center of
* the robot.
* @param singleTagStdDevs Single AprilTag standard deviations of estimated
* poses from the camera.
* @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated
* poses from the camera.
*/
Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation,
Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix)
{
Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix) {
latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning);
camera = new PhotonCamera(name);
@@ -429,21 +406,22 @@ public class Vision
robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation);
poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
robotToCamTransform);
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
robotToCamTransform);
poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
this.singleTagStdDevs = singleTagStdDevs;
this.multiTagStdDevs = multiTagStdDevsMatrix;
if (Robot.isSimulation())
{
if (Robot.isSimulation()) {
SimCameraProperties cameraProp = new SimCameraProperties();
// A 640 x 480 camera with a 100 degree diagonal FOV.
cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100));
// Approximate detection noise with average and standard deviation error in pixels.
// 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).
// Set the camera image capture framerate (Note: this is limited by robot loop
// rate).
cameraProp.setFPS(30);
// The average and standard deviation in milliseconds of image data latency.
cameraProp.setAvgLatencyMs(35);
@@ -459,35 +437,31 @@ public class Vision
*
* @param systemSim {@link VisionSystemSim} to use.
*/
public void addToVisionSim(VisionSystemSim systemSim)
{
if (Robot.isSimulation())
{
public void addToVisionSim(VisionSystemSim systemSim) {
if (Robot.isSimulation()) {
systemSim.addCamera(cameraSim, robotToCamTransform);
}
}
/**
* Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most
* Get the result with the least ambiguity from the best tracked target within
* the Cache. This may not be the most
* recent result!
*
* @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result!
* @return The result in the cache with the least ambiguous best tracked target.
* This is not the most recent result!
*/
public Optional<PhotonPipelineResult> getBestResult()
{
if (resultsList.isEmpty())
{
public Optional<PhotonPipelineResult> getBestResult() {
if (resultsList.isEmpty()) {
return Optional.empty();
}
PhotonPipelineResult bestResult = resultsList.get(0);
double amiguity = bestResult.getBestTarget().getPoseAmbiguity();
double currentAmbiguity = 0;
for (PhotonPipelineResult result : resultsList)
{
PhotonPipelineResult bestResult = resultsList.get(0);
double amiguity = bestResult.getBestTarget().getPoseAmbiguity();
double currentAmbiguity = 0;
for (PhotonPipelineResult result : resultsList) {
currentAmbiguity = result.getBestTarget().getPoseAmbiguity();
if (currentAmbiguity < amiguity && currentAmbiguity > 0)
{
if (currentAmbiguity < amiguity && currentAmbiguity > 0) {
bestResult = result;
amiguity = currentAmbiguity;
}
@@ -498,63 +472,63 @@ public class Vision
/**
* Get the latest result from the current cache.
*
* @return Empty optional if nothing is found. Latest result if something is there.
* @return Empty optional if nothing is found. Latest result if something is
* there.
*/
public Optional<PhotonPipelineResult> getLatestResult()
{
public Optional<PhotonPipelineResult> getLatestResult() {
return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0));
}
/**
* Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the
* Get the estimated robot pose. Updates the current robot pose estimation,
* standard deviations, and flushes the
* cache of results.
*
* @return Estimated pose.
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose()
{
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
updateUnreadResults();
return estimatedRobotPose;
}
/**
* Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
* Update the latest results, cached with a maximum refresh rate of 1req/15ms.
* Sorts the list by timestamp.
*/
private void updateUnreadResults()
{
private void updateUnreadResults() {
double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
for (PhotonPipelineResult result : resultsList)
{
for (PhotonPipelineResult result : resultsList) {
mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
}
resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
});
if (!resultsList.isEmpty())
{
updateEstimatedGlobalPose();
}
resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
});
if (!resultsList.isEmpty()) {
updateEstimatedGlobalPose();
}
}
/**
* The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once
* The latest estimated robot pose on the field from vision data. This may be
* empty. This should only be called once
* per loop.
*
* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
* <p>
* Also includes updates for the standard deviations, which can (optionally) be
* retrieved with
* {@link Cameras#updateEstimationStdDevs}
*
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
* estimation.
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate
* timestamp, and targets used for
* estimation.
*/
private void updateEstimatedGlobalPose()
{
private void updateEstimatedGlobalPose() {
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : resultsList)
{
for (var change : resultsList) {
visionEst = poseEstimator.update(change);
updateEstimationStdDevs(visionEst, change.getTargets());
}
@@ -562,63 +536,54 @@ public class Vision
}
/**
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based
* Calculates new standard deviations This algorithm is a heuristic that creates
* dynamic standard deviations based
* on number of tags, estimation strategy, and distance from the tags.
*
* @param estimatedPose The estimated pose to guess standard deviations for.
* @param targets All targets in this camera frame
*/
private void updateEstimationStdDevs(
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets)
{
if (estimatedPose.isEmpty())
{
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
if (estimatedPose.isEmpty()) {
// No pose input. Default to single-tag std devs
curStdDevs = singleTagStdDevs;
} else
{
} else {
// Pose present. Start running Heuristic
var estStdDevs = singleTagStdDevs;
int numTags = 0;
double avgDist = 0;
var estStdDevs = singleTagStdDevs;
int numTags = 0;
double avgDist = 0;
// Precalculation - see how many tags we found, and calculate an average-distance metric
for (var tgt : targets)
{
// Precalculation - see how many tags we found, and calculate an
// average-distance metric
for (var tgt : targets) {
var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
if (tagPose.isEmpty())
{
if (tagPose.isEmpty()) {
continue;
}
numTags++;
avgDist +=
tagPose
.get()
.toPose2d()
.getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
avgDist += tagPose
.get()
.toPose2d()
.getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
}
if (numTags == 0)
{
if (numTags == 0) {
// No tags visible. Default to single-tag std devs
curStdDevs = singleTagStdDevs;
} else
{
} else {
// One or more tags visible, run the full heuristic.
avgDist /= numTags;
// Decrease std devs if multiple targets are visible
if (numTags > 1)
{
if (numTags > 1) {
estStdDevs = multiTagStdDevs;
}
// Increase std devs based on (average) distance
if (numTags == 1 && avgDist > 4)
{
if (numTags == 1 && avgDist > 4) {
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
} else
{
} else {
estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
}
curStdDevs = estStdDevs;