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

63
src/main/java/frc/robot/Constants.java
View File

@@ -4,11 +4,16 @@
package frc.robot;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
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.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.GenericEntry;
@@ -90,13 +95,6 @@ public final class Constants {
public static void getIndexerAndRampMotorRPM() {
INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(1000);
}
public static final int LEFT_ACTUATOR_PWM_PORT = 1;
public static final int RIGHT_ACTUATOR_PWM_PORT = 3;
public static double DESIRED_POTENTIOMETER_DISTANCE;
// TO DO: need to equation that calculates desired potentiometer distance
}
public static class IntakeConstants {
@@ -111,7 +109,7 @@ public final class Constants {
public static final int INTAKE_WHEELS_MOTOR_ID = 50;
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = 1.2550222873687744;
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
public static class IntakeRotatorPID {
@@ -131,34 +129,41 @@ public final class Constants {
}
public static class LimeLight {
public static final String LIMELIGHT_NAME = "limelight";
// public static final int[] ALL_REEF_APRILTAGS = { 6, 7, 8, 9, 10, 11, 17, 18,
// 19, 20, 21, 22 };
public static final AprilTagFieldLayout APRILTAG_FIELD_LAYOUT = AprilTagFieldLayout
.loadField(AprilTagFields.k2026RebuiltAndymark);
public static final double BUMPER_WIDTH = Units.inchesToMeters(0.0); // Get This Value // Original: 2.75
// public static final double ROBOT_WIDTH = Units.inchesToMeters(30 +
// BUMPER_WIDTH); // Tis a square, don't need this
public static final double ROBOT_SIDE_LENGTH = Units.inchesToMeters(27);
// public static final Transform2d HALF_ROBOT = new
// Transform2d(ROBOT_SIDE_LENGTH / 3.0, 0, new Rotation2d());
public static double LIMELIGHT_TY;
}
public static class TargetingConstants {
public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
ORANGE_ROBOT_TO_CAM);
public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
BLACK_ROBOT_TO_CAM);
public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
RED_ROBOT_TO_CAM);
public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
PURPLE_ROBOT_TO_CAM);
}
public static class ClimberConstants {
public static final int CLIMB_MOTOR_ID = 60;
public static final int RATCHET_PWM_PORT = 0;
public static final int RATCHET_PWM_PORT = 9;
public static final double RATCHET_UNLOCK_ANGLE = 0;
public static final double RATCHET_LOCK_ANGLE = 180;

13
src/main/java/frc/robot/Robot.java
View File

@@ -89,11 +89,20 @@ public class Robot extends TimedRobot {
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
// Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
// Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
Constants.IntakeConstants.getIntakeWheelsSpeed();
Constants.ShooterConstants.getShooterVelocity();
Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
//distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
// distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM,
Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM,
Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM,
Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM,
Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
}
/**

437
src/main/java/frc/robot/RobotContainer.java
View File

@@ -33,6 +33,9 @@ import java.io.File;
import java.lang.annotation.Target;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.function.DoubleSupplier;
import javax.lang.model.util.ElementScanner14;
import frc.robot.subsystems.TargetingSubsystems;
import swervelib.SwerveInputStream;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -48,230 +51,254 @@ import frc.robot.subsystems.ClimberSubsystem;
*/
public class RobotContainer {
// Replace with CommandPS4Controller or CommandJoystick if needed
final CommandXboxController driverXbox = new CommandXboxController(0);
// The robot's subsystems and commands are defined here...
private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
"swerve/neo"));
// Replace with CommandPS4Controller or CommandJoystick if needed
final CommandXboxController driverXbox = new CommandXboxController(0);
// The robot's subsystems and commands are defined here...
private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
"swerve/neo"));
// Establish a Sendable Chooser that will be able to be sent to the
// SmartDashboard, allowing selection of desired auto
private final SendableChooser<Command> autoChooser;
// Establish a Sendable Chooser that will be able to be sent to the
// SmartDashboard, allowing selection of desired auto
private final SendableChooser<Command> autoChooser;
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
/**
* Converts driver input into a field-relative ChassisSpeeds that is controlled
* by angular velocity.
*/
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> driverXbox.getLeftY() * -1,
() -> driverXbox.getLeftX() * -1)
.withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
/**
* Converts driver input into a field-relative ChassisSpeeds that is controlled
* by angular velocity.
*/
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> driverXbox.getLeftY() * -1,
() -> driverXbox.getLeftX() * -1)
.withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
/**
* Clone's the angular velocity input stream and converts it to a fieldRelative
* input stream.
*/
SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX,
driverXbox::getRightY)
.headingWhile(true);
/**
* Clone's the angular velocity input stream and converts it to a fieldRelative
* input stream.
*/
SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()
.withControllerHeadingAxis(driverXbox::getRightX,
driverXbox::getRightY)
.headingWhile(true);
/**
* Clone's the angular velocity input stream and converts it to a robotRelative
* input stream.
*/
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
.allianceRelativeControl(true);
/**
* Clone's the angular velocity input stream and converts it to a robotRelative
* input stream.
*/
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
.allianceRelativeControl(true);
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> -driverXbox.getLeftY(),
() -> -driverXbox.getLeftX())
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
2))
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
// Derive the heading axis with math!
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
.withControllerHeadingAxis(() -> Math.sin(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2),
() -> Math.cos(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2))
.headingWhile(true)
.translationHeadingOffset(true)
.translationHeadingOffset(Rotation2d.fromDegrees(
0));
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> -driverXbox.getLeftY(),
() -> -driverXbox.getLeftX())
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
2))
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
// Derive the heading axis with math!
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
.withControllerHeadingAxis(() -> Math.sin(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2),
() -> Math.cos(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2))
.headingWhile(true)
.translationHeadingOffset(true)
.translationHeadingOffset(Rotation2d.fromDegrees(
0));
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
DriverStation.silenceJoystickConnectionWarning(true);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
DriverStation.silenceJoystickConnectionWarning(true);
// Create the NamedCommands that will be used in PathPlanner
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
// Create the NamedCommands that will be used in PathPlanner
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
// Set the default auto (do nothing)
autoChooser.setDefaultOption("Do Nothing", Commands.none());
// Set the default auto (do nothing)
autoChooser.setDefaultOption("Do Nothing", Commands.none());
// Add a simple auto option to have the robot drive forward for 1 second then
// stop
autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
// Add a simple auto option to have the robot drive forward for 1 second then
// stop
autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
// Put the autoChooser on the SmartDashboard
SmartDashboard.putData("Auto Chooser", autoChooser);
}
/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
* an arbitrary predicate, or via the
* named factories in
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
* for
* {@link CommandXboxController
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
* Flight joysticks}.
*/
private void configureBindings() {
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngle);
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngleKeyboard);
driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand()));
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
// () -> -driverXbox.getLeftX()));
if (driverXbox.getRightY() < -0.4) {
m_ClimberSubsystem.liftRobotCommand();
} else if (driverXbox.getRightY() > 0.4) {
m_ClimberSubsystem.lowerRobotCommand();
} else {
m_ClimberSubsystem.stopClimberCommand();
}
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
if (Robot.isSimulation()) {
Pose2d target = new Pose2d(new Translation2d(1, 4),
Rotation2d.fromDegrees(90));
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
driveDirectAngleKeyboard.driveToPose(() -> target,
new ProfiledPIDController(5,
0,
0,
new Constraints(5, 2)),
new ProfiledPIDController(5,
0,
0,
new Constraints(Units.degreesToRadians(360),
Units.degreesToRadians(180))));
driverXbox.start()
.onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
// driverXbox.b().whileTrue(
// drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
// );
// Put the autoChooser on the SmartDashboard
SmartDashboard.putData("Auto Chooser", autoChooser);
}
if (DriverStation.isTest()) {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above!
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
driverXbox.leftBumper().onTrue(Commands.none());
driverXbox.rightBumper().onTrue(Commands.none());
} else {
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.start().whileTrue(Commands.none());
driverXbox.back().whileTrue(Commands.none());
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.rightBumper().onTrue(Commands.none());
/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
* an arbitrary predicate, or via the
* named factories in
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
* for
* {@link CommandXboxController
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
* Flight joysticks}.
*/
private void configureBindings() {
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngle);
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
.driveFieldOriented(driveAngularVelocityKeyboard);
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngleKeyboard);
driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand())
.onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand())
.onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
driverXbox.rightStick().onTrue(climbCommand());
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand()
.andThen(m_IntakeSubsystem.deployintakeCommand()));
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
// () -> -driverXbox.getLeftX()));
if (driverXbox.getRightY() < -0.4) {
m_ClimberSubsystem.liftRobotCommand();
} else if (driverXbox.getRightY() > 0.4) {
m_ClimberSubsystem.lowerRobotCommand();
} else {
m_ClimberSubsystem.stopClimberCommand();
}
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
if (Robot.isSimulation()) {
Pose2d target = new Pose2d(new Translation2d(1, 4),
Rotation2d.fromDegrees(90));
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
driveDirectAngleKeyboard.driveToPose(() -> target,
new ProfiledPIDController(5,
0,
0,
new Constraints(5, 2)),
new ProfiledPIDController(5,
0,
0,
new Constraints(Units.degreesToRadians(360),
Units.degreesToRadians(180))));
driverXbox.start()
.onTrue(Commands.runOnce(() -> drivebase
.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
driverXbox.button(2)
.whileTrue(Commands.runEnd(
() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
// driverXbox.b().whileTrue(
// drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
// );
}
if (DriverStation.isTest()) {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
// above!
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
driverXbox.leftBumper().onTrue(Commands.none());
driverXbox.rightBumper().onTrue(Commands.none());
} else {
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.start().whileTrue(Commands.none());
driverXbox.back().whileTrue(Commands.none());
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.rightBumper().onTrue(Commands.none());
}
}
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Pass in the selected auto from the SmartDashboard as our desired autnomous
// commmand
return autoChooser.getSelected();
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Pass in the selected auto from the SmartDashboard as our desired autnomous
// commmand
return autoChooser.getSelected();
}
public void setMotorBrake(boolean brake) {
drivebase.setMotorBrake(brake);
}
public void setMotorBrake(boolean brake) {
drivebase.setMotorBrake(brake);
}
public SwerveSubsystem getSwerveDrive() {
return drivebase;
}
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
public Command climbCommand() {
if (driverXbox.getRightY() > -0.5) {
return m_ClimberSubsystem.lowerRobotCommand();
} else if (driverXbox.getRightX() < 0.5) {
return m_ClimberSubsystem.liftRobotCommand();
} else
return m_ClimberSubsystem.stopClimberCommand();
}
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_ShooterSubsystem.shootFuelCommand(),
m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
}

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;