CTRE Phoenix 6 C++ 24.50.0-alpha-2
SwerveDrivetrain.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
15namespace ctre {
16namespace phoenix6 {
17namespace swerve {
18
19/**
20 * \brief Swerve Drive class utilizing CTR Electronics Phoenix 6 API.
21 *
22 * This class handles the kinematics, configuration, and odometry of a
23 * swerve drive utilizing CTR Electronics devices. We recommend
24 * that users use the Swerve Mechanism Generator in Tuner X to create
25 * a template project that demonstrates how to use this class.
26 *
27 * This class will construct the hardware devices internally, so the user
28 * only specifies the constants (IDs, PID gains, gear ratios, etc).
29 * Getters for these hardware devices are available.
30 *
31 * If using the generator, the order in which modules are constructed is
32 * Front Left, Front Right, Back Left, Back Right. This means if you need
33 * the Back Left module, call \c GetModule(2); to get the 3rd index
34 * (0-indexed) module, corresponding to the Back Left module.
35 */
37public:
38 /** \brief Performs swerve module updates in a separate thread to minimize latency. */
40
41 /**
42 * \brief Plain-Old-Data class holding the state of the swerve drivetrain.
43 * This encapsulates most data that is relevant for telemetry or
44 * decision-making from the Swerve Drive.
45 */
47
48protected:
50 std::vector<std::unique_ptr<SwerveModule>> _modules;
51
54
55public:
56 /**
57 * \brief Constructs a CTRE SwerveDrivetrain using the specified constants.
58 *
59 * This constructs the underlying hardware devices, so user should not construct
60 * the devices themselves. If they need the devices, they can access them
61 * through getters in the classes.
62 *
63 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
64 * \param modules Constants for each specific module
65 */
66 template < typename... ModuleConstants,
67 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
68 SwerveDrivetrain(SwerveDrivetrainConstants const &drivetrainConstants, ModuleConstants const &... modules) :
69 SwerveDrivetrain{drivetrainConstants, 0_Hz, modules...}
70 {}
71
72 /**
73 * \brief Constructs a CTRE SwerveDrivetrain using the specified constants.
74 *
75 * This constructs the underlying hardware devices, so user should not construct
76 * the devices themselves. If they need the devices, they can access them
77 * through getters in the classes.
78 *
79 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
80 * \param odometryUpdateFrequency The frequency to run the odometry loop. If
81 * unspecified or set to 0 Hz, this is 250 Hz on
82 * CAN FD, and 100 Hz on CAN 2.0.
83 * \param modules Constants for each specific module
84 */
85 template < typename... ModuleConstants,
86 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
87 SwerveDrivetrain(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency,
88 ModuleConstants const &... modules) :
89 SwerveDrivetrain{drivetrainConstants, odometryUpdateFrequency, std::array{0.1, 0.1, 0.1}, std::array{0.9, 0.9, 0.9}, modules...}
90 {}
91
92 /**
93 * \brief Constructs a CTRE SwerveDrivetrain using the specified constants.
94 *
95 * This constructs the underlying hardware devices, so user should not construct
96 * the devices themselves. If they need the devices, they can access them
97 * through getters in the classes.
98 *
99 * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
100 * \param odometryUpdateFrequency The frequency to run the odometry loop. If
101 * unspecified or set to 0 Hz, this is 250 Hz on
102 * CAN FD, and 100 Hz on CAN 2.0.
103 * \param odometryStandardDeviation The standard deviation for odometry calculation
104 * \param visionStandardDeviation The standard deviation for vision calculation
105 * \param modules Constants for each specific module
106 */
107 template < typename... ModuleConstants,
108 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
109 SwerveDrivetrain(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency,
110 std::array<double, 3> const &odometryStandardDeviation, std::array<double, 3> const &visionStandardDeviation,
111 ModuleConstants const &... modules) :
112 _drivetrain{drivetrainConstants, odometryUpdateFrequency,
113 odometryStandardDeviation, visionStandardDeviation,
114 modules...},
115 _modules{CreateModuleArray(drivetrainConstants.CANBusName, modules...)},
116 _pigeon2{drivetrainConstants.Pigeon2Id, std::string{drivetrainConstants.CANBusName}},
117 _simDrive{_drivetrain.GetModuleLocations(), _pigeon2.GetSimState(), modules...}
118 {
119 if (drivetrainConstants.Pigeon2Configs) {
120 auto retval = GetPigeon2().GetConfigurator().Apply(*drivetrainConstants.Pigeon2Configs);
121 if (!retval.IsOK()) {
122 printf("Pigeon2 ID %d failed config with error: %s\n", GetPigeon2().GetDeviceID(), retval.GetName());
123 }
124 }
125 /* do not start thread until after applying Pigeon 2 configs */
127 }
128
129 virtual ~SwerveDrivetrain() = default;
130
131private:
132 template <typename... ModuleConstants, size_t... Idxs>
133 std::vector<std::unique_ptr<SwerveModule>> CreateModuleArrayHelper(std::string_view canbusName, std::index_sequence<Idxs...>, ModuleConstants const &... constants)
134 {
135 static_assert(sizeof...(ModuleConstants) == sizeof...(Idxs));
136 std::vector<std::unique_ptr<SwerveModule>> modules;
137 modules.reserve(sizeof...(ModuleConstants));
138 (modules.emplace_back(std::make_unique<SwerveModule>(constants, canbusName, _drivetrain.GetModule(Idxs))), ...);
139 return modules;
140 }
141
142 template <typename... ModuleConstants>
143 std::vector<std::unique_ptr<SwerveModule>> CreateModuleArray(std::string_view canbusName, ModuleConstants const &... constants)
144 {
145 return CreateModuleArrayHelper(canbusName, std::make_index_sequence<sizeof...(ModuleConstants)>{}, constants...);
146 }
147
148public:
149 /**
150 * Updates all the simulation state variables for this
151 * drivetrain class. User provides the update variables for the simulation.
152 *
153 * \param dt time since last update call
154 * \param supplyVoltage voltage as seen at the motor controllers
155 */
156 void UpdateSimState(units::second_t dt, units::volt_t supplyVoltage)
157 {
158 _simDrive.Update(dt, supplyVoltage, _modules);
159 }
160
161 /**
162 * \brief Gets a reference to the data acquisition thread.
163 *
164 * \returns DAQ thread
165 */
167 {
168 return _drivetrain.GetDaqThread();
169 }
170
171 /**
172 * Applies the specified control request to this swerve drivetrain.
173 *
174 * \param request Request to apply
175 */
176 template < typename Request,
177 typename = std::enable_if_t< std::is_base_of_v<requests::SwerveRequest, std::remove_reference_t<Request>> > >
178 void SetControl(Request &&request)
179 {
181 [this, request=std::forward<Request>(request)](auto const &params, auto const &modules) mutable {
182 return request.Apply(params, modules);
183 }
184 );
185 }
186
187 /**
188 * Configures the neutral mode to use for all modules' drive motors.
189 *
190 * \param neutralMode The drive motor neutral mode
191 * \returns Status code of the first failed config call, or OK if all succeeded
192 */
194 {
195 return _drivetrain.ConfigNeutralMode(neutralMode);
196 }
197
198 /**
199 * \brief Zero's this swerve drive's odometry entirely.
200 *
201 * This will zero the entire odometry, and place the robot at 0,0
202 */
204 {
206 }
207
208 /**
209 * \brief Takes the current orientation of the robot and makes it X forward for
210 * field-relative maneuvers.
211 */
213 {
215 }
216
217 /**
218 * \brief Takes the specified location and makes it the current pose for
219 * field-relative maneuvers
220 *
221 * \param location Pose to make the current pose
222 */
223 void SeedFieldRelative(frc::Pose2d location)
224 {
225 _drivetrain.SeedFieldRelative(std::move(location));
226 }
227
228 /**
229 * Takes the requests#SwerveRequest#ForwardReference#RedAlliance perpective direction
230 * and treats it as the forward direction for
231 * requests#SwerveRequest#ForwardReference#OperatorPerspective.
232 *
233 * If the operator is in the Blue Alliance Station, this should be 0 degrees.
234 * If the operator is in the Red Alliance Station, this should be 180 degrees.
235 *
236 * @param fieldDirection Heading indicating which direction is forward from
237 * the requests#SwerveRequest#ForwardReference#RedAlliance perspective
238 */
239 void SetOperatorPerspectiveForward(frc::Rotation2d fieldDirection)
240 {
242 }
243
244 /**
245 * \brief Check if the odometry is currently valid
246 *
247 * \returns True if odometry is valid
248 */
249 bool IsOdometryValid() const
250 {
252 }
253
254 /**
255 * \brief Get a reference to the module at the specified index.
256 * The index corresponds to the module described in the constructor.
257 *
258 * \param index Which module to get
259 * \returns Reference to SwerveModule
260 */
261 SwerveModule &GetModule(size_t index) { return *_modules.at(index); }
262 /**
263 * \brief Get a reference to the module at the specified index.
264 * The index corresponds to the module described in the constructor.
265 *
266 * \param index Which module to get
267 * \returns Reference to SwerveModule
268 */
269 SwerveModule const &GetModule(size_t index) const { return *_modules.at(index); }
270 /**
271 * \brief Get a reference to the full array of modules.
272 * The indexes correspond to the module described in the constructor.
273 *
274 * \returns Reference to the SwerveModule array
275 */
276 std::vector<std::unique_ptr<SwerveModule>> const &GetModules() const { return _modules; }
277
278 /**
279 * \brief Gets the locations of the swerve modules.
280 *
281 * \returns Reference to the array of swerve module locations
282 */
283 std::vector<Translation2d> const &GetModuleLocations() const { return _drivetrain.GetModuleLocations(); }
284
285 /**
286 * \brief Gets the current state of the swerve drivetrain.
287 *
288 * \returns Current state of the drivetrain
289 */
291 {
292 return _drivetrain.GetState();
293 }
294
295 /**
296 * \brief Adds a vision measurement to the Kalman Filter. This will correct the
297 * odometry pose estimate while still accounting for measurement noise.
298 *
299 * This method can be called as infrequently as you want, as long as you are
300 * calling impl#SwerveDrivePoseEstimator#Update every loop.
301 *
302 * To promote stability of the pose estimate and make it robust to bad vision
303 * data, we recommend only adding vision measurements that are already within
304 * one meter or so of the current pose estimate.
305 *
306 * \param visionRobotPose The pose of the robot as measured by the
307 * vision camera.
308 * \param timestamp The timestamp of the vision measurement in
309 * seconds. Note that if you don't use your
310 * own time source by calling
311 * impl#SwerveDrivePoseEstimator#UpdateWithTime,
312 * then you must use a timestamp with an epoch
313 * since system startup (i.e., the epoch of this
314 * timestamp is the same epoch as
315 * GetCurrentTimeSeconds).
316 * This means that you should use
317 * GetCurrentTimeSeconds as
318 * your time source in this case.
319 */
320 void AddVisionMeasurement(frc::Pose2d visionRobotPose, units::second_t timestamp)
321 {
322 _drivetrain.AddVisionMeasurement(std::move(visionRobotPose), timestamp);
323 }
324
325 /**
326 * \brief Adds a vision measurement to the Kalman Filter. This will correct the
327 * odometry pose estimate while still accounting for measurement noise.
328 *
329 * This method can be called as infrequently as you want, as long as you are
330 * calling impl#SwerveDrivePoseEstimator#Update every loop.
331 *
332 * To promote stability of the pose estimate and make it robust to bad vision
333 * data, we recommend only adding vision measurements that are already within
334 * one meter or so of the current pose estimate.
335 *
336 * Note that the vision measurement standard deviations passed into this method
337 * will continue to apply to future measurements until a subsequent call to
338 * #SetVisionMeasurementStdDevs or this method.
339 *
340 * \param visionRobotPose The pose of the robot as measured by the
341 * vision camera.
342 * \param timestamp The timestamp of the vision measurement in
343 * seconds. Note that if you don't use your
344 * own time source by calling
345 * impl#SwerveDrivePoseEstimator#UpdateWithTime,
346 * then you must use a timestamp with an epoch
347 * since system startup (i.e., the epoch of this
348 * timestamp is the same epoch as
349 * GetCurrentTimeSeconds).
350 * This means that you should use
351 * GetCurrentTimeSeconds as
352 * your time source in this case.
353 * \param visionMeasurementStdDevs Standard deviations of the vision pose
354 * measurement (x position in meters, y position
355 * in meters, and heading in radians). Increase
356 * these numbers to trust the vision pose
357 * measurement less.
358 */
360 frc::Pose2d visionRobotPose,
361 units::second_t timestamp,
362 std::array<double, 3> visionMeasurementStdDevs)
363 {
364 _drivetrain.AddVisionMeasurement(std::move(visionRobotPose), timestamp, visionMeasurementStdDevs);
365 }
366
367 /**
368 * \brief Sets the pose estimator's trust of global measurements. This might be used to
369 * change trust in vision measurements after the autonomous period, or to change
370 * trust as distance to a vision target increases.
371 *
372 * \param visionMeasurementStdDevs Standard deviations of the vision
373 * measurements. Increase these
374 * numbers to trust global measurements from
375 * vision less. This matrix is in the form [x,
376 * y, theta]ᵀ, with units in meters and radians.
377 */
378 void SetVisionMeasurementStdDevs(std::array<double, 3> visionMeasurementStdDevs)
379 {
380 _drivetrain.SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
381 }
382
383 /**
384 * \brief Register the specified lambda to be executed whenever the SwerveDriveState
385 * is updated in the odometry thread.
386 *
387 * It is imperative that this function is cheap, as it will be executed synchronously
388 * with the odometry call; if this takes a long time, it may negatively impact the
389 * odometry of this stack.
390 *
391 * This can also be used for logging data if the function performs logging instead of telemetry.
392 *
393 * \param telemetryFunction Function to call for telemetry or logging
394 */
395 void RegisterTelemetry(std::function<void(SwerveDriveState const &)> telemetryFunction)
396 {
397 _drivetrain.RegisterTelemetry(std::move(telemetryFunction));
398 }
399
400 /**
401 * \brief Gets the current orientation of the robot as a frc#Rotation3d from
402 * the Pigeon 2 quaternion values.
403 *
404 * \returns The robot orientation as a frc#Rotation3d
405 */
406 frc::Rotation3d GetRotation3d() const
407 {
408 return _pigeon2.GetRotation3d();
409 }
410
411 /**
412 * \brief Gets this drivetrain's Pigeon 2 reference.
413 *
414 * This should be used only to access signals and change configurations that the
415 * swerve drivetrain does not configure itself.
416 *
417 * \returns This drivetrain's Pigeon 2 reference
418 */
420 {
421 return _pigeon2;
422 }
423};
424
425}
426}
427}
ctre::phoenix::StatusCode Apply(const Pigeon2Configuration &configs)
Applies the contents of the specified config to the device.
Definition: CorePigeon2.hpp:269
Class description for the Pigeon 2 IMU sensor that measures orientation.
Definition: Pigeon2.hpp:29
frc::Rotation3d GetRotation3d() const
Returns the orientation of the robot as a frc::Rotation3d created from the quaternion signals.
configs::Pigeon2Configurator & GetConfigurator()
Gets the configurator for this Pigeon2.
Definition: CorePigeon2.hpp:1115
The state of the motor controller bridge when output is neutral or disabled.
Definition: SpnEnums.hpp:1698
Simplified swerve drive simulation class.
Definition: SimSwerveDrivetrain.hpp:33
void Update(units::second_t dt, units::volt_t supplyVoltage, std::vector< std::unique_ptr< SwerveModule > > const &modulesToApply)
Definition: SimSwerveDrivetrain.hpp:97
Swerve Drive class utilizing CTR Electronics Phoenix 6 API.
Definition: SwerveDrivetrain.hpp:36
void TareEverything()
Zero's this swerve drive's odometry entirely.
Definition: SwerveDrivetrain.hpp:203
std::vector< std::unique_ptr< SwerveModule > > _modules
Definition: SwerveDrivetrain.hpp:50
void AddVisionMeasurement(frc::Pose2d visionRobotPose, units::second_t timestamp, std::array< double, 3 > visionMeasurementStdDevs)
Adds a vision measurement to the Kalman Filter.
Definition: SwerveDrivetrain.hpp:359
SwerveDrivetrain(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 SwerveDrivetrain using the specified constants.
Definition: SwerveDrivetrain.hpp:109
SimSwerveDrivetrain _simDrive
Definition: SwerveDrivetrain.hpp:53
SwerveModule & GetModule(size_t index)
Get a reference to the module at the specified index.
Definition: SwerveDrivetrain.hpp:261
hardware::Pigeon2 & GetPigeon2()
Gets this drivetrain's Pigeon 2 reference.
Definition: SwerveDrivetrain.hpp:419
void SetVisionMeasurementStdDevs(std::array< double, 3 > visionMeasurementStdDevs)
Sets the pose estimator's trust of global measurements.
Definition: SwerveDrivetrain.hpp:378
void SetOperatorPerspectiveForward(frc::Rotation2d fieldDirection)
Takes the requests::SwerveRequest::ForwardReference::RedAlliance perpective direction and treats it a...
Definition: SwerveDrivetrain.hpp:239
std::vector< std::unique_ptr< SwerveModule > > const & GetModules() const
Get a reference to the full array of modules.
Definition: SwerveDrivetrain.hpp:276
void RegisterTelemetry(std::function< void(SwerveDriveState const &)> telemetryFunction)
Register the specified lambda to be executed whenever the SwerveDriveState is updated in the odometry...
Definition: SwerveDrivetrain.hpp:395
ctre::phoenix::StatusCode ConfigNeutralMode(signals::NeutralModeValue neutralMode)
Configures the neutral mode to use for all modules' drive motors.
Definition: SwerveDrivetrain.hpp:193
bool IsOdometryValid() const
Check if the odometry is currently valid.
Definition: SwerveDrivetrain.hpp:249
void SeedFieldRelative()
Takes the current orientation of the robot and makes it X forward for field-relative maneuvers.
Definition: SwerveDrivetrain.hpp:212
SwerveDrivetrain(SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, ModuleConstants const &... modules)
Constructs a CTRE SwerveDrivetrain using the specified constants.
Definition: SwerveDrivetrain.hpp:87
void SeedFieldRelative(frc::Pose2d location)
Takes the specified location and makes it the current pose for field-relative maneuvers.
Definition: SwerveDrivetrain.hpp:223
void SetControl(Request &&request)
Applies the specified control request to this swerve drivetrain.
Definition: SwerveDrivetrain.hpp:178
frc::Rotation3d GetRotation3d() const
Gets the current orientation of the robot as a frc::Rotation3d from the Pigeon 2 quaternion values.
Definition: SwerveDrivetrain.hpp:406
OdometryThread & GetDaqThread()
Gets a reference to the data acquisition thread.
Definition: SwerveDrivetrain.hpp:166
SwerveDriveState GetState() const
Gets the current state of the swerve drivetrain.
Definition: SwerveDrivetrain.hpp:290
SwerveModule const & GetModule(size_t index) const
Get a reference to the module at the specified index.
Definition: SwerveDrivetrain.hpp:269
SwerveDrivetrain(SwerveDrivetrainConstants const &drivetrainConstants, ModuleConstants const &... modules)
Constructs a CTRE SwerveDrivetrain using the specified constants.
Definition: SwerveDrivetrain.hpp:68
void UpdateSimState(units::second_t dt, units::volt_t supplyVoltage)
Updates all the simulation state variables for this drivetrain class.
Definition: SwerveDrivetrain.hpp:156
std::vector< Translation2d > const & GetModuleLocations() const
Gets the locations of the swerve modules.
Definition: SwerveDrivetrain.hpp:283
hardware::Pigeon2 _pigeon2
Definition: SwerveDrivetrain.hpp:52
void AddVisionMeasurement(frc::Pose2d visionRobotPose, units::second_t timestamp)
Adds a vision measurement to the Kalman Filter.
Definition: SwerveDrivetrain.hpp:320
impl::SwerveDrivetrainImpl _drivetrain
Definition: SwerveDrivetrain.hpp:49
Swerve Module class that encapsulates a swerve module powered by CTR Electronics devices.
Definition: SwerveModule.hpp:28
Performs swerve module updates in a separate thread to minimize latency.
Definition: SwerveDrivetrainImpl.hpp:33
void Start()
Definition: SwerveDrivetrainImpl.hpp:67
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
void SetControl(SwerveRequestFunc &&request)
Applies the specified control function to this swerve drivetrain.
Definition: SwerveDrivetrainImpl.hpp:291
std::vector< Translation2d > const & GetModuleLocations() const
Gets the locations of the swerve modules.
Definition: SwerveDrivetrainImpl.hpp:416
SwerveDriveState GetState() const
Gets the current state of the swerve drivetrain.
Definition: SwerveDrivetrainImpl.hpp:423
bool IsOdometryValid() const
Check if the odometry is currently valid.
Definition: SwerveDrivetrainImpl.hpp:382
friend class OdometryThread
Definition: SwerveDrivetrainImpl.hpp:139
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
ctre::phoenix::StatusCode ConfigNeutralMode(signals::NeutralModeValue neutralMode)
Configures the neutral mode to use for all modules' drive motors.
Definition: SwerveDrivetrainImpl.hpp:307
SwerveModuleImpl & GetModule(size_t index)
Get a reference to the module at the specified index.
Definition: SwerveDrivetrainImpl.hpp:394
OdometryThread & GetDaqThread()
Gets a reference to the data acquisition thread.
Definition: SwerveDrivetrainImpl.hpp:284
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
void AddVisionMeasurement(Pose2d visionRobotPose, units::second_t timestamp)
Adds a vision measurement to the Kalman Filter.
Definition: SwerveDrivetrainImpl.hpp:454
Status codes reported by APIs, including OK, warnings, and errors.
Definition: StatusCodes.h:27
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
std::optional< configs::Pigeon2Configuration > Pigeon2Configs
The configuration object to apply to the Pigeon2.
Definition: SwerveDrivetrainConstants.hpp:43
Plain-Old-Data class holding the state of the swerve drivetrain.
Definition: SwerveDrivetrainImpl.hpp:104