CTRE Phoenix 6 C++ 25.2.1
Loading...
Searching...
No Matches
SimSwerveDrivetrain.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 <frc/simulation/DCMotorSim.h>
13#include <frc/system/plant/LinearSystemId.h>
14
15namespace ctre {
16namespace phoenix6 {
17namespace swerve {
18
19/**
20 * \brief Simplified swerve drive simulation class.
21 *
22 * This class assumes that the swerve drive is perfect, meaning
23 * that there is no scrub and the wheels do not slip.
24 *
25 * In addition, it assumes the inertia of the robot is governed only
26 * by the inertia of the steer module and the individual drive wheels.
27 * Robot-wide inertia is not accounted for, and neither is translational
28 * vs rotational inertia of the robot.
29 *
30 * These assumptions provide a simplified example that can demonstrate the
31 * behavior of a swerve drive in simulation. Users are encouraged to
32 * expand this model for their own use.
33 */
34template <
35 typename DriveMotorT,
36 typename SteerMotorT,
37 typename EncoderT,
38 typename = std::enable_if_t<std::is_base_of_v<hardware::traits::CommonTalon, DriveMotorT>>,
39 typename = std::enable_if_t<std::is_base_of_v<hardware::traits::CommonTalon, SteerMotorT>>,
40 typename = std::enable_if_t<std::disjunction_v<
41 std::is_same<hardware::CANcoder, EncoderT>,
42 std::is_same<hardware::CANdi, EncoderT>,
43 std::is_same<hardware::TalonFXS, EncoderT>
44 >>
45>
47protected:
49 public:
50 /** \brief Reference to motor simulation for drive motor */
51 frc::sim::DCMotorSim DriveMotor;
52 /** \brief Reference to motor simulation for the steer motor */
53 frc::sim::DCMotorSim SteerMotor;
54 /** \brief Reference to steer gearing for updating encoder */
55 units::scalar_t DriveGearing;
56 /** \brief Reference to steer gearing for updating encoder */
57 units::scalar_t SteerGearing;
58 /** \brief Voltage necessary for the drive motor to overcome friction */
59 units::volt_t DriveFrictionVoltage;
60 /** \brief Voltage necessary for the steer motor to overcome friction */
61 units::volt_t SteerFrictionVoltage;
62 /** \brief Whether the drive motor is inverted */
64 /** \brief Whether the steer motor is inverted */
66 /** \brief Whether the azimuth encoder is inverted */
68
70 units::scalar_t driveGearing, units::kilogram_square_meter_t driveInertia,
71 units::volt_t driveFrictionVoltage, bool driveMotorInverted,
72 DriveMotorArrangement driveMotorType,
73 units::scalar_t steerGearing, units::kilogram_square_meter_t steerInertia,
74 units::volt_t steerFrictionVoltage, bool steerMotorInverted,
75 SteerMotorArrangement steerMotorType,
76 bool encoderInverted
77 ) :
78 DriveMotor{frc::LinearSystemId::DCMotorSystem(GetDriveMotorGearbox(driveMotorType), driveInertia, driveGearing), GetDriveMotorGearbox(driveMotorType)},
79 SteerMotor{frc::LinearSystemId::DCMotorSystem(GetSteerMotorGearbox(steerMotorType), steerInertia, steerGearing), GetSteerMotorGearbox(steerMotorType)},
80 DriveGearing{driveGearing},
81 SteerGearing{steerGearing},
82 DriveFrictionVoltage{driveFrictionVoltage},
83 SteerFrictionVoltage{steerFrictionVoltage},
84 DriveMotorInverted{driveMotorInverted},
85 SteerMotorInverted{steerMotorInverted},
86 EncoderInverted{encoderInverted}
87 {}
88
89 static frc::DCMotor GetDriveMotorGearbox(DriveMotorArrangement driveMotorType)
90 {
91 switch (driveMotorType) {
93 default:
94 return frc::DCMotor::KrakenX60FOC(1);
96 return frc::DCMotor::NEO(1);
98 return frc::DCMotor::NeoVortex(1);
99 }
100 }
101
102 static frc::DCMotor GetSteerMotorGearbox(SteerMotorArrangement steerMotorType)
103 {
104 switch (steerMotorType) {
106 default:
107 return frc::DCMotor::KrakenX60FOC(1);
109 return frc::DCMotor{12_V, 3.1_Nm, 202_A, 4_A, 774_rad_per_s, 1};
111 return frc::DCMotor::NEO(1);
113 return frc::DCMotor::NeoVortex(1);
115 return frc::DCMotor::NEO550(1);
119 return frc::DCMotor::CIM(1);
120 }
121 }
122 };
123
125 std::vector<SimSwerveModule> _modules;
126
128 Rotation2d _lastAngle{};
129
130public:
131 template <
132 typename... ModuleConstants,
133 typename = std::enable_if_t<std::conjunction_v<
134 std::is_same<
135 ModuleConstants,
137 >...
138 >>
139 >
141 std::vector<Translation2d> wheelLocations,
142 sim::Pigeon2SimState &pigeonSim,
143 ModuleConstants const &... moduleConstants
144 ) :
145 _pigeonSim{pigeonSim},
146 _modules{
148 moduleConstants.DriveMotorGearRatio,
149 moduleConstants.DriveInertia,
150 moduleConstants.DriveFrictionVoltage,
151 moduleConstants.DriveMotorInverted,
152 moduleConstants.DriveMotorType,
153 moduleConstants.SteerMotorGearRatio,
154 moduleConstants.SteerInertia,
155 moduleConstants.SteerFrictionVoltage,
156 moduleConstants.SteerMotorInverted,
157 moduleConstants.SteerMotorType,
158 moduleConstants.EncoderInverted
159 }...
160 },
161 _kinem{std::move(wheelLocations)}
162 {}
163
164 void Update(
165 units::second_t dt,
166 units::volt_t supplyVoltage,
167 std::vector<std::unique_ptr<SwerveModule<DriveMotorT, SteerMotorT, EncoderT>>> const &modulesToApply
168 ) {
169 if (modulesToApply.size() != _modules.size()) return;
170
171 std::vector<SwerveModuleState> states(modulesToApply.size());
172 /* update our sim devices */
173 for (size_t i = 0; i < modulesToApply.size(); ++i) {
174 auto &driveMotor = modulesToApply[i]->GetDriveMotor().GetSimState();
175 auto &steerMotor = modulesToApply[i]->GetSteerMotor().GetSimState();
176 auto &encoder = modulesToApply[i]->GetEncoder().GetSimState();
177
178 if constexpr (std::is_same_v<hardware::TalonFXS, DriveMotorT>) {
180 } else {
182 }
183
184 if constexpr (std::is_same_v<hardware::TalonFXS, SteerMotorT>) {
186 steerMotor.ExtSensorOrientation = _modules[i].SteerMotorInverted != _modules[i].EncoderInverted ? sim::ChassisReference::Clockwise_Positive : sim::ChassisReference::CounterClockwise_Positive;
187 } else {
189 }
190
191 if constexpr (std::is_same_v<hardware::CANcoder, EncoderT>) {
193 } else if constexpr (std::is_same_v<hardware::CANdi, EncoderT>) {
194 // encoder.Orientation = _modules[i].EncoderInverted ? sim::ChassisReference::Clockwise_Positive : sim::ChassisReference::CounterClockwise_Positive;
195 }
196
197 driveMotor.SetSupplyVoltage(supplyVoltage);
198 steerMotor.SetSupplyVoltage(supplyVoltage);
199 encoder.SetSupplyVoltage(supplyVoltage);
200
201 _modules[i].DriveMotor.SetInputVoltage(AddFriction(driveMotor.GetMotorVoltage(), _modules[i].DriveFrictionVoltage));
202 _modules[i].SteerMotor.SetInputVoltage(AddFriction(steerMotor.GetMotorVoltage(), _modules[i].SteerFrictionVoltage));
203
204 _modules[i].DriveMotor.Update(dt);
205 _modules[i].SteerMotor.Update(dt);
206
207 driveMotor.SetRawRotorPosition(_modules[i].DriveMotor.GetAngularPosition() * _modules[i].DriveGearing);
208 driveMotor.SetRotorVelocity(_modules[i].DriveMotor.GetAngularVelocity() * _modules[i].DriveGearing);
209
210 steerMotor.SetRawRotorPosition(_modules[i].SteerMotor.GetAngularPosition() * _modules[i].SteerGearing);
211 steerMotor.SetRotorVelocity(_modules[i].SteerMotor.GetAngularVelocity() * _modules[i].SteerGearing);
212 if constexpr (std::is_same_v<hardware::TalonFXS, SteerMotorT>) {
213 /* azimuth encoders see the mechanism, so don't account for the steer gearing */
214 steerMotor.SetPulseWidthPosition(_modules[i].SteerMotor.GetAngularPosition());
215 steerMotor.SetPulseWidthVelocity(_modules[i].SteerMotor.GetAngularVelocity());
216 }
217
218 if constexpr (std::is_same_v<hardware::CANcoder, EncoderT>) {
219 /* azimuth encoders see the mechanism, so don't account for the steer gearing */
220 encoder.SetRawPosition(_modules[i].SteerMotor.GetAngularPosition());
221 encoder.SetVelocity(_modules[i].SteerMotor.GetAngularVelocity());
222 } else if constexpr (std::is_same_v<hardware::CANdi, EncoderT>) {
223 /* azimuth encoders see the mechanism, so don't account for the steer gearing */
224 // encoder.SetRawPosition(_modules[i].SteerMotor.GetAngularPosition());
225 // encoder.SetVelocity(_modules[i].SteerMotor.GetAngularVelocity());
226 }
227
228 states[i] = modulesToApply[i]->GetCurrentState();
229 }
230
231 auto const angularVel = _kinem.ToChassisSpeeds(states).omega;
232 _lastAngle = _lastAngle + Rotation2d{angularVel * dt};
235 }
236
237protected:
238 /**
239 * \brief Applies the effects of friction to dampen the motor voltage.
240 *
241 * \param motorVoltage Voltage output by the motor
242 * \param frictionVoltage Voltage required to overcome friction
243 * \returns Friction-dampened motor voltage
244 */
245 static units::volt_t AddFriction(units::volt_t motorVoltage, units::volt_t frictionVoltage)
246 {
247 if (units::math::abs(motorVoltage) < frictionVoltage) {
248 motorVoltage = 0_V;
249 } else if (motorVoltage > 0_V) {
250 motorVoltage -= frictionVoltage;
251 } else {
252 motorVoltage += frictionVoltage;
253 }
254 return motorVoltage;
255 }
256};
257
258}
259}
260}
Class to control the state of a simulated hardware::Pigeon2.
Definition Pigeon2SimState.hpp:31
ctre::phoenix::StatusCode SetAngularVelocityZ(units::angular_velocity::degrees_per_second_t dps)
Sets the simulated angular velocity Z component of the Pigeon2.
ctre::phoenix::StatusCode SetRawYaw(units::angle::degree_t deg)
Sets the simulated raw yaw of the Pigeon2.
static frc::DCMotor GetDriveMotorGearbox(DriveMotorArrangement driveMotorType)
Definition SimSwerveDrivetrain.hpp:89
static frc::DCMotor GetSteerMotorGearbox(SteerMotorArrangement steerMotorType)
Definition SimSwerveDrivetrain.hpp:102
SimSwerveModule(units::scalar_t driveGearing, units::kilogram_square_meter_t driveInertia, units::volt_t driveFrictionVoltage, bool driveMotorInverted, DriveMotorArrangement driveMotorType, units::scalar_t steerGearing, units::kilogram_square_meter_t steerInertia, units::volt_t steerFrictionVoltage, bool steerMotorInverted, SteerMotorArrangement steerMotorType, bool encoderInverted)
Definition SimSwerveDrivetrain.hpp:69
units::scalar_t SteerGearing
Reference to steer gearing for updating encoder.
Definition SimSwerveDrivetrain.hpp:57
bool SteerMotorInverted
Whether the steer motor is inverted.
Definition SimSwerveDrivetrain.hpp:65
units::scalar_t DriveGearing
Reference to steer gearing for updating encoder.
Definition SimSwerveDrivetrain.hpp:55
units::volt_t SteerFrictionVoltage
Voltage necessary for the steer motor to overcome friction.
Definition SimSwerveDrivetrain.hpp:61
bool DriveMotorInverted
Whether the drive motor is inverted.
Definition SimSwerveDrivetrain.hpp:63
bool EncoderInverted
Whether the azimuth encoder is inverted.
Definition SimSwerveDrivetrain.hpp:67
frc::sim::DCMotorSim SteerMotor
Reference to motor simulation for the steer motor.
Definition SimSwerveDrivetrain.hpp:53
units::volt_t DriveFrictionVoltage
Voltage necessary for the drive motor to overcome friction.
Definition SimSwerveDrivetrain.hpp:59
frc::sim::DCMotorSim DriveMotor
Reference to motor simulation for drive motor.
Definition SimSwerveDrivetrain.hpp:51
Simplified swerve drive simulation class.
Definition SimSwerveDrivetrain.hpp:46
static units::volt_t AddFriction(units::volt_t motorVoltage, units::volt_t frictionVoltage)
Applies the effects of friction to dampen the motor voltage.
Definition SimSwerveDrivetrain.hpp:245
void Update(units::second_t dt, units::volt_t supplyVoltage, std::vector< std::unique_ptr< SwerveModule< DriveMotorT, SteerMotorT, EncoderT > > > const &modulesToApply)
Definition SimSwerveDrivetrain.hpp:164
sim::Pigeon2SimState & _pigeonSim
Definition SimSwerveDrivetrain.hpp:124
Rotation2d _lastAngle
Definition SimSwerveDrivetrain.hpp:128
std::vector< SimSwerveModule > _modules
Definition SimSwerveDrivetrain.hpp:125
SimSwerveDrivetrain(std::vector< Translation2d > wheelLocations, sim::Pigeon2SimState &pigeonSim, ModuleConstants const &... moduleConstants)
Definition SimSwerveDrivetrain.hpp:140
impl::SwerveDriveKinematics _kinem
Definition SimSwerveDrivetrain.hpp:127
Swerve Module class that encapsulates a swerve module powered by CTR Electronics devices.
Definition SwerveModule.hpp:45
Class that converts a chassis velocity (dx, dy, and dtheta components) into individual module states ...
Definition SwerveDriveKinematics.hpp:42
ChassisSpeeds ToChassisSpeeds(WheelSpeeds const &moduleStates) const
Performs forward kinematics to return the resulting chassis state from the given module states.
@ Clockwise_Positive
The device should read a clockwise rotation as positive motion.
@ CounterClockwise_Positive
The device should read a counter-clockwise rotation as positive motion.
SteerMotorArrangement
Supported motor arrangements for the steer motors.
Definition SwerveModuleConstants.hpp:52
@ TalonFXS_Brushed_AC
Brushed motor connected to a Talon FXS on terminals A and C.
@ TalonFXS_Brushed_AB
Brushed motor connected to a Talon FXS on terminals A and B.
@ TalonFX_Integrated
Talon FX integrated brushless motor.
@ TalonFXS_Brushed_BC
Brushed motor connected to a Talon FXS on terminals B and C.
@ TalonFXS_NEO550_JST
Third party NEO550 brushless motor connected to a Talon FXS over JST.
@ TalonFXS_NEO_JST
Third party NEO brushless motor connected to a Talon FXS over JST.
@ TalonFXS_VORTEX_JST
Third party VORTEX brushless motor connected to a Talon FXS over JST.
@ TalonFXS_Minion_JST
CTR Electronics MinionĀ® brushless motor connected to a Talon FXS over JST.
DriveMotorArrangement
Supported motor arrangements for the drive motors.
Definition SwerveModuleConstants.hpp:34
@ TalonFX_Integrated
Talon FX integrated brushless motor.
@ TalonFXS_NEO_JST
Third party NEO brushless motor connected to a Talon FXS over JST.
@ TalonFXS_VORTEX_JST
Third party VORTEX brushless motor connected to a Talon FXS over JST.
Definition MotionMagicExpoTorqueCurrentFOC.hpp:18
Definition span.hpp:401
All constants for a swerve module.
Definition SwerveModuleConstants.hpp:154