50 std::vector<std::unique_ptr<SwerveModule>>
_modules;
66 template <
typename... ModuleConstants,
67 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
85 template <
typename... ModuleConstants,
86 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
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...}
107 template <
typename... ModuleConstants,
108 typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> > >
110 std::array<double, 3>
const &odometryStandardDeviation, std::array<double, 3>
const &visionStandardDeviation,
111 ModuleConstants
const &... modules) :
112 _drivetrain{drivetrainConstants, odometryUpdateFrequency,
113 odometryStandardDeviation, visionStandardDeviation,
115 _modules{CreateModuleArray(drivetrainConstants.CANBusName, modules...)},
116 _pigeon2{drivetrainConstants.Pigeon2Id,
std::string{drivetrainConstants.CANBusName}},
121 if (!retval.IsOK()) {
122 printf(
"Pigeon2 ID %d failed config with error: %s\n",
GetPigeon2().GetDeviceID(), retval.GetName());
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)
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))), ...);
142 template <
typename... ModuleConstants>
143 std::vector<std::unique_ptr<SwerveModule>> CreateModuleArray(std::string_view canbusName, ModuleConstants
const &... constants)
145 return CreateModuleArrayHelper(canbusName, std::make_index_sequence<
sizeof...(ModuleConstants)>{}, constants...);
176 template <
typename Request,
177 typename = std::enable_if_t< std::is_base_of_v<requests::SwerveRequest, std::remove_reference_t<Request>> > >
181 [
this, request=std::forward<Request>(request)](
auto const ¶ms,
auto const &modules)
mutable {
182 return request.Apply(params, modules);
360 frc::Pose2d visionRobotPose,
361 units::second_t timestamp,
362 std::array<double, 3> visionMeasurementStdDevs)
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
virtual ~SwerveDrivetrain()=default
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
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