21template <
size_t NumModules>
26 units::meters_per_second_t maxSpeed)
27 : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
31 units::meters_per_second_t velocity)
const override {
34 auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
35 {xVelocity, yVelocity, velocity * curvature});
36 m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
38 auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
44 units::meters_per_second_t speed)
const override {
50 units::meters_per_second_t m_maxSpeed;
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition Pose2d.h:128
constexpr double Cos() const
Returns the cosine of the rotation.
Definition Rotation2d.h:231
constexpr double Sin() const
Returns the sine of the rotation.
Definition Rotation2d.h:238
A class that enforces constraints on the swerve drive kinematics.
Definition SwerveDriveKinematicsConstraint.h:22
MinMax MinMaxAcceleration(const Pose2d &pose, units::curvature_t curvature, units::meters_per_second_t speed) const override
Returns the minimum and maximum allowable acceleration for the trajectory given pose,...
Definition SwerveDriveKinematicsConstraint.h:43
SwerveDriveKinematicsConstraint(const frc::SwerveDriveKinematics< NumModules > &kinematics, units::meters_per_second_t maxSpeed)
Definition SwerveDriveKinematicsConstraint.h:24
units::meters_per_second_t MaxVelocity(const Pose2d &pose, units::curvature_t curvature, units::meters_per_second_t velocity) const override
Returns the max velocity given the current pose and curvature.
Definition SwerveDriveKinematicsConstraint.h:29
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition SwerveDriveKinematics.h:54
An interface for defining user-defined velocity and acceleration constraints while generating traject...
Definition TrajectoryConstraint.h:21
constexpr UnitTypeLhs hypot(const UnitTypeLhs &x, const UnitTypeRhs &y)
Computes the square root of the sum-of-squares of x and y.
Definition math.h:508
Represents a minimum and maximum acceleration.
Definition TrajectoryConstraint.h:37