71 _isRunning.store(
true, std::memory_order_relaxed);
80 _isRunning.store(
false, std::memory_order_relaxed);
149 std::vector<std::unique_ptr<SwerveModuleImpl>>
_modules;
181 template <
typename... ModuleConstants,
182 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
213 template <
typename... ModuleConstants,
214 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
216 ModuleConstants
const &... modules) :
234 SwerveDrivetrainImpl{drivetrainConstants, odometryUpdateFrequency,
std::array{0.1, 0.1, 0.1},
std::array{0.9, 0.9, 0.9}, modules}
252 template <
typename... ModuleConstants,
253 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
255 std::array<double, 3>
const &odometryStandardDeviation, std::array<double, 3>
const &visionStandardDeviation,
256 ModuleConstants
const &... modules) :
258 visionStandardDeviation,
std::array{modules...}}
276 std::array<double, 3>
const &odometryStandardDeviation, std::array<double, 3>
const &visionStandardDeviation,
293 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
311 auto status = module->ConfigNeutralMode(neutralMode);
326 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
328 for (
size_t i = 0; i <
_modules.size(); ++i) {
341 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
353 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
373 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
425 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
456 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
495 Pose2d visionRobotPose,
496 units::second_t timestamp,
497 std::array<double, 3> visionMeasurementStdDevs)
499 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
516 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
534 std::lock_guard<std::recursive_mutex> lock{
_stateLock};
Class for getting information about an available CAN bus.
Definition: CANBus.hpp:19
T GetValue() const
Gets the cached value from this status signal.
Definition: StatusSignal.hpp:783
Class description for the Pigeon 2 IMU sensor that measures orientation.
Definition: CorePigeon2.hpp:1076
The state of the motor controller bridge when output is neutral or disabled.
Definition: SpnEnums.hpp:1698
static LinearFilter< T > MovingAverage(int taps)
Creates a K-tap FIR moving average filter of the form: y[n] = 1/k (x[k] + x[k-1] + … + x[0])
Definition: LinearFilter.hpp:160
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.hpp:235
This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve dr...
Definition: SwerveDrivePoseEstimator.hpp:107
void ResetPosition(Rotation2d const &gyroAngle, WheelPositions wheelPositions, Pose2d const &pose)
Resets the robot's position on the field.
Definition: SwerveDrivePoseEstimator.hpp:226
void AddVisionMeasurement(Pose2d const &visionRobotPose, units::second_t timestamp)
Adds a vision measurement to the Kalman Filter.
void SetVisionMeasurementStdDevs(std::array< double, 3 > const &visionMeasurementStdDevs)
Sets the pose estimator's trust in vision measurements.
Performs swerve module updates in a separate thread to minimize latency.
Definition: SwerveDrivetrainImpl.hpp:33
std::thread _thread
Definition: SwerveDrivetrainImpl.hpp:42
std::atomic< bool > _isRunning
Definition: SwerveDrivetrainImpl.hpp:44
OdometryThread(SwerveDrivetrainImpl &drivetrain)
~OdometryThread()
Definition: SwerveDrivetrainImpl.hpp:62
std::vector< BaseStatusSignal * > _allSignals
Definition: SwerveDrivetrainImpl.hpp:46
bool IsOdometryValid() const
Definition: SwerveDrivetrainImpl.hpp:85
units::second_t _lastTime
Definition: SwerveDrivetrainImpl.hpp:51
void Stop()
Definition: SwerveDrivetrainImpl.hpp:76
std::atomic< int > _threadPriorityToSet
Definition: SwerveDrivetrainImpl.hpp:57
static constexpr int START_THREAD_PRIORITY
Definition: SwerveDrivetrainImpl.hpp:35
LinearFilter< units::second_t > _lowPass
Definition: SwerveDrivetrainImpl.hpp:49
std::atomic< int32_t > _failedDaqs
Definition: SwerveDrivetrainImpl.hpp:55
void Start()
Definition: SwerveDrivetrainImpl.hpp:67
bool _lastTimeValid
Definition: SwerveDrivetrainImpl.hpp:50
units::second_t _averageLoopTime
Definition: SwerveDrivetrainImpl.hpp:52
SwerveDrivetrainImpl * _drivetrain
Definition: SwerveDrivetrainImpl.hpp:40
void SetThreadPriority(int priority)
Definition: SwerveDrivetrainImpl.hpp:90
std::atomic< int32_t > _successfulDaqs
Definition: SwerveDrivetrainImpl.hpp:54
std::mutex _threadMtx
Definition: SwerveDrivetrainImpl.hpp:43
static constexpr int kLowPassTaps
Definition: SwerveDrivetrainImpl.hpp:48
int _lastThreadPriority
Definition: SwerveDrivetrainImpl.hpp:58
Swerve Drive class utilizing CTR Electronics Phoenix 6 API.
Definition: SwerveDrivetrainImpl.hpp:30
void TareEverything()
Zero's this swerve drive's odometry entirely.
Definition: SwerveDrivetrainImpl.hpp:324
SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, std::array< double, 3 > const &odometryStandardDeviation, std::array< double, 3 > const &visionStandardDeviation, span< SwerveModuleConstants const > modules)
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
void SetControl(SwerveRequestFunc &&request)
Applies the specified control function to this swerve drivetrain.
Definition: SwerveDrivetrainImpl.hpp:291
std::vector< Translation2d > _moduleLocations
Definition: SwerveDrivetrainImpl.hpp:151
std::vector< Translation2d > const & GetModuleLocations() const
Gets the locations of the swerve modules.
Definition: SwerveDrivetrainImpl.hpp:416
Rotation2d _operatorForwardDirection
Definition: SwerveDrivetrainImpl.hpp:159
SwerveDriveState GetState() const
Gets the current state of the swerve drivetrain.
Definition: SwerveDrivetrainImpl.hpp:423
SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, ModuleConstants const &... modules)
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
Definition: SwerveDrivetrainImpl.hpp:183
std::function< ctre::phoenix::StatusCode(ControlParameters const &, std::vector< std::unique_ptr< SwerveModuleImpl > > const &)> SwerveRequestFunc
Definition: SwerveDrivetrainImpl.hpp:136
SwerveDriveState _cachedState
Definition: SwerveDrivetrainImpl.hpp:165
std::unique_ptr< OdometryThread > _odometryThread
Definition: SwerveDrivetrainImpl.hpp:168
hardware::core::CorePigeon2 _pigeon2
Definition: SwerveDrivetrainImpl.hpp:145
SwerveDrivePoseEstimator _odometry
Definition: SwerveDrivetrainImpl.hpp:156
std::function< void(SwerveDriveState const &)> _telemetryFunction
Definition: SwerveDrivetrainImpl.hpp:166
SwerveModuleImpl const & GetModule(size_t index) const
Get a reference to the module at the specified index.
Definition: SwerveDrivetrainImpl.hpp:402
StatusSignal< units::degrees_per_second_t > _pigeonAngularVelocity
Definition: SwerveDrivetrainImpl.hpp:147
SwerveDriveKinematics _kinematics
Definition: SwerveDrivetrainImpl.hpp:155
bool IsOdometryValid() const
Check if the odometry is currently valid.
Definition: SwerveDrivetrainImpl.hpp:382
void SeedFieldRelative(Pose2d location)
Takes the specified location and makes it the current pose for field-relative maneuvers.
Definition: SwerveDrivetrainImpl.hpp:351
hardware::core::CorePigeon2 & GetPigeon2()
Gets this drivetrain's Pigeon 2 reference.
Definition: SwerveDrivetrainImpl.hpp:546
void RegisterTelemetry(std::function< void(SwerveDriveState const &)> telemetryFunction)
Register the specified lambda to be executed whenever the SwerveDriveState is updated in the odometry...
Definition: SwerveDrivetrainImpl.hpp:532
SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, std::array< double, 3 > const &odometryStandardDeviation, std::array< double, 3 > const &visionStandardDeviation, ModuleConstants const &... modules)
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
Definition: SwerveDrivetrainImpl.hpp:254
void AddVisionMeasurement(Pose2d visionRobotPose, units::second_t timestamp, std::array< double, 3 > visionMeasurementStdDevs)
Adds a vision measurement to the Kalman Filter.
Definition: SwerveDrivetrainImpl.hpp:494
std::vector< std::unique_ptr< SwerveModuleImpl > > const & GetModules() const
Get a reference to the full array of modules.
Definition: SwerveDrivetrainImpl.hpp:409
ctre::phoenix::StatusCode ConfigNeutralMode(signals::NeutralModeValue neutralMode)
Configures the neutral mode to use for all modules' drive motors.
Definition: SwerveDrivetrainImpl.hpp:307
std::vector< SwerveModuleState > _moduleStates
Definition: SwerveDrivetrainImpl.hpp:153
StatusSignal< units::degree_t > _pigeonYaw
Definition: SwerveDrivetrainImpl.hpp:146
SwerveRequestFunc _requestToApply
Definition: SwerveDrivetrainImpl.hpp:161
SwerveModuleImpl & GetModule(size_t index)
Get a reference to the module at the specified index.
Definition: SwerveDrivetrainImpl.hpp:394
bool _isOnCANFD
Definition: SwerveDrivetrainImpl.hpp:142
ControlParameters _requestParameters
Definition: SwerveDrivetrainImpl.hpp:162
SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, span< SwerveModuleConstants const > modules)
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
Definition: SwerveDrivetrainImpl.hpp:196
OdometryThread & GetDaqThread()
Gets a reference to the data acquisition thread.
Definition: SwerveDrivetrainImpl.hpp:284
SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, span< SwerveModuleConstants const > modules)
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
Definition: SwerveDrivetrainImpl.hpp:232
Rotation2d _fieldRelativeOffset
Definition: SwerveDrivetrainImpl.hpp:158
std::vector< SwerveModulePosition > _modulePositions
Definition: SwerveDrivetrainImpl.hpp:152
void SetOperatorPerspectiveForward(Rotation2d fieldDirection)
Takes the requests::SwerveRequest::ForwardReference::RedAlliance perpective direction and treats it a...
Definition: SwerveDrivetrainImpl.hpp:371
void SeedFieldRelative()
Takes the current orientation of the robot and makes it X forward for field-relative maneuvers.
Definition: SwerveDrivetrainImpl.hpp:339
void SetVisionMeasurementStdDevs(std::array< double, 3 > visionMeasurementStdDevs)
Sets the pose estimator's trust of global measurements.
Definition: SwerveDrivetrainImpl.hpp:514
std::recursive_mutex _stateLock
Definition: SwerveDrivetrainImpl.hpp:164
SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, ModuleConstants const &... modules)
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
Definition: SwerveDrivetrainImpl.hpp:215
CANBus _canbus
Definition: SwerveDrivetrainImpl.hpp:141
std::vector< std::unique_ptr< SwerveModuleImpl > > _modules
Definition: SwerveDrivetrainImpl.hpp:149
units::hertz_t _updateFrequency
Definition: SwerveDrivetrainImpl.hpp:143
void AddVisionMeasurement(Pose2d visionRobotPose, units::second_t timestamp)
Adds a vision measurement to the Kalman Filter.
Definition: SwerveDrivetrainImpl.hpp:454
Swerve Module class that encapsulates a swerve module powered by CTR Electronics devices.
Definition: SwerveModuleImpl.hpp:27
Status codes reported by APIs, including OK, warnings, and errors.
Definition: StatusCodes.h:27
static constexpr int OK
No Error.
Definition: StatusCodes.h:34
constexpr bool IsOK() const
Definition: StatusCodes.h:855
Represents the state of one swerve module.
Definition: StatusCodes.h:18
Common constants for a swerve drivetrain.
Definition: SwerveDrivetrainConstants.hpp:19
Represents the speed of a robot chassis.
Definition: SwerveDriveKinematics.hpp:96
Contains everything the control requests need to calculate the module state.
Definition: SwerveDrivetrainImpl.hpp:124
units::second_t timestamp
Definition: SwerveDrivetrainImpl.hpp:132
Translation2d const * swervePositions
Definition: SwerveDrivetrainImpl.hpp:126
units::meters_per_second_t kMaxSpeed
Definition: SwerveDrivetrainImpl.hpp:127
units::second_t updatePeriod
Definition: SwerveDrivetrainImpl.hpp:133
impl::SwerveDriveKinematics * kinematics
Definition: SwerveDrivetrainImpl.hpp:125
Pose2d currentPose
Definition: SwerveDrivetrainImpl.hpp:131
Rotation2d operatorForwardDirection
Definition: SwerveDrivetrainImpl.hpp:129
impl::ChassisSpeeds currentChassisSpeed
Definition: SwerveDrivetrainImpl.hpp:130
Plain-Old-Data class holding the state of the swerve drivetrain.
Definition: SwerveDrivetrainImpl.hpp:104
std::vector< SwerveModuleState > ModuleStates
The current module states.
Definition: SwerveDrivetrainImpl.hpp:110
int32_t SuccessfulDaqs
Number of successful data acquisitions.
Definition: SwerveDrivetrainImpl.hpp:116
ChassisSpeeds Speeds
The current velocity of the robot.
Definition: SwerveDrivetrainImpl.hpp:108
Pose2d Pose
The current pose of the robot.
Definition: SwerveDrivetrainImpl.hpp:106
std::vector< SwerveModuleState > ModuleTargets
The target module states.
Definition: SwerveDrivetrainImpl.hpp:112
int32_t FailedDaqs
Number of failed data acquisitions.
Definition: SwerveDrivetrainImpl.hpp:118
units::second_t OdometryPeriod
The measured odometry update period.
Definition: SwerveDrivetrainImpl.hpp:114