CTRE Phoenix 6 C++ 24.50.0-alpha-2
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
14namespace ctre {
15namespace phoenix6 {
16namespace swerve {
17
18/**
19 * \brief Simplified swerve drive simulation class.
20 *
21 * This class assumes that the swerve drive is perfect, meaning
22 * that there is no scrub and the wheels do not slip.
23 *
24 * In addition, it assumes the inertia of the robot is governed only
25 * by the inertia of the steer module and the individual drive wheels.
26 * Robot-wide inertia is not accounted for, and neither is translational
27 * vs rotational inertia of the robot.
28 *
29 * These assumptions provide a simplified example that can demonstrate the
30 * behavior of a swerve drive in simulation. Users are encouraged to
31 * expand this model for their own use.
32 */
34protected:
36 public:
37 /** \brief Reference to motor simulation for drive motor */
38 frc::sim::DCMotorSim DriveMotor;
39 /** \brief Reference to motor simulation for the steer motor */
40 frc::sim::DCMotorSim SteerMotor;
41 /** \brief Reference to steer gearing for updating CANcoder */
42 units::scalar_t DriveGearing;
43 /** \brief Reference to steer gearing for updating CANcoder */
44 units::scalar_t SteerGearing;
45 /** \brief Voltage necessary for the drive motor to overcome friction */
46 units::volt_t DriveFrictionVoltage;
47 /** \brief Voltage necessary for the steer motor to overcome friction */
48 units::volt_t SteerFrictionVoltage;
49 /** \brief Whether the drive motor is inverted */
51 /** \brief Whether the steer motor is inverted */
53
54 SimSwerveModule(units::scalar_t driveGearing, units::kilogram_square_meter_t driveInertia,
55 units::volt_t driveFrictionVoltage, bool driveMotorInverted,
56 units::scalar_t steerGearing, units::kilogram_square_meter_t steerInertia,
57 units::volt_t steerFrictionVoltage, bool steerMotorInverted) :
58 DriveMotor{frc::DCMotor::KrakenX60FOC(1), driveGearing, driveInertia},
59 SteerMotor{frc::DCMotor::KrakenX60FOC(1), steerGearing, steerInertia},
60 DriveGearing{driveGearing},
61 SteerGearing{steerGearing},
62 DriveFrictionVoltage{driveFrictionVoltage},
63 SteerFrictionVoltage{steerFrictionVoltage},
64 DriveMotorInverted{driveMotorInverted},
65 SteerMotorInverted{steerMotorInverted}
66 {}
67 };
68
70 std::vector<SimSwerveModule> _modules;
71
73 Rotation2d _lastAngle{};
74
75public:
76 template < typename... ModuleConstants,
77 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
78 SimSwerveDrivetrain(std::vector<Translation2d> wheelLocations,
79 sim::Pigeon2SimState &pigeonSim,
80 ModuleConstants const &... moduleConstants) :
81 _pigeonSim{pigeonSim},
84 moduleConstants.DriveMotorGearRatio,
85 moduleConstants.DriveInertia,
86 moduleConstants.DriveFrictionVoltage,
87 moduleConstants.DriveMotorInverted,
88 moduleConstants.SteerMotorGearRatio,
89 moduleConstants.SteerInertia,
90 moduleConstants.SteerFrictionVoltage,
91 moduleConstants.SteerMotorInverted
92 }...
93 },
94 _kinem{std::move(wheelLocations)}
95 {}
96
97 void Update(units::second_t dt, units::volt_t supplyVoltage, std::vector<std::unique_ptr<SwerveModule>> const &modulesToApply)
98 {
99 if (modulesToApply.size() != _modules.size()) return;
100
101 std::vector<impl::SwerveModuleState> states(modulesToApply.size());
102 /* update our sim devices */
103 for (size_t i = 0; i < modulesToApply.size(); ++i) {
104 sim::TalonFXSimState &driveMotor = modulesToApply[i]->GetDriveMotor().GetSimState();
105 sim::TalonFXSimState &steerMotor = modulesToApply[i]->GetSteerMotor().GetSimState();
106 sim::CANcoderSimState &cancoder = modulesToApply[i]->GetCANcoder().GetSimState();
107
110
111 driveMotor.SetSupplyVoltage(supplyVoltage);
112 steerMotor.SetSupplyVoltage(supplyVoltage);
113 cancoder.SetSupplyVoltage(supplyVoltage);
114
115 _modules[i].DriveMotor.SetInputVoltage(AddFriction(driveMotor.GetMotorVoltage(), _modules[i].DriveFrictionVoltage));
116 _modules[i].SteerMotor.SetInputVoltage(AddFriction(steerMotor.GetMotorVoltage(), _modules[i].SteerFrictionVoltage));
117
118 _modules[i].DriveMotor.Update(dt);
119 _modules[i].SteerMotor.Update(dt);
120
121 driveMotor.SetRawRotorPosition(_modules[i].DriveMotor.GetAngularPosition() * _modules[i].DriveGearing);
122 driveMotor.SetRotorVelocity(_modules[i].DriveMotor.GetAngularVelocity() * _modules[i].DriveGearing);
123
124 steerMotor.SetRawRotorPosition(_modules[i].SteerMotor.GetAngularPosition() * _modules[i].SteerGearing);
125 steerMotor.SetRotorVelocity(_modules[i].SteerMotor.GetAngularVelocity() * _modules[i].SteerGearing);
126
127 /* CANcoders see the mechanism, so don't account for the steer gearing */
128 cancoder.SetRawPosition(_modules[i].SteerMotor.GetAngularPosition());
129 cancoder.SetVelocity(_modules[i].SteerMotor.GetAngularVelocity());
130
131 states[i] = modulesToApply[i]->GetCurrentState();
132 }
133
134 auto const angularVel = _kinem.ToChassisSpeeds(states).omega;
135 _lastAngle = _lastAngle + Rotation2d{angularVel * dt};
138 }
139
140protected:
141 /**
142 * Applies the effects of friction to dampen the motor voltage.
143 *
144 * \param motorVoltage Voltage output by the motor
145 * \param frictionVoltage Voltage required to overcome friction
146 * \returns Friction-dampened motor voltage
147 */
148 static units::volt_t AddFriction(units::volt_t motorVoltage, units::volt_t frictionVoltage)
149 {
150 if (units::math::abs(motorVoltage) < frictionVoltage) {
151 motorVoltage = 0_V;
152 } else if (motorVoltage > 0_V) {
153 motorVoltage -= frictionVoltage;
154 } else {
155 motorVoltage += frictionVoltage;
156 }
157 return motorVoltage;
158 }
159};
160
161}
162}
163}
Class to control the state of a simulated hardware::CANcoder.
Definition: CANcoderSimState.hpp:32
ctre::phoenix::StatusCode SetSupplyVoltage(units::voltage::volt_t volts)
Sets the simulated supply voltage of the CANcoder.
ctre::phoenix::StatusCode SetVelocity(units::angular_velocity::turns_per_second_t rps)
Sets the simulated velocity of the CANcoder.
ctre::phoenix::StatusCode SetRawPosition(units::angle::turn_t rotations)
Sets the simulated raw position of the CANcoder.
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.
Class to control the state of a simulated hardware::TalonFX.
Definition: TalonFXSimState.hpp:33
ctre::phoenix::StatusCode SetRawRotorPosition(units::angle::turn_t rotations)
Sets the simulated raw rotor position of the TalonFX.
units::voltage::volt_t GetMotorVoltage() const
Gets the simulated output voltage of the motor.
ctre::phoenix::StatusCode SetRotorVelocity(units::angular_velocity::turns_per_second_t rps)
Sets the simulated rotor velocity of the TalonFX.
ctre::phoenix::StatusCode SetSupplyVoltage(units::voltage::volt_t volts)
Sets the simulated supply voltage of the TalonFX.
ChassisReference Orientation
The orientation of the TalonFX relative to the robot chassis.
Definition: TalonFXSimState.hpp:45
frc::sim::DCMotorSim DriveMotor
Reference to motor simulation for drive motor.
Definition: SimSwerveDrivetrain.hpp:38
frc::sim::DCMotorSim SteerMotor
Reference to motor simulation for the steer motor.
Definition: SimSwerveDrivetrain.hpp:40
units::volt_t DriveFrictionVoltage
Voltage necessary for the drive motor to overcome friction.
Definition: SimSwerveDrivetrain.hpp:46
bool SteerMotorInverted
Whether the steer motor is inverted.
Definition: SimSwerveDrivetrain.hpp:52
units::volt_t SteerFrictionVoltage
Voltage necessary for the steer motor to overcome friction.
Definition: SimSwerveDrivetrain.hpp:48
units::scalar_t SteerGearing
Reference to steer gearing for updating CANcoder.
Definition: SimSwerveDrivetrain.hpp:44
bool DriveMotorInverted
Whether the drive motor is inverted.
Definition: SimSwerveDrivetrain.hpp:50
units::scalar_t DriveGearing
Reference to steer gearing for updating CANcoder.
Definition: SimSwerveDrivetrain.hpp:42
SimSwerveModule(units::scalar_t driveGearing, units::kilogram_square_meter_t driveInertia, units::volt_t driveFrictionVoltage, bool driveMotorInverted, units::scalar_t steerGearing, units::kilogram_square_meter_t steerInertia, units::volt_t steerFrictionVoltage, bool steerMotorInverted)
Definition: SimSwerveDrivetrain.hpp:54
Simplified swerve drive simulation class.
Definition: SimSwerveDrivetrain.hpp:33
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:148
impl::SwerveDriveKinematics _kinem
Definition: SimSwerveDrivetrain.hpp:72
sim::Pigeon2SimState & _pigeonSim
Definition: SimSwerveDrivetrain.hpp:69
SimSwerveDrivetrain(std::vector< Translation2d > wheelLocations, sim::Pigeon2SimState &pigeonSim, ModuleConstants const &... moduleConstants)
Definition: SimSwerveDrivetrain.hpp:78
std::vector< SimSwerveModule > _modules
Definition: SimSwerveDrivetrain.hpp:70
Rotation2d _lastAngle
Definition: SimSwerveDrivetrain.hpp:73
void Update(units::second_t dt, units::volt_t supplyVoltage, std::vector< std::unique_ptr< SwerveModule > > const &modulesToApply)
Definition: SimSwerveDrivetrain.hpp:97
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.hpp:235
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.
Represents the state of one swerve module.
Definition: StatusCodes.h:18
Definition: span.hpp:401
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition: SwerveDriveKinematics.hpp:108