001/* 002 * Copyright (C) Cross The Road Electronics. All rights reserved. 003 * License information can be found in CTRE_LICENSE.txt 004 * For support and suggestions contact support@ctr-electronics.com or file 005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases 006 */ 007package com.ctre.phoenix6.mechanisms.swerve; 008 009import java.util.concurrent.locks.ReadWriteLock; 010import java.util.concurrent.locks.ReentrantReadWriteLock; 011import java.util.function.Consumer; 012 013import com.ctre.phoenix6.BaseStatusSignal; 014import com.ctre.phoenix6.CANBus; 015import com.ctre.phoenix6.StatusCode; 016import com.ctre.phoenix6.StatusSignal; 017import com.ctre.phoenix6.Utils; 018import com.ctre.phoenix6.hardware.Pigeon2; 019import com.ctre.phoenix6.signals.NeutralModeValue; 020 021import static com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SwerveControlRequestParameters; 022 023import edu.wpi.first.math.Matrix; 024import edu.wpi.first.math.VecBuilder; 025import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 026import edu.wpi.first.math.filter.LinearFilter; 027import edu.wpi.first.math.filter.MedianFilter; 028import edu.wpi.first.math.geometry.Pose2d; 029import edu.wpi.first.math.geometry.Rotation2d; 030import edu.wpi.first.math.geometry.Rotation3d; 031import edu.wpi.first.math.geometry.Translation2d; 032import edu.wpi.first.math.kinematics.ChassisSpeeds; 033import edu.wpi.first.math.kinematics.SwerveDriveKinematics; 034import edu.wpi.first.math.kinematics.SwerveModulePosition; 035import edu.wpi.first.math.kinematics.SwerveModuleState; 036import edu.wpi.first.math.numbers.N1; 037import edu.wpi.first.math.numbers.N3; 038import edu.wpi.first.wpilibj.Threads; 039import edu.wpi.first.wpilibj.Timer; 040 041/** 042 * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API. 043 * <p> 044 * This class handles the kinematics, configuration, and odometry of a 045 * swerve drive utilizing CTR Electronics devices. We recommend 046 * that users use the Swerve Mechanism Generator in Tuner X to create 047 * a template project that demonstrates how to use this class. 048 * <p> 049 * This class will construct the hardware devices internally, so the user 050 * only specifies the constants (IDs, PID gains, gear ratios, etc). 051 * Getters for these hardware devices are available. 052 * <p> 053 * If using the generator, the order in which modules are constructed is 054 * Front Left, Front Right, Back Left, Back Right. This means if you need 055 * the Back Left module, call {@code getModule(2);} to get the 3rd index 056 * (0-indexed) module, corresponding to the Back Left module. 057 */ 058public class SwerveDrivetrain { 059 protected final boolean IsOnCANFD; 060 061 protected final double UpdateFrequency; 062 protected final int ModuleCount; 063 protected final SwerveModule[] Modules; 064 065 protected final Pigeon2 m_pigeon2; 066 protected final StatusSignal<Double> m_yawGetter; 067 protected final StatusSignal<Double> m_angularVelocity; 068 069 protected SwerveDriveKinematics m_kinematics; 070 protected SwerveDrivePoseEstimator m_odometry; 071 protected SwerveModulePosition[] m_modulePositions; 072 protected SwerveModuleState[] m_moduleStates; 073 protected Translation2d[] m_moduleLocations; 074 protected OdometryThread m_odometryThread; 075 protected Rotation2d m_fieldRelativeOffset; 076 protected Rotation2d m_operatorForwardDirection; 077 078 protected SwerveRequest m_requestToApply = new SwerveRequest.Idle(); 079 protected SwerveControlRequestParameters m_requestParameters = new SwerveControlRequestParameters(); 080 081 protected final ReadWriteLock m_stateLock = new ReentrantReadWriteLock(); 082 083 protected final SimSwerveDrivetrain m_simDrive; 084 085 /** 086 * Plain-Old-Data class holding the state of the swerve drivetrain. 087 * This encapsulates most data that is relevant for telemetry or 088 * decision-making from the Swerve Drive. 089 */ 090 public static class SwerveDriveState { 091 /** Number of successful data acquisitions */ 092 public int SuccessfulDaqs; 093 /** Number of failed data acquisitions */ 094 public int FailedDaqs; 095 /** The current pose of the robot */ 096 public Pose2d Pose; 097 /** The current velocity of the robot */ 098 public ChassisSpeeds speeds; 099 /** The current module states */ 100 public SwerveModuleState[] ModuleStates; 101 /** The target module states */ 102 public SwerveModuleState[] ModuleTargets; 103 /** The measured odometry update period, in seconds */ 104 public double OdometryPeriod; 105 } 106 107 protected Consumer<SwerveDriveState> m_telemetryFunction = null; 108 protected final SwerveDriveState m_cachedState = new SwerveDriveState(); 109 110 /* Perform swerve module updates in a separate thread to minimize latency */ 111 public class OdometryThread { 112 protected static final int START_THREAD_PRIORITY = 1; // Testing shows 1 (minimum realtime) is sufficient for tighter 113 // odometry loops. 114 // If the odometry period is far away from the desired frequency, 115 // increasing this may help 116 117 protected final Thread m_thread; 118 protected volatile boolean m_running = false; 119 120 protected final BaseStatusSignal[] m_allSignals; 121 122 protected final MedianFilter peakRemover = new MedianFilter(3); 123 protected final LinearFilter lowPass = LinearFilter.movingAverage(50); 124 protected double lastTime = 0; 125 protected double currentTime = 0; 126 protected double averageLoopTime = 0; 127 protected int SuccessfulDaqs = 0; 128 protected int FailedDaqs = 0; 129 130 protected int lastThreadPriority = START_THREAD_PRIORITY; 131 protected volatile int threadPriorityToSet = START_THREAD_PRIORITY; 132 133 public OdometryThread() { 134 m_thread = new Thread(this::run); 135 /* Mark this thread as a "daemon" (background) thread 136 * so it doesn't hold up program shutdown */ 137 m_thread.setDaemon(true); 138 139 /* 4 signals for each module + 2 for Pigeon2 */ 140 m_allSignals = new BaseStatusSignal[(ModuleCount * 4) + 2]; 141 for (int i = 0; i < ModuleCount; ++i) { 142 var signals = Modules[i].getSignals(); 143 m_allSignals[(i * 4) + 0] = signals[0]; 144 m_allSignals[(i * 4) + 1] = signals[1]; 145 m_allSignals[(i * 4) + 2] = signals[2]; 146 m_allSignals[(i * 4) + 3] = signals[3]; 147 } 148 m_allSignals[m_allSignals.length - 2] = m_yawGetter; 149 m_allSignals[m_allSignals.length - 1] = m_angularVelocity; 150 } 151 152 /** 153 * Starts the odometry thread. 154 */ 155 public void start() { 156 m_running = true; 157 m_thread.start(); 158 } 159 160 /** 161 * Stops the odometry thread. 162 */ 163 public void stop() { 164 stop(0); 165 } 166 167 /** 168 * Stops the odometry thread with a timeout. 169 * 170 * @param millis The time to wait in milliseconds 171 */ 172 public void stop(long millis) { 173 m_running = false; 174 try { 175 m_thread.join(millis); 176 } catch (final InterruptedException ex) { 177 Thread.currentThread().interrupt(); 178 } 179 } 180 181 public void run() { 182 /* Make sure all signals update at the correct update frequency */ 183 BaseStatusSignal.setUpdateFrequencyForAll(UpdateFrequency, m_allSignals); 184 Threads.setCurrentThreadPriority(true, START_THREAD_PRIORITY); 185 186 /* Run as fast as possible, our signals will control the timing */ 187 while (m_running) { 188 /* Synchronously wait for all signals in drivetrain */ 189 /* Wait up to twice the period of the update frequency */ 190 StatusCode status; 191 if (IsOnCANFD) { 192 status = BaseStatusSignal.waitForAll(2.0 / UpdateFrequency, m_allSignals); 193 } else { 194 /* Wait for the signals to update */ 195 Timer.delay(1.0 / UpdateFrequency); 196 status = BaseStatusSignal.refreshAll(m_allSignals); 197 } 198 199 try { 200 m_stateLock.writeLock().lock(); 201 202 lastTime = currentTime; 203 currentTime = Utils.getCurrentTimeSeconds(); 204 /* We don't care about the peaks, as they correspond to GC events, and we want the period generally low passed */ 205 averageLoopTime = lowPass.calculate(peakRemover.calculate(currentTime - lastTime)); 206 207 /* Get status of first element */ 208 if (status.isOK()) { 209 SuccessfulDaqs++; 210 } else { 211 FailedDaqs++; 212 } 213 214 /* Now update odometry */ 215 /* Keep track of the change in azimuth rotations */ 216 for (int i = 0; i < ModuleCount; ++i) { 217 m_modulePositions[i] = Modules[i].getPosition(false); 218 m_moduleStates[i] = Modules[i].getCurrentState(); 219 } 220 double yawDegrees = BaseStatusSignal.getLatencyCompensatedValue( 221 m_yawGetter, m_angularVelocity); 222 223 /* Keep track of previous and current pose to account for the carpet vector */ 224 m_odometry.update(Rotation2d.fromDegrees(yawDegrees), m_modulePositions); 225 226 ChassisSpeeds speeds = m_kinematics.toChassisSpeeds(m_moduleStates); 227 228 /* And now that we've got the new odometry, update the controls */ 229 m_requestParameters.currentPose = m_odometry.getEstimatedPosition() 230 .relativeTo(new Pose2d(0, 0, m_fieldRelativeOffset)); 231 m_requestParameters.kinematics = m_kinematics; 232 m_requestParameters.swervePositions = m_moduleLocations; 233 m_requestParameters.currentChassisSpeed = speeds; 234 m_requestParameters.timestamp = currentTime; 235 m_requestParameters.updatePeriod = 1.0 / UpdateFrequency; 236 m_requestParameters.operatorForwardDirection = m_operatorForwardDirection; 237 238 m_requestToApply.apply(m_requestParameters, Modules); 239 240 /* Update our cached state with the newly updated data */ 241 m_cachedState.FailedDaqs = FailedDaqs; 242 m_cachedState.SuccessfulDaqs = SuccessfulDaqs; 243 m_cachedState.Pose = m_odometry.getEstimatedPosition(); 244 m_cachedState.speeds = speeds; 245 m_cachedState.OdometryPeriod = averageLoopTime; 246 247 if (m_cachedState.ModuleStates == null) { 248 m_cachedState.ModuleStates = new SwerveModuleState[Modules.length]; 249 } 250 if (m_cachedState.ModuleTargets == null) { 251 m_cachedState.ModuleTargets = new SwerveModuleState[Modules.length]; 252 } 253 for (int i = 0; i < Modules.length; ++i) { 254 m_cachedState.ModuleStates[i] = Modules[i].getCurrentState(); 255 m_cachedState.ModuleTargets[i] = Modules[i].getTargetState(); 256 } 257 258 if (m_telemetryFunction != null) { 259 /* Log our state */ 260 m_telemetryFunction.accept(m_cachedState); 261 } 262 } finally { 263 m_stateLock.writeLock().unlock(); 264 } 265 266 /** 267 * This is inherently synchronous, since lastThreadPriority 268 * is only written here and threadPriorityToSet is only read here 269 */ 270 if (threadPriorityToSet != lastThreadPriority) { 271 Threads.setCurrentThreadPriority(true, threadPriorityToSet); 272 lastThreadPriority = threadPriorityToSet; 273 } 274 } 275 } 276 277 public boolean odometryIsValid() { 278 return SuccessfulDaqs > 2; // Wait at least 3 daqs before saying the odometry is valid 279 } 280 281 /** 282 * Sets the DAQ thread priority to a real time priority under the specified priority level 283 * 284 * @param priority Priority level to set the DAQ thread to. 285 * This is a value between 0 and 99, with 99 indicating higher priority and 0 indicating lower priority. 286 */ 287 public void setThreadPriority(int priority) { 288 threadPriorityToSet = priority; 289 } 290 } 291 292 protected boolean checkIsOnCanFD(String canbusName) { 293 return CANBus.isNetworkFD(canbusName); 294 } 295 296 /** 297 * Constructs a CTRSwerveDrivetrain using the specified constants. 298 * <p> 299 * This constructs the underlying hardware devices, so user should not construct 300 * the devices themselves. If they need the devices, they can access them 301 * through 302 * getters in the classes. 303 * 304 * @param driveTrainConstants Drivetrain-wide constants for the swerve drive 305 * @param modules Constants for each specific module 306 */ 307 public SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { 308 this(driveTrainConstants, 0, modules); 309 } 310 /** 311 * Constructs a CTRSwerveDrivetrain using the specified constants. 312 * <p> 313 * This constructs the underlying hardware devices, so user should not construct 314 * the devices themselves. If they need the devices, they can access them 315 * through 316 * getters in the classes. 317 * 318 * @param driveTrainConstants Drivetrain-wide constants for the swerve drive 319 * @param OdometryUpdateFrequency The frequency to run the odometry loop. If 320 * unspecified, this is 250 Hz on CAN FD, and 321 * 100 Hz on CAN 2.0. 322 * @param modules Constants for each specific module 323 */ 324 public SwerveDrivetrain( 325 SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, 326 SwerveModuleConstants... modules) { 327 this(driveTrainConstants, OdometryUpdateFrequency, 328 VecBuilder.fill(0.1, 0.1, 0.1), 329 VecBuilder.fill(0.9, 0.9, 0.9), 330 modules); 331 } 332 333 /** 334 * Constructs a CTRSwerveDrivetrain using the specified constants. 335 * <p> 336 * This constructs the underlying hardware devices, so user should not construct 337 * the devices themselves. If they need the devices, they can access them 338 * through 339 * getters in the classes. 340 * 341 * @param driveTrainConstants Drivetrain-wide constants for the swerve drive 342 * @param OdometryUpdateFrequency The frequency to run the odometry loop. If 343 * unspecified, this is 250 Hz on CAN FD, and 344 * 100 Hz on CAN 2.0. 345 * @param odometryStandardDeviation The standard deviation for odometry calculation 346 * @param visionStandardDeviation The standard deviation for vision calculation 347 * @param modules Constants for each specific module 348 */ 349 public SwerveDrivetrain( 350 SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, 351 Matrix<N3, N1> odometryStandardDeviation, Matrix<N3, N1> visionStandardDeviation, 352 SwerveModuleConstants... modules) { 353 ModuleCount = modules.length; 354 355 m_pigeon2 = new Pigeon2(driveTrainConstants.Pigeon2Id, driveTrainConstants.CANbusName); 356 m_yawGetter = m_pigeon2.getYaw().clone(); 357 m_angularVelocity = m_pigeon2.getAngularVelocityZWorld().clone(); 358 359 if (driveTrainConstants.Pigeon2Configs != null) { 360 StatusCode response = m_pigeon2.getConfigurator().apply(driveTrainConstants.Pigeon2Configs); 361 if (!response.isOK()) { 362 System.out.println( 363 "Pigeon2 ID " + m_pigeon2.getDeviceID() + " failed config with error: " + response.toString()); 364 } 365 } 366 367 Modules = new SwerveModule[ModuleCount]; 368 m_modulePositions = new SwerveModulePosition[ModuleCount]; 369 m_moduleStates = new SwerveModuleState[ModuleCount]; 370 m_moduleLocations = new Translation2d[ModuleCount]; 371 372 int iteration = 0; 373 for (SwerveModuleConstants module : modules) { 374 Modules[iteration] = new SwerveModule(module, driveTrainConstants.CANbusName); 375 m_moduleLocations[iteration] = new Translation2d(module.LocationX, module.LocationY); 376 m_modulePositions[iteration] = Modules[iteration].getPosition(true); 377 m_moduleStates[iteration] = Modules[iteration].getCurrentState(); 378 379 iteration++; 380 } 381 m_kinematics = new SwerveDriveKinematics(m_moduleLocations); 382 m_odometry = new SwerveDrivePoseEstimator(m_kinematics, new Rotation2d(), m_modulePositions, new Pose2d(), odometryStandardDeviation, visionStandardDeviation); 383 384 m_fieldRelativeOffset = new Rotation2d(); 385 m_operatorForwardDirection = new Rotation2d(); 386 387 m_simDrive = new SimSwerveDrivetrain(m_moduleLocations, m_pigeon2, driveTrainConstants, modules); 388 389 IsOnCANFD = checkIsOnCanFD(driveTrainConstants.CANbusName); 390 if (OdometryUpdateFrequency == 0) { 391 UpdateFrequency = IsOnCANFD ? 250 : 100; 392 } else { 393 UpdateFrequency = OdometryUpdateFrequency; 394 } 395 396 m_odometryThread = new OdometryThread(); 397 m_odometryThread.start(); 398 } 399 400 /** 401 * Gets a reference to the data acquisition thread. 402 * 403 * @return DAQ thread 404 */ 405 public OdometryThread getDaqThread() { 406 return m_odometryThread; 407 } 408 409 /** 410 * Applies the specified control request to this swerve drivetrain. 411 * 412 * @param request Request to apply 413 */ 414 public void setControl(SwerveRequest request) { 415 try { 416 m_stateLock.writeLock().lock(); 417 418 m_requestToApply = request; 419 } finally { 420 m_stateLock.writeLock().unlock(); 421 } 422 } 423 424 /** 425 * Configures the neutral mode to use for all modules' drive motors. 426 * 427 * @param neutralMode The drive motor neutral mode 428 * @return Status code of the first failed config call, or OK if all succeeded 429 */ 430 public StatusCode configNeutralMode(NeutralModeValue neutralMode) { 431 var status = StatusCode.OK; 432 for (var module : Modules) { 433 var moduleStatus = module.configNeutralMode(neutralMode); 434 if (status.isOK()) { 435 status = moduleStatus; 436 } 437 } 438 return status; 439 } 440 441 /** 442 * Zero's this swerve drive's odometry entirely. 443 * <p> 444 * This will zero the entire odometry, and place the robot at 0,0 445 */ 446 public void tareEverything() { 447 try { 448 m_stateLock.writeLock().lock(); 449 450 for (int i = 0; i < ModuleCount; ++i) { 451 Modules[i].resetPosition(); 452 m_modulePositions[i] = Modules[i].getPosition(true); 453 } 454 m_odometry.resetPosition(Rotation2d.fromDegrees(m_yawGetter.getValue()), m_modulePositions, new Pose2d()); 455 } finally { 456 m_stateLock.writeLock().unlock(); 457 } 458 } 459 460 /** 461 * Takes the current orientation of the robot and makes it X forward for 462 * field-relative 463 * maneuvers. 464 */ 465 public void seedFieldRelative() { 466 try { 467 m_stateLock.writeLock().lock(); 468 469 m_fieldRelativeOffset = getState().Pose.getRotation(); 470 } finally { 471 m_stateLock.writeLock().unlock(); 472 } 473 } 474 475 /** 476 * Takes the {@link SwerveRequest.ForwardReference#RedAlliance} perpective direction 477 * and treats it as the forward direction for 478 * {@link SwerveRequest.ForwardReference#OperatorPerspective}. 479 * <p> 480 * If the operator is in the Blue Alliance Station, this should be 0 degrees. 481 * If the operator is in the Red Alliance Station, this should be 180 degrees. 482 * 483 * @param fieldDirection Heading indicating which direction is forward from 484 * the {@link SwerveRequest.ForwardReference#RedAlliance} perspective. 485 */ 486 public void setOperatorPerspectiveForward(Rotation2d fieldDirection) { 487 m_operatorForwardDirection = fieldDirection; 488 } 489 490 /** 491 * Takes the specified location and makes it the current pose for 492 * field-relative maneuvers 493 * 494 * @param location Pose to make the current pose at. 495 */ 496 public void seedFieldRelative(Pose2d location) { 497 try { 498 m_stateLock.writeLock().lock(); 499 500 m_odometry.resetPosition(Rotation2d.fromDegrees(m_yawGetter.getValue()), m_modulePositions, location); 501 /* We need to update our cached pose immediately so that race conditions don't happen */ 502 m_cachedState.Pose = location; 503 } finally { 504 m_stateLock.writeLock().unlock(); 505 } 506 } 507 508 /** 509 * Check if the odometry is currently valid 510 * 511 * @return True if odometry is valid 512 */ 513 public boolean odometryIsValid() { 514 try { 515 m_stateLock.readLock().lock(); 516 517 return m_odometryThread.odometryIsValid(); 518 } finally { 519 m_stateLock.readLock().unlock(); 520 } 521 } 522 523 /** 524 * Get a reference to the module at the specified index. 525 * The index corresponds to the module described in the constructor 526 * 527 * @param index Which module to get 528 * @return Reference to SwerveModule 529 */ 530 public SwerveModule getModule(int index) { 531 if (index >= Modules.length) 532 return null; 533 return Modules[index]; 534 } 535 536 /** 537 * Gets the current state of the swerve drivetrain. 538 * 539 * @return Current state of the drivetrain 540 */ 541 public SwerveDriveState getState() { 542 try { 543 m_stateLock.readLock().lock(); 544 545 return m_cachedState; 546 } finally { 547 m_stateLock.readLock().unlock(); 548 } 549 } 550 551 /** 552 * Gets the current orientation of the robot as a {@link Rotation3d} from 553 * the Pigeon 2 quaternion values. 554 * 555 * @return The robot orientation as a {@link Rotation3d} 556 */ 557 public Rotation3d getRotation3d() { 558 return m_pigeon2.getRotation3d(); 559 } 560 561 /** 562 * Adds a vision measurement to the Kalman Filter. This will correct the 563 * odometry pose estimate 564 * while still accounting for measurement noise. 565 * 566 * <p> 567 * This method can be called as infrequently as you want, as long as you are 568 * calling {@link 569 * SwerveDrivePoseEstimator#update} every loop. 570 * 571 * <p> 572 * To promote stability of the pose estimate and make it robust to bad vision 573 * data, we 574 * recommend only adding vision measurements that are already within one meter 575 * or so of the 576 * current pose estimate. 577 * 578 * <p> 579 * Note that the vision measurement standard deviations passed into this method 580 * will continue 581 * to apply to future measurements until a subsequent call to {@link 582 * SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method. 583 * 584 * @param visionRobotPoseMeters The pose of the robot as measured by the 585 * vision camera. 586 * @param timestampSeconds The timestamp of the vision measurement in 587 * seconds. Note that if you 588 * don't use your own time source by calling 589 * {@link 590 * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, 591 * then 592 * you must use a timestamp with an epoch since 593 * FPGA startup (i.e., the epoch of this 594 * timestamp is the same epoch as 595 * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). 596 * This means that you should use 597 * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} 598 * as 599 * your time source in this case. 600 * @param visionMeasurementStdDevs Standard deviations of the vision pose 601 * measurement (x position 602 * in meters, y position in meters, and heading 603 * in radians). Increase these numbers to trust 604 * the vision pose measurement less. 605 */ 606 public void addVisionMeasurement( 607 Pose2d visionRobotPoseMeters, 608 double timestampSeconds, 609 Matrix<N3, N1> visionMeasurementStdDevs) { 610 try { 611 m_stateLock.writeLock().lock(); 612 m_odometry.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); 613 } finally { 614 m_stateLock.writeLock().unlock(); 615 } 616 } 617 618 /** 619 * Adds a vision measurement to the Kalman Filter. This will correct the 620 * odometry pose estimate 621 * while still accounting for measurement noise. 622 * 623 * <p> 624 * This method can be called as infrequently as you want, as long as you are 625 * calling {@link 626 * SwerveDrivePoseEstimator#update} every loop. 627 * 628 * <p> 629 * To promote stability of the pose estimate and make it robust to bad vision 630 * data, we 631 * recommend only adding vision measurements that are already within one meter 632 * or so of the 633 * current pose estimate. 634 * 635 * @param visionRobotPoseMeters The pose of the robot as measured by the vision 636 * camera. 637 * @param timestampSeconds The timestamp of the vision measurement in 638 * seconds. Note that if you 639 * don't use your own time source by calling {@link 640 * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} 641 * then you 642 * must use a timestamp with an epoch since FPGA 643 * startup (i.e., the epoch of this timestamp is 644 * the same epoch as 645 * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) 646 * This means that 647 * you should use 648 * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} 649 * as your time source 650 * or sync the epochs. 651 */ 652 public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { 653 try { 654 m_stateLock.writeLock().lock(); 655 m_odometry.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); 656 } finally { 657 m_stateLock.writeLock().unlock(); 658 } 659 } 660 661 /** 662 * Sets the pose estimator's trust of global measurements. This might be used to 663 * change trust in 664 * vision measurements after the autonomous period, or to change trust as 665 * distance to a vision 666 * target increases. 667 * 668 * @param visionMeasurementStdDevs Standard deviations of the vision 669 * measurements. Increase these 670 * numbers to trust global measurements from 671 * vision less. This matrix is in the form [x, 672 * y, 673 * theta]ᵀ, with units in meters and radians. 674 */ 675 public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) { 676 try { 677 m_stateLock.writeLock().lock(); 678 m_odometry.setVisionMeasurementStdDevs(visionMeasurementStdDevs); 679 } finally { 680 m_stateLock.writeLock().unlock(); 681 } 682 } 683 684 /** 685 * Updates all the simulation state variables for this 686 * drivetrain class. User provides the update variables for the simulation. 687 * 688 * @param dtSeconds time since last update call 689 * @param supplyVoltage voltage as seen at the motor controllers 690 */ 691 public void updateSimState(double dtSeconds, double supplyVoltage) { 692 m_simDrive.update(dtSeconds, supplyVoltage, Modules); 693 } 694 695 696 /** 697 * Register the specified lambda to be executed whenever our SwerveDriveState function 698 * is updated in our odometry thread. 699 * <p> 700 * It is imperative that this function is cheap, as it will be executed along with 701 * the odometry call, and if this takes a long time, it may negatively impact 702 * the odometry of this stack. 703 * <p> 704 * This can also be used for logging data if the function performs logging instead of telemetry 705 * 706 * @param telemetryFunction Function to call for telemetry or logging 707 */ 708 public void registerTelemetry(Consumer<SwerveDriveState> telemetryFunction) { 709 try { 710 m_stateLock.writeLock().lock(); 711 m_telemetryFunction = telemetryFunction; 712 } finally { 713 m_stateLock.writeLock().unlock(); 714 } 715 } 716 717 /** 718 * Gets this drivetrain's Pigeon 2 reference. 719 * <p> 720 * This should be used only to access signals and change configurations that the 721 * swerve drivetrain does not configure itself. 722 * 723 * @return This drivetrain's Pigeon 2 reference 724 */ 725 public Pigeon2 getPigeon2() { 726 return m_pigeon2; 727 } 728}