CTRE Phoenix 6 C++ 25.2.1
Loading...
Searching...
No Matches
SwerveModuleImpl.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
12#include "units/force.h"
13#include "units/torque.h"
14#include <array>
15
16namespace ctre {
17namespace phoenix6 {
18namespace swerve {
19namespace impl {
20
21/**
22 * \brief All possible control requests for the module steer motor.
23 */
24enum class SteerRequestType {
25 /**
26 * \brief Control the drive motor using a Motion Magic® Expo request.
27 * The control output type is determined by SwerveModuleConstants#SteerMotorClosedLoopOutput.
28 */
30 /**
31 * \brief Control the drive motor using an unprofiled position request.
32 * The control output type is determined by SwerveModuleConstants#SteerMotorClosedLoopOutput.
33 */
34 Position = 1,
35};
36
37/**
38 * \brief All possible control requests for the module drive motor.
39 */
40enum class DriveRequestType {
41 /**
42 * \brief Control the drive motor using an open-loop voltage request.
43 */
45 /**
46 * \brief Control the drive motor using a velocity closed-loop request.
47 * The control output type is determined by SwerveModuleConstants#DriveMotorClosedLoopOutput.
48 */
49 Velocity = 1,
50};
51
52/**
53 * \brief Swerve Module class that encapsulates a swerve module powered by
54 * CTR Electronics devices.
55 *
56 * This class handles the hardware devices but does not configure them for
57 * swerve module operation using the Phoenix 6 API. Users should create a
58 * high-level SwerveModule instead of using this directly.
59 */
61public:
62 /**
63 * \brief Contains everything the swerve module needs to apply a request.
64 */
66 /**
67 * \brief Unoptimized speed and direction the module should target.
68 */
69 SwerveModuleState State{};
70
71 /**
72 * \brief Robot-centric wheel force feedforward to apply in the
73 * X direction. X is defined as forward according to WPILib
74 * convention, so this determines the forward force to apply.
75 *
76 * This force should include friction applied to the ground.
77 */
78 units::newton_t WheelForceFeedforwardX = 0_N;
79 /**
80 * \brief Robot-centric wheel force feedforward to apply in the
81 * Y direction. Y is defined as to the left according to WPILib
82 * convention, so this determines the force to apply to the left.
83 *
84 * This force should include friction applied to the ground.
85 */
86 units::newton_t WheelForceFeedforwardY = 0_N;
87
88 /**
89 * \brief The type of control request to use for the drive motor.
90 */
92 /**
93 * \brief The type of control request to use for the steer motor.
94 */
96
97 /**
98 * \brief The update period of the module request. Setting this to
99 * a non-zero value adds a velocity feedforward to the steer motor.
100 */
101 units::second_t UpdatePeriod = 0_s;
102
103 /**
104 * \brief When using Voltage-based control, set to true (default) to use FOC
105 * commutation (requires Phoenix Pro), which increases peak power by ~15%. Set to
106 * false to use trapezoidal commutation. This is ignored when using Torque-based
107 * control, which always uses FOC.
108 *
109 * FOC improves motor performance by leveraging torque (current) control.
110 * However, this may be inconvenient for applications that require specifying
111 * duty cycle or voltage. CTR-Electronics has developed a hybrid method that
112 * combines the performances gains of FOC while still allowing applications to
113 * provide duty cycle or voltage demand. This not to be confused with simple
114 * sinusoidal control or phase voltage control which lacks the performance
115 * gains.
116 */
117 bool EnableFOC = true;
118
119 /**
120 * \brief Modifies the State parameter and returns itself.
121 *
122 * Unoptimized speed and direction the module should target.
123 *
124 * \param newState Parameter to modify
125 * \returns Itself
126 */
127 ModuleRequest &WithState(SwerveModuleState newState)
128 {
129 this->State = std::move(newState);
130 return *this;
131 }
132
133 /**
134 * \brief Modifies the WheelForceFeedforwardX parameter and returns itself.
135 *
136 * Robot-centric wheel force feedforward to apply in the
137 * X direction. X is defined as forward according to WPILib
138 * convention, so this determines the forward force to apply.
139 *
140 * This force should include friction.
141 *
142 * \param newWheelForceFeedforwardX Parameter to modify
143 * \returns Itself
144 */
145 ModuleRequest &WithWheelForceFeedforwardX(units::newton_t newWheelForceFeedforwardX)
146 {
147 this->WheelForceFeedforwardX = newWheelForceFeedforwardX;
148 return *this;
149 }
150 /**
151 * \brief Modifies the WheelForceFeedforwardY parameter and returns itself.
152 *
153 * Robot-centric wheel force feedforward to apply in the
154 * Y direction. Y is defined as to the left according to WPILib
155 * convention, so this determines the force to apply to the left.
156 *
157 * This force should include friction.
158 *
159 * \param newWheelForceFeedforwardY Parameter to modify
160 * \returns Itself
161 */
162 ModuleRequest &WithWheelForceFeedforwardY(units::newton_t newWheelForceFeedforwardY)
163 {
164 this->WheelForceFeedforwardY = newWheelForceFeedforwardY;
165 return *this;
166 }
167
168 /**
169 * \brief Modifies the DriveRequest parameter and returns itself.
170 *
171 * The type of control request to use for the drive motor.
172 *
173 * \param newDriveRequest Parameter to modify
174 * \returns Itself
175 */
177 {
178 this->DriveRequest = newDriveRequest;
179 return *this;
180 }
181 /**
182 * \brief Modifies the SteerRequest parameter and returns itself.
183 *
184 * The type of control request to use for the steer motor.
185 *
186 * \param newSteerRequest Parameter to modify
187 * \returns Itself
188 */
190 {
191 this->SteerRequest = newSteerRequest;
192 return *this;
193 }
194
195 /**
196 * \brief Modifies the UpdatePeriod parameter and returns itself.
197 *
198 * The update period of the module request. Setting this to a
199 * non-zero value adds a velocity feedforward to the steer motor.
200 *
201 * \param newUpdatePeriod Parameter to modify
202 * \returns Itself
203 */
204 ModuleRequest &WithUpdatePeriod(units::second_t newUpdatePeriod)
205 {
206 this->UpdatePeriod = newUpdatePeriod;
207 return *this;
208 }
209
210 /**
211 * \brief Modifies the EnableFOC parameter and returns itself.
212 *
213 * When using Voltage-based control, set to true (default) to use FOC commutation
214 * (requires Phoenix Pro), which increases peak power by ~15%. Set to false to
215 * use trapezoidal commutation. This is ignored when using Torque-based control,
216 * which always uses FOC.
217 *
218 * FOC improves motor performance by leveraging torque (current) control.
219 * However, this may be inconvenient for applications that require specifying
220 * duty cycle or voltage. CTR-Electronics has developed a hybrid method that
221 * combines the performances gains of FOC while still allowing applications to
222 * provide duty cycle or voltage demand. This not to be confused with simple
223 * sinusoidal control or phase voltage control which lacks the performance
224 * gains.
225 *
226 * \param newEnableFOC Parameter to modify
227 * \returns Itself
228 */
229 ModuleRequest &WithEnableFOC(bool newEnableFOC)
230 {
231 this->EnableFOC = newEnableFOC;
232 return *this;
233 }
234 };
235
236private:
237 std::unique_ptr<hardware::traits::CommonTalon> _driveMotor;
238 std::unique_ptr<hardware::traits::CommonTalon> _steerMotor;
239 hardware::traits::CommonTalonWithFOC *_driveMotorFOC{};
240 hardware::traits::CommonTalonWithFOC *_steerMotorFOC{};
241
242 StatusSignal<units::turn_t> _drivePosition;
244 StatusSignal<units::turn_t> _steerPosition;
246
248 mutable StatusSignal<units::ampere_t> _driveMotorStallCurrent;
249
250 ClosedLoopOutputType _driveClosedLoopOutput;
251 ClosedLoopOutputType _steerClosedLoopOutput;
252
253 using turns_per_meter = units::compound_unit<units::turns, units::inverse<units::meters>>;
254 using turns_per_meter_t = units::unit_t<turns_per_meter>;
255
256 turns_per_meter_t kDriveRotationsPerMeter;
257 units::meter_t kDriveNmPerWheelN;
258 units::scalar_t kCouplingRatioDriveRotorToEncoder;
259 units::meters_per_second_t kSpeedAt12Volts;
260
261 bool _isOnCANFD;
262
263 SwerveModulePosition _currentPosition;
264 SwerveModuleState _targetState;
265
266public:
267 /**
268 * \brief Construct a SwerveModuleImpl with the specified constants.
269 *
270 * \param constants Constants used to construct the module
271 * \param canbus The CAN bus this module is on
272 */
273 template <typename DriveMotorConfigsT, typename SteerMotorConfigsT, typename EncoderConfigsT>
276 CANBus canbus
277 );
278
279 /**
280 * \brief Applies the desired ModuleRequest to this module.
281 *
282 * \param moduleRequest The request to apply to this module
283 */
284 void Apply(ModuleRequest const &moduleRequest);
285
286 /**
287 * \brief Controls this module using the specified drive and steer control requests.
288 *
289 * This is intended only to be used for characterization of the robot; do not use this for normal use.
290 *
291 * \param driveRequest The control request to apply to the drive motor
292 * \param steerRequest The control request to apply to the steer motor
293 */
294 template <typename DriveReq, typename SteerReq>
295 void Apply(DriveReq &&driveRequest, SteerReq &&steerRequest)
296 {
297 if (_driveMotorFOC) {
298 _driveMotorFOC->SetControl(driveRequest.WithUpdateFreqHz(0_Hz));
299 } else {
300 _driveMotor->SetControl(driveRequest.WithUpdateFreqHz(0_Hz));
301 }
302
303 if (_steerMotorFOC) {
304 _steerMotorFOC->SetControl(steerRequest.WithUpdateFreqHz(0_Hz));
305 } else {
306 _steerMotor->SetControl(steerRequest.WithUpdateFreqHz(0_Hz));
307 }
308 }
309
310 /**
311 * \brief Configures the neutral mode to use for the module's drive motor.
312 *
313 * \param neutralMode The drive motor neutral mode
314 * \param timeoutSeconds Maximum amount of time to wait when performing configuration
315 * \returns Status code response of the request
316 */
317 ctre::phoenix::StatusCode ConfigNeutralMode(signals::NeutralModeValue neutralMode, units::second_t timeoutSeconds = 0.100_s);
318
319 /**
320 * \brief Gets the state of this module and passes it back as a
321 * SwerveModulePosition object with latency compensated values.
322 *
323 * This function is blocking when it performs a refresh.
324 *
325 * \param refresh True if the signals should be refreshed
326 * \returns SwerveModulePosition containing this module's state.
327 */
328 SwerveModulePosition GetPosition(bool refresh);
329
330 /**
331 * \brief Gets the last cached swerve module position.
332 * This differs from #GetPosition in that it will not
333 * perform any latency compensation or refresh the signals.
334 *
335 * \returns Last cached SwerveModulePosition
336 */
337 SwerveModulePosition GetCachedPosition() const { return _currentPosition; }
338
339 /**
340 * \brief Get the current state of the module.
341 *
342 * This is typically used for telemetry, as the SwerveModulePosition
343 * is used for odometry.
344 *
345 * \returns Current state of the module
346 */
347 SwerveModuleState GetCurrentState() const
348 {
349 return SwerveModuleState{_driveVelocity.GetValue() / kDriveRotationsPerMeter, {_steerPosition.GetValue()}};
350 }
351
352 /**
353 * \brief Get the target state of the module.
354 *
355 * This is typically used for telemetry.
356 *
357 * \returns Target state of the module
358 */
359 SwerveModuleState GetTargetState() const { return _targetState; }
360
361 /**
362 * \brief Resets this module's drive motor position to 0 rotations.
363 */
365 {
366 /* Only touch drive pos, not steer */
367 _driveMotor->SetPosition(0_tr);
368 }
369
370 /**
371 * \brief Gets the closed-loop output type to use for the drive motor.
372 *
373 * \returns Drive motor closed-loop output type
374 */
376 {
377 return _driveClosedLoopOutput;
378 }
379
380 /**
381 * \brief Gets the closed-loop output type to use for the steer motor.
382 *
383 * \returns Steer motor closed-loop output type
384 */
386 {
387 return _steerClosedLoopOutput;
388 }
389
390private:
391 /**
392 * \brief Collection of all possible torque feedforward outputs that
393 * can be applied to the motor for a given wheel force feedforward.
394 */
395 struct MotorTorqueFeedforwards {
396 units::newton_meter_t torque;
397 units::ampere_t torqueCurrent;
398 units::volt_t voltage;
399 };
400
401 units::turns_per_second_t ApplyVelocityCorrections(units::turns_per_second_t velocity, units::turn_t targetAngle) const;
402 MotorTorqueFeedforwards CalculateMotorTorqueFeedforwards(
403 units::newton_t wheelForceFeedforwardX,
404 units::newton_t wheelForceFeedforwardY
405 ) const;
406
408
409 std::array<BaseStatusSignal *, 4> GetSignals()
410 {
411 return std::array<BaseStatusSignal *, 4>{&_drivePosition, &_driveVelocity, &_steerPosition, &_steerVelocity};
412 }
413};
414
415}
416}
417}
418}
Class for getting information about an available CAN bus.
Definition CANBus.hpp:19
Represents a status signal with data of type T, and operations available to retrieve information abou...
Definition StatusSignal.hpp:656
T GetValue() const
Gets the cached value from this status signal.
Definition StatusSignal.hpp:758
Contains everything common between Talon motor controllers that support FOC (requires Phoenix Pro).
Definition CommonTalonWithFOC.hpp:27
virtual ctre::phoenix::StatusCode SetControl(const controls::TorqueCurrentFOC &request)=0
Request a specified motor current (field oriented control).
The state of the motor controller bridge when output is neutral or disabled.
Definition SpnEnums.hpp:2303
Swerve Drive class utilizing CTR Electronics Phoenix 6 API.
Definition SwerveDrivetrainImpl.hpp:30
Swerve Module class that encapsulates a swerve module powered by CTR Electronics devices.
Definition SwerveModuleImpl.hpp:60
SwerveModuleState GetCurrentState() const
Get the current state of the module.
Definition SwerveModuleImpl.hpp:347
SwerveModuleImpl(SwerveModuleConstants< DriveMotorConfigsT, SteerMotorConfigsT, EncoderConfigsT > const &constants, CANBus canbus)
Construct a SwerveModuleImpl with the specified constants.
ClosedLoopOutputType GetSteerClosedLoopOutputType() const
Gets the closed-loop output type to use for the steer motor.
Definition SwerveModuleImpl.hpp:385
SwerveModuleState GetTargetState() const
Get the target state of the module.
Definition SwerveModuleImpl.hpp:359
void Apply(ModuleRequest const &moduleRequest)
Applies the desired ModuleRequest to this module.
void Apply(DriveReq &&driveRequest, SteerReq &&steerRequest)
Controls this module using the specified drive and steer control requests.
Definition SwerveModuleImpl.hpp:295
ClosedLoopOutputType GetDriveClosedLoopOutputType() const
Gets the closed-loop output type to use for the drive motor.
Definition SwerveModuleImpl.hpp:375
SwerveModulePosition GetPosition(bool refresh)
Gets the state of this module and passes it back as a SwerveModulePosition object with latency compen...
ctre::phoenix::StatusCode ConfigNeutralMode(signals::NeutralModeValue neutralMode, units::second_t timeoutSeconds=0.100_s)
Configures the neutral mode to use for the module's drive motor.
void ResetPosition()
Resets this module's drive motor position to 0 rotations.
Definition SwerveModuleImpl.hpp:364
SwerveModulePosition GetCachedPosition() const
Gets the last cached swerve module position.
Definition SwerveModuleImpl.hpp:337
Status codes reported by APIs, including OK, warnings, and errors.
Definition StatusCodes.h:27
SteerRequestType
All possible control requests for the module steer motor.
Definition SwerveModuleImpl.hpp:24
@ MotionMagicExpo
Control the drive motor using a Motion Magic® Expo request.
@ Position
Control the drive motor using an unprofiled position request.
DriveRequestType
All possible control requests for the module drive motor.
Definition SwerveModuleImpl.hpp:40
@ Velocity
Control the drive motor using a velocity closed-loop request.
@ OpenLoopVoltage
Control the drive motor using an open-loop voltage request.
ClosedLoopOutputType
Supported closed-loop output types.
Definition SwerveModuleConstants.hpp:25
Definition MotionMagicExpoTorqueCurrentFOC.hpp:18
All constants for a swerve module.
Definition SwerveModuleConstants.hpp:154
Contains everything the swerve module needs to apply a request.
Definition SwerveModuleImpl.hpp:65
ModuleRequest & WithSteerRequest(SteerRequestType newSteerRequest)
Modifies the SteerRequest parameter and returns itself.
Definition SwerveModuleImpl.hpp:189
DriveRequestType DriveRequest
The type of control request to use for the drive motor.
Definition SwerveModuleImpl.hpp:91
units::newton_t WheelForceFeedforwardY
Robot-centric wheel force feedforward to apply in the Y direction.
Definition SwerveModuleImpl.hpp:86
units::second_t UpdatePeriod
The update period of the module request.
Definition SwerveModuleImpl.hpp:101
ModuleRequest & WithEnableFOC(bool newEnableFOC)
Modifies the EnableFOC parameter and returns itself.
Definition SwerveModuleImpl.hpp:229
ModuleRequest & WithWheelForceFeedforwardY(units::newton_t newWheelForceFeedforwardY)
Modifies the WheelForceFeedforwardY parameter and returns itself.
Definition SwerveModuleImpl.hpp:162
SteerRequestType SteerRequest
The type of control request to use for the steer motor.
Definition SwerveModuleImpl.hpp:95
ModuleRequest & WithDriveRequest(DriveRequestType newDriveRequest)
Modifies the DriveRequest parameter and returns itself.
Definition SwerveModuleImpl.hpp:176
bool EnableFOC
When using Voltage-based control, set to true (default) to use FOC commutation (requires Phoenix Pro)...
Definition SwerveModuleImpl.hpp:117
ModuleRequest & WithState(SwerveModuleState newState)
Modifies the State parameter and returns itself.
Definition SwerveModuleImpl.hpp:127
ModuleRequest & WithWheelForceFeedforwardX(units::newton_t newWheelForceFeedforwardX)
Modifies the WheelForceFeedforwardX parameter and returns itself.
Definition SwerveModuleImpl.hpp:145
units::newton_t WheelForceFeedforwardX
Robot-centric wheel force feedforward to apply in the X direction.
Definition SwerveModuleImpl.hpp:78
SwerveModuleState State
Unoptimized speed and direction the module should target.
Definition SwerveModuleImpl.hpp:69
ModuleRequest & WithUpdatePeriod(units::second_t newUpdatePeriod)
Modifies the UpdatePeriod parameter and returns itself.
Definition SwerveModuleImpl.hpp:204