PLS5618/MapleSimExample
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Change to YALL

Browse Source
This commit is contained in:
Olivier Demers
2026-01-15 16:40:47 -05:00
parent 655efca537
commit 18fbd0deb3
7 changed files with 234 additions and 1767 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1589
src/main/java/frc/robot/LimelightHelpers.java
View File

File diff suppressed because it is too large Load Diff

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

@@ -4,9 +4,6 @@
package frc.robot;
import com.ctre.phoenix6.HootAutoReplay;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -16,41 +13,13 @@ public class Robot extends TimedRobot {
private final RobotContainer m_robotContainer;
/* log and replay timestamp and joystick data */
private final HootAutoReplay m_timeAndJoystickReplay = new HootAutoReplay()
.withTimestampReplay()
.withJoystickReplay();
private final boolean kUseLimelight = false;
public Robot() {
m_robotContainer = new RobotContainer();
}
@Override
public void robotPeriodic() {
m_timeAndJoystickReplay.update();
CommandScheduler.getInstance().run();
/*
* This example of adding Limelight is very simple and may not be sufficient for on-field use.
* Users typically need to provide a standard deviation that scales with the distance to target
* and changes with number of tags available.
*
* This example is sufficient to show that vision integration is possible, though exact implementation
* of how to use vision should be tuned per-robot and to the team's specification.
*/
if (kUseLimelight) {
var driveState = m_robotContainer.drivetrain.getState();
double headingDeg = driveState.Pose.getRotation().getDegrees();
double omegaRps = Units.radiansToRotations(driveState.Speeds.omegaRadiansPerSecond);
LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0);
var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
}
}
}
@Override

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

@@ -4,11 +4,12 @@
package frc.robot;
import static edu.wpi.first.units.Units.*;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.FollowPathCommand;
@@ -19,13 +20,15 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.AprilTagsLimelight;
import frc.robot.subsystems.CommandSwerveDrivetrain;
public class RobotContainer {
private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top
// speed
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second
// max angular velocity
/* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
@@ -41,6 +44,7 @@ public class RobotContainer {
private final CommandXboxController joystick = new CommandXboxController(0);
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
public final AprilTagsLimelight aprilTagsLimelight = new AprilTagsLimelight(drivetrain);
/* Path follower */
private final SendableChooser<Command> autoChooser;
@@ -59,32 +63,24 @@ public class RobotContainer {
// Note that X is defined as forward according to WPILib convention,
// and Y is defined as to the left according to WPILib convention.
drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically
drivetrain.applyRequest(() ->
drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
.withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left)
)
);
// Drivetrain will execute this command periodically
drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed)
.withVelocityY(-joystick.getLeftX() * MaxSpeed)
.withRotationalRate(-joystick.getRightX() * MaxAngularRate)));
// Idle while the robot is disabled. This ensures the configured
// neutral mode is applied to the drive motors while disabled.
final var idle = new SwerveRequest.Idle();
RobotModeTriggers.disabled().whileTrue(
drivetrain.applyRequest(() -> idle).ignoringDisable(true)
);
drivetrain.applyRequest(() -> idle).ignoringDisable(true));
joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
joystick.b().whileTrue(drivetrain.applyRequest(() ->
point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))
));
joystick.b().whileTrue(drivetrain.applyRequest(
() -> point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
joystick.povUp().whileTrue(drivetrain.applyRequest(() ->
forwardStraight.withVelocityX(0.5).withVelocityY(0))
);
joystick.povDown().whileTrue(drivetrain.applyRequest(() ->
forwardStraight.withVelocityX(-0.5).withVelocityY(0))
);
joystick.povUp().whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
joystick.povDown()
.whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
// Run SysId routines when holding back/start and X/Y.
// Note that each routine should be run exactly once in a single log.

7
src/main/java/frc/robot/Vision.java Normal file
View File

@@ -0,0 +1,7 @@
package frc.robot;
import limelight.Limelight;
public class Vision {
public final Limelight limelight = new Limelight("AprilTags");
}

33
src/main/java/frc/robot/subsystems/AprilTagsLimelight.java Normal file
View File

@@ -0,0 +1,33 @@
package frc.robot.subsystems;
import java.util.Optional;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import limelight.Limelight;
import limelight.networktables.LimelightPoseEstimator;
import limelight.networktables.LimelightPoseEstimator.EstimationMode;
import limelight.networktables.PoseEstimate;
public class AprilTagsLimelight extends SubsystemBase {
private CommandSwerveDrivetrain drivetrain;
private final Limelight limelight = new Limelight("AprilTags");
private final LimelightPoseEstimator poseEstimator = limelight.createPoseEstimator(EstimationMode.MEGATAG2);;
public AprilTagsLimelight(CommandSwerveDrivetrain drivetrain) {
this.drivetrain = drivetrain;
}
@Override
public void periodic() {
limelight.getSettings()
.withRobotOrientation(drivetrain.getOrientation3d())
.save();
Optional<PoseEstimate> visionEstimate = poseEstimator.getPoseEstimate();
visionEstimate.ifPresent((PoseEstimate poseEstimate) -> {
drivetrain.addVisionMeasurement(poseEstimate.pose.toPose2d(), poseEstimate.timestampSeconds);
});
}
}

279
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -1,6 +1,7 @@
package frc.robot.subsystems;
import static edu.wpi.first.units.Units.*;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Volts;
import java.util.Optional;
import java.util.function.Supplier;
@@ -10,7 +11,6 @@ import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
@@ -29,8 +29,8 @@ import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
import limelight.networktables.Orientation3d;
/**
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
@@ -59,64 +59,65 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
/*
* SysId routine for characterizing translation. This is used to find PID gains
* for the drive motors.
*/
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
),
new SysIdRoutine.Mechanism(
output -> setControl(m_translationCharacterization.withVolts(output)),
null,
this
)
);
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())),
new SysIdRoutine.Mechanism(
output -> setControl(m_translationCharacterization.withVolts(output)),
null,
this));
/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
/*
* SysId routine for characterizing steer. This is used to find PID gains for
* the steer motors.
*/
@SuppressWarnings("unused")
private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(7), // Use dynamic voltage of 7 V
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
),
new SysIdRoutine.Mechanism(
volts -> setControl(m_steerCharacterization.withVolts(volts)),
null,
this
)
);
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(7), // Use dynamic voltage of 7 V
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())),
new SysIdRoutine.Mechanism(
volts -> setControl(m_steerCharacterization.withVolts(volts)),
null,
this));
/*
* SysId routine for characterizing rotation.
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
* This is used to find PID gains for the FieldCentricFacingAngle
* HeadingController.
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on
* importing the log to SysId.
*/
@SuppressWarnings("unused")
private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
new SysIdRoutine.Config(
/* This is in radians per second², but SysId only supports "volts per second" */
Volts.of(Math.PI / 6).per(Second),
/* This is in radians per second, but SysId only supports "volts" */
Volts.of(Math.PI),
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
),
new SysIdRoutine.Mechanism(
output -> {
/* output is actually radians per second, but SysId only supports "volts" */
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
/* also log the requested output for SysId */
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
},
null,
this
)
);
new SysIdRoutine.Config(
/* This is in radians per second², but SysId only supports "volts per second" */
Volts.of(Math.PI / 6).per(Second),
/* This is in radians per second, but SysId only supports "volts" */
Volts.of(Math.PI),
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())),
new SysIdRoutine.Mechanism(
output -> {
/* output is actually radians per second, but SysId only supports "volts" */
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
/* also log the requested output for SysId */
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
},
null,
this));
/* The SysId routine to test */
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
@@ -124,17 +125,18 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* This constructs the underlying hardware devices, so users should not
* construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param modules Constants for each specific module
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules
) {
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules) {
super(drivetrainConstants, modules);
if (Utils.isSimulation()) {
startSimThread();
@@ -145,8 +147,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* This constructs the underlying hardware devices, so users should not
* construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
@@ -156,10 +160,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
SwerveModuleConstants<?, ?, ?>... modules
) {
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
SwerveModuleConstants<?, ?, ?>... modules) {
super(drivetrainConstants, odometryUpdateFrequency, modules);
if (Utils.isSimulation()) {
startSimThread();
@@ -170,30 +173,38 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* This constructs the underlying hardware devices, so users should not
* construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param drivetrainConstants Drivetrain-wide constants for the swerve
* drive
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
* unspecified or set to 0 Hz, this is 250 Hz on
* unspecified or set to 0 Hz, this is 250 Hz
* on
* CAN FD, and 100 Hz on CAN 2.0.
* @param odometryStandardDeviation The standard deviation for odometry calculation
* in the form [x, y, theta]ᵀ, with units in meters
* @param odometryStandardDeviation The standard deviation for odometry
* calculation
* in the form [x, y, theta]ᵀ, with units in
* meters
* and radians
* @param visionStandardDeviation The standard deviation for vision calculation
* in the form [x, y, theta]ᵀ, with units in meters
* @param visionStandardDeviation The standard deviation for vision
* calculation
* in the form [x, y, theta]ᵀ, with units in
* meters
* and radians
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
Matrix<N3, N1> odometryStandardDeviation,
Matrix<N3, N1> visionStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
Matrix<N3, N1> odometryStandardDeviation,
Matrix<N3, N1> visionStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules) {
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation,
modules);
if (Utils.isSimulation()) {
startSimThread();
}
@@ -204,33 +215,34 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
try {
var config = RobotConfig.fromGUISettings();
AutoBuilder.configure(
() -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
() -> getState().Speeds, // Supplier of current robot speeds
// Consumer of ChassisSpeeds and feedforwards to drive the robot
(speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
),
new PPHolonomicDriveController(
// PID constants for translation
new PIDConstants(10, 0, 0),
// PID constants for rotation
new PIDConstants(7, 0, 0)
),
config,
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this // Subsystem for requirements
() -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
() -> getState().Speeds, // Supplier of current robot speeds
// Consumer of ChassisSpeeds and feedforwards to drive the robot
(speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())),
new PPHolonomicDriveController(
// PID constants for translation
new PIDConstants(10, 0, 0),
// PID constants for rotation
new PIDConstants(7, 0, 0)),
config,
// Assume the path needs to be flipped for Red vs Blue, this is normally the
// case
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this // Subsystem for requirements
);
} catch (Exception ex) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder",
ex.getStackTrace());
}
}
/**
* Returns a command that applies the specified control request to this swerve drivetrain.
* Returns a command that applies the specified control request to this swerve
* drivetrain.
*
* @param request Function returning the request to apply
* @return Command to run
@@ -261,22 +273,32 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
return m_sysIdRoutineToApply.dynamic(direction);
}
public Orientation3d getOrientation3d() {
return new Orientation3d(getRotation3d(),
getPigeon2().getAngularVelocityZDevice().getValue(), // Yaw
getPigeon2().getAngularVelocityYDevice().getValue(), // Pitch
getPigeon2().getAngularVelocityXDevice().getValue()); // Roll
}
@Override
public void periodic() {
/*
* Periodically try to apply the operator perspective.
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
* This allows us to correct the perspective in case the robot code restarts mid-match.
* Otherwise, only check and apply the operator perspective if the DS is disabled.
* This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
* If we haven't applied the operator perspective before, then we should apply
* it regardless of DS state.
* This allows us to correct the perspective in case the robot code restarts
* mid-match.
* Otherwise, only check and apply the operator perspective if the DS is
* disabled.
* This ensures driving behavior doesn't change until an explicit disable event
* occurs during testing.
*/
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
DriverStation.getAlliance().ifPresent(allianceColor -> {
setOperatorPerspectiveForward(
allianceColor == Alliance.Red
? kRedAlliancePerspectiveRotation
: kBlueAlliancePerspectiveRotation
);
allianceColor == Alliance.Red
? kRedAlliancePerspectiveRotation
: kBlueAlliancePerspectiveRotation);
m_hasAppliedOperatorPerspective = true;
});
}
@@ -298,11 +320,14 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate
* while still accounting for measurement noise.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
* @param visionRobotPoseMeters The pose of the robot as measured by the vision
* camera.
* @param timestampSeconds The timestamp of the vision measurement in
* seconds.
*/
@Override
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -310,32 +335,38 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate
* while still accounting for measurement noise.
* <p>
* Note that the vision measurement standard deviations passed into this method
* will continue to apply to future measurements until a subsequent call to
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
* in the form [x, y, theta]ᵀ, with units in meters and radians.
* @param visionRobotPoseMeters The pose of the robot as measured by the
* vision camera.
* @param timestampSeconds The timestamp of the vision measurement in
* seconds.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement
* in the form [x, y, theta]ᵀ, with units in
* meters and radians.
*/
@Override
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs
) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds),
visionMeasurementStdDevs);
}
/**
* Return the pose at a given timestamp, if the buffer is not empty.
*
* @param timestampSeconds The timestamp of the pose in seconds.
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
* @return The pose at the given timestamp (or Optional.empty() if the buffer is
* empty).
*/
@Override
public Optional<Pose2d> samplePoseAt(double timestampSeconds) {

20
vendordeps/yall.json Normal file
View File

@@ -0,0 +1,20 @@
{
"fileName": "yall.json",
"name": "YALL",
"version": "2026.1.13",
"frcYear": "2026",
"uuid": "29aa8c8f-05ae-40aa-96f9-552a5c8a0347",
"mavenUrls": [
"https://Yet-Another-Software-Suite.github.io/YALL/releases"
],
"jsonUrl": "https://Yet-Another-Software-Suite.github.io/YALL/yall.json",
"javaDependencies": [
{
"groupId": "yall",
"artifactId": "YALL-java",
"version": "2026.1.13"
}
],
"jniDependencies": [],
"cppDependencies": []
}