CTRE Phoenix 6 C++ 24.50.0-alpha-2
SwerveDrivetrainImpl.hpp
Go to the documentation of this file.
1/*
2 * Copyright (C) Cross The Road Electronics.  All rights reserved.
3 * License information can be found in CTRE_LICENSE.txt
4 * For support and suggestions contact support@ctr-electronics.com or file
5 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
6 */
7#pragma once
8
14
15#include <atomic>
16#include <thread>
17
18namespace ctre {
19namespace phoenix6 {
20namespace swerve {
21namespace impl {
22
23/**
24 * \brief Swerve Drive class utilizing CTR Electronics Phoenix 6 API.
25 *
26 * This class handles the kinematics and odometry (but not configuration)
27 * of a swerve drive utilizing CTR Electronics devices. Users should
28 * create a high-level SwerveDrivetrain instead of using this directly.
29 */
31public:
32 /** \brief Performs swerve module updates in a separate thread to minimize latency. */
34 protected:
35 static constexpr int START_THREAD_PRIORITY = 1; // Testing shows 1 (minimum realtime) is sufficient for tighter
36 // odometry loops.
37 // If the odometry period is far away from the desired frequency,
38 // increasing this may help
39
41
42 std::thread _thread;
43 std::mutex _threadMtx;
44 std::atomic<bool> _isRunning = false;
45
46 std::vector<BaseStatusSignal *> _allSignals;
47
48 static constexpr int kLowPassTaps = 50;
50 bool _lastTimeValid = false;
51 units::second_t _lastTime{};
52 units::second_t _averageLoopTime{};
53
54 std::atomic<int32_t> _successfulDaqs{};
55 std::atomic<int32_t> _failedDaqs{};
56
59
60 public:
63 {
64 Stop();
65 }
66
67 void Start()
68 {
69 std::lock_guard<std::mutex> lock{_threadMtx};
70 if (!_thread.joinable()) {
71 _isRunning.store(true, std::memory_order_relaxed);
72 _thread = std::thread{[this] { Run(); }};
73 }
74 }
75
76 void Stop()
77 {
78 std::lock_guard<std::mutex> lock{_threadMtx};
79 if (_thread.joinable()) {
80 _isRunning.store(false, std::memory_order_relaxed);
81 _thread.join();
82 }
83 }
84
85 bool IsOdometryValid() const
86 {
87 return _successfulDaqs.load(std::memory_order_relaxed) > 2;
88 }
89
90 void SetThreadPriority(int priority)
91 {
92 _threadPriorityToSet.store(priority, std::memory_order_relaxed);
93 }
94
95 protected:
96 void Run();
97 };
98
99 /**
100 * \brief Plain-Old-Data class holding the state of the swerve drivetrain.
101 * This encapsulates most data that is relevant for telemetry or
102 * decision-making from the Swerve Drive.
103 */
105 /** \brief The current pose of the robot */
106 Pose2d Pose;
107 /** \brief The current velocity of the robot */
109 /** \brief The current module states */
110 std::vector<SwerveModuleState> ModuleStates;
111 /** \brief The target module states */
112 std::vector<SwerveModuleState> ModuleTargets;
113 /** \brief The measured odometry update period */
114 units::second_t OdometryPeriod;
115 /** \brief Number of successful data acquisitions */
117 /** \brief Number of failed data acquisitions */
118 int32_t FailedDaqs;
119 };
120
121 /**
122 * \brief Contains everything the control requests need to calculate the module state.
123 */
126 Translation2d const *swervePositions;
127 units::meters_per_second_t kMaxSpeed;
128
132 units::second_t timestamp;
133 units::second_t updatePeriod;
134 };
135
136 using SwerveRequestFunc = std::function<ctre::phoenix::StatusCode(ControlParameters const &, std::vector<std::unique_ptr<SwerveModuleImpl>> const &)>;
137
138protected:
139 friend class OdometryThread;
140
143 units::hertz_t _updateFrequency;
144
148
149 std::vector<std::unique_ptr<SwerveModuleImpl>> _modules;
150
151 std::vector<Translation2d> _moduleLocations;
152 std::vector<SwerveModulePosition> _modulePositions;
153 std::vector<SwerveModuleState> _moduleStates;
154
157
160
163
164 mutable std::recursive_mutex _stateLock;
166 std::function<void(SwerveDriveState const &)> _telemetryFunction{};
167
168 std::unique_ptr<OdometryThread> _odometryThread;
169
170public:
171 /**
172 * \brief Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
173 *
174 * This constructs the underlying hardware devices, so user should not construct
175 * the devices themselves. If they need the devices, they can access them
176 * through getters in the classes.
177 *
178 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
179 * \param modules Constants for each specific module
180 */
181 template < typename... ModuleConstants,
182 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
183 SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, ModuleConstants const &... modules) :
184 SwerveDrivetrainImpl{drivetrainConstants, std::array{modules...}}
185 {}
186 /**
187 * \brief Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
188 *
189 * This constructs the underlying hardware devices, so user should not construct
190 * the devices themselves. If they need the devices, they can access them
191 * through getters in the classes.
192 *
193 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
194 * \param modules Constants for each specific module
195 */
197 SwerveDrivetrainImpl{drivetrainConstants, 0_Hz, modules}
198 {}
199
200 /**
201 * \brief Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
202 *
203 * This constructs the underlying hardware devices, so user should not construct
204 * the devices themselves. If they need the devices, they can access them
205 * through getters in the classes.
206 *
207 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
208 * \param odometryUpdateFrequency The frequency to run the odometry loop. If
209 * unspecified or set to 0 Hz, this is 250 Hz on
210 * CAN FD, and 100 Hz on CAN 2.0.
211 * \param modules Constants for each specific module
212 */
213 template < typename... ModuleConstants,
214 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
215 SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency,
216 ModuleConstants const &... modules) :
217 SwerveDrivetrainImpl{drivetrainConstants, odometryUpdateFrequency, std::array{modules...}}
218 {}
219 /**
220 * \brief Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
221 *
222 * This constructs the underlying hardware devices, so user should not construct
223 * the devices themselves. If they need the devices, they can access them
224 * through getters in the classes.
225 *
226 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
227 * \param odometryUpdateFrequency The frequency to run the odometry loop. If
228 * unspecified or set to 0 Hz, this is 250 Hz on
229 * CAN FD, and 100 Hz on CAN 2.0.
230 * \param modules Constants for each specific module
231 */
232 SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency,
234 SwerveDrivetrainImpl{drivetrainConstants, odometryUpdateFrequency, std::array{0.1, 0.1, 0.1}, std::array{0.9, 0.9, 0.9}, modules}
235 {}
236
237 /**
238 * \brief Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
239 *
240 * This constructs the underlying hardware devices, so user should not construct
241 * the devices themselves. If they need the devices, they can access them
242 * through getters in the classes.
243 *
244 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
245 * \param odometryUpdateFrequency The frequency to run the odometry loop. If
246 * unspecified or set to 0 Hz, this is 250 Hz on
247 * CAN FD, and 100 Hz on CAN 2.0.
248 * \param odometryStandardDeviation The standard deviation for odometry calculation
249 * \param visionStandardDeviation The standard deviation for vision calculation
250 * \param modules Constants for each specific module
251 */
252 template < typename... ModuleConstants,
253 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
254 SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency,
255 std::array<double, 3> const &odometryStandardDeviation, std::array<double, 3> const &visionStandardDeviation,
256 ModuleConstants const &... modules) :
257 SwerveDrivetrainImpl{drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation,
258 visionStandardDeviation, std::array{modules...}}
259 {}
260 /**
261 * \brief Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
262 *
263 * This constructs the underlying hardware devices, so user should not construct
264 * the devices themselves. If they need the devices, they can access them
265 * through getters in the classes.
266 *
267 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
268 * \param odometryUpdateFrequency The frequency to run the odometry loop. If
269 * unspecified or set to 0 Hz, this is 250 Hz on
270 * CAN FD, and 100 Hz on CAN 2.0.
271 * \param odometryStandardDeviation The standard deviation for odometry calculation
272 * \param visionStandardDeviation The standard deviation for vision calculation
273 * \param modules Constants for each specific module
274 */
275 SwerveDrivetrainImpl(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency,
276 std::array<double, 3> const &odometryStandardDeviation, std::array<double, 3> const &visionStandardDeviation,
278
279 /**
280 * \brief Gets a reference to the data acquisition thread.
281 *
282 * \returns DAQ thread
283 */
285
286 /**
287 * Applies the specified control function to this swerve drivetrain.
288 *
289 * \param request Request function to apply
290 */
292 {
293 std::lock_guard<std::recursive_mutex> lock{_stateLock};
294 if (request) {
295 _requestToApply = std::move(request);
296 } else {
297 _requestToApply = [](auto&, auto&) { return ctre::phoenix::StatusCode::OK; };
298 }
299 }
300
301 /**
302 * Configures the neutral mode to use for all modules' drive motors.
303 *
304 * \param neutralMode The drive motor neutral mode
305 * \returns Status code of the first failed config call, or OK if all succeeded
306 */
308 {
310 for (auto &module : _modules) {
311 auto status = module->ConfigNeutralMode(neutralMode);
312 if (retval.IsOK()) {
313 retval = status;
314 }
315 }
316 return retval;
317 }
318
319 /**
320 * \brief Zero's this swerve drive's odometry entirely.
321 *
322 * This will zero the entire odometry, and place the robot at 0,0
323 */
325 {
326 std::lock_guard<std::recursive_mutex> lock{_stateLock};
327
328 for (size_t i = 0; i < _modules.size(); ++i) {
329 _modules[i]->ResetPosition();
330 _modulePositions[i] = _modules[i]->GetPosition(true);
331 }
333 }
334
335 /**
336 * \brief Takes the current orientation of the robot and makes it X forward for
337 * field-relative maneuvers.
338 */
340 {
341 std::lock_guard<std::recursive_mutex> lock{_stateLock};
343 }
344
345 /**
346 * \brief Takes the specified location and makes it the current pose for
347 * field-relative maneuvers
348 *
349 * \param location Pose to make the current pose
350 */
351 void SeedFieldRelative(Pose2d location)
352 {
353 std::lock_guard<std::recursive_mutex> lock{_stateLock};
354
356 /* We need to update our cached pose immediately to prevent race conditions */
357 _cachedState.Pose = location;
358 }
359
360 /**
361 * Takes the requests#SwerveRequest#ForwardReference#RedAlliance perpective direction
362 * and treats it as the forward direction for
363 * requests#SwerveRequest#ForwardReference#OperatorPerspective.
364 *
365 * If the operator is in the Blue Alliance Station, this should be 0 degrees.
366 * If the operator is in the Red Alliance Station, this should be 180 degrees.
367 *
368 * @param fieldDirection Heading indicating which direction is forward from
369 * the requests#SwerveRequest#ForwardReference#RedAlliance perspective
370 */
371 void SetOperatorPerspectiveForward(Rotation2d fieldDirection)
372 {
373 std::lock_guard<std::recursive_mutex> lock{_stateLock};
374 _operatorForwardDirection = fieldDirection;
375 }
376
377 /**
378 * \brief Check if the odometry is currently valid
379 *
380 * \returns True if odometry is valid
381 */
382 bool IsOdometryValid() const
383 {
384 return _odometryThread->IsOdometryValid();
385 }
386
387 /**
388 * \brief Get a reference to the module at the specified index.
389 * The index corresponds to the module described in the constructor.
390 *
391 * \param index Which module to get
392 * \returns Reference to SwerveModuleImpl
393 */
394 SwerveModuleImpl &GetModule(size_t index) { return *_modules[index]; }
395 /**
396 * \brief Get a reference to the module at the specified index.
397 * The index corresponds to the module described in the constructor.
398 *
399 * \param index Which module to get
400 * \returns Reference to SwerveModuleImpl
401 */
402 SwerveModuleImpl const &GetModule(size_t index) const { return *_modules[index]; }
403 /**
404 * \brief Get a reference to the full array of modules.
405 * The indexes correspond to the module described in the constructor.
406 *
407 * \returns Reference to the SwerveModuleImpl array
408 */
409 std::vector<std::unique_ptr<SwerveModuleImpl>> const &GetModules() const { return _modules; }
410
411 /**
412 * \brief Gets the locations of the swerve modules.
413 *
414 * \returns Reference to the array of swerve module locations
415 */
416 std::vector<Translation2d> const &GetModuleLocations() const { return _moduleLocations; }
417
418 /**
419 * \brief Gets the current state of the swerve drivetrain.
420 *
421 * \returns Current state of the drivetrain
422 */
424 {
425 std::lock_guard<std::recursive_mutex> lock{_stateLock};
426 return _cachedState;
427 }
428
429 /**
430 * \brief Adds a vision measurement to the Kalman Filter. This will correct the
431 * odometry pose estimate while still accounting for measurement noise.
432 *
433 * This method can be called as infrequently as you want, as long as you are
434 * calling impl#SwerveDrivePoseEstimator#Update every loop.
435 *
436 * To promote stability of the pose estimate and make it robust to bad vision
437 * data, we recommend only adding vision measurements that are already within
438 * one meter or so of the current pose estimate.
439 *
440 * \param visionRobotPose The pose of the robot as measured by the
441 * vision camera.
442 * \param timestamp The timestamp of the vision measurement in
443 * seconds. Note that if you don't use your
444 * own time source by calling
445 * impl#SwerveDrivePoseEstimator#UpdateWithTime,
446 * then you must use a timestamp with an epoch
447 * since system startup (i.e., the epoch of this
448 * timestamp is the same epoch as
449 * GetCurrentTimeSeconds).
450 * This means that you should use
451 * GetCurrentTimeSeconds as
452 * your time source in this case.
453 */
454 void AddVisionMeasurement(Pose2d visionRobotPose, units::second_t timestamp)
455 {
456 std::lock_guard<std::recursive_mutex> lock{_stateLock};
457 _odometry.AddVisionMeasurement(visionRobotPose, timestamp);
458 }
459
460 /**
461 * \brief Adds a vision measurement to the Kalman Filter. This will correct the
462 * odometry pose estimate while still accounting for measurement noise.
463 *
464 * This method can be called as infrequently as you want, as long as you are
465 * calling impl#SwerveDrivePoseEstimator#Update every loop.
466 *
467 * To promote stability of the pose estimate and make it robust to bad vision
468 * data, we recommend only adding vision measurements that are already within
469 * one meter or so of the current pose estimate.
470 *
471 * Note that the vision measurement standard deviations passed into this method
472 * will continue to apply to future measurements until a subsequent call to
473 * #SetVisionMeasurementStdDevs or this method.
474 *
475 * \param visionRobotPose The pose of the robot as measured by the
476 * vision camera.
477 * \param timestamp The timestamp of the vision measurement in
478 * seconds. Note that if you don't use your
479 * own time source by calling
480 * impl#SwerveDrivePoseEstimator#UpdateWithTime,
481 * then you must use a timestamp with an epoch
482 * since system startup (i.e., the epoch of this
483 * timestamp is the same epoch as
484 * GetCurrentTimeSeconds).
485 * This means that you should use
486 * GetCurrentTimeSeconds as
487 * your time source in this case.
488 * \param visionMeasurementStdDevs Standard deviations of the vision pose
489 * measurement (x position in meters, y position
490 * in meters, and heading in radians). Increase
491 * these numbers to trust the vision pose
492 * measurement less.
493 */
495 Pose2d visionRobotPose,
496 units::second_t timestamp,
497 std::array<double, 3> visionMeasurementStdDevs)
498 {
499 std::lock_guard<std::recursive_mutex> lock{_stateLock};
500 _odometry.AddVisionMeasurement(visionRobotPose, timestamp, visionMeasurementStdDevs);
501 }
502
503 /**
504 * \brief Sets the pose estimator's trust of global measurements. This might be used to
505 * change trust in vision measurements after the autonomous period, or to change
506 * trust as distance to a vision target increases.
507 *
508 * \param visionMeasurementStdDevs Standard deviations of the vision
509 * measurements. Increase these
510 * numbers to trust global measurements from
511 * vision less. This matrix is in the form [x,
512 * y, theta]ᵀ, with units in meters and radians.
513 */
514 void SetVisionMeasurementStdDevs(std::array<double, 3> visionMeasurementStdDevs)
515 {
516 std::lock_guard<std::recursive_mutex> lock{_stateLock};
517 _odometry.SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
518 }
519
520 /**
521 * \brief Register the specified lambda to be executed whenever the SwerveDriveState
522 * is updated in the odometry thread.
523 *
524 * It is imperative that this function is cheap, as it will be executed synchronously
525 * with the odometry call; if this takes a long time, it may negatively impact the
526 * odometry of this stack.
527 *
528 * This can also be used for logging data if the function performs logging instead of telemetry.
529 *
530 * \param telemetryFunction Function to call for telemetry or logging
531 */
532 void RegisterTelemetry(std::function<void(SwerveDriveState const &)> telemetryFunction)
533 {
534 std::lock_guard<std::recursive_mutex> lock{_stateLock};
535 _telemetryFunction = std::move(telemetryFunction);
536 }
537
538 /**
539 * \brief Gets this drivetrain's Pigeon 2 reference.
540 *
541 * This should be used only to access signals and change configurations that the
542 * swerve drivetrain does not configure itself.
543 *
544 * \returns This drivetrain's Pigeon 2 reference
545 */
547};
548
549}
550}
551}
552}
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()
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
Definition: span.hpp:134
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
Definition: span.hpp:401
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