Package swervelib.math
Class SwerveModuleState2
java.lang.Object
edu.wpi.first.math.kinematics.SwerveModuleState
swervelib.math.SwerveModuleState2
- All Implemented Interfaces:
Comparable<edu.wpi.first.math.kinematics.SwerveModuleState>
public class SwerveModuleState2
extends edu.wpi.first.math.kinematics.SwerveModuleState
Second order kinematics swerve module state.
-
Field Summary
FieldsFields inherited from class edu.wpi.first.math.kinematics.SwerveModuleState
angle, speedMetersPerSecond -
Constructor Summary
ConstructorsConstructorDescriptionConstructs a SwerveModuleState with zeros for speed and angle.SwerveModuleState2(double speedMetersPerSecond, edu.wpi.first.math.geometry.Rotation2d angle, double omegaRadPerSecond) Constructs a SwerveModuleState.SwerveModuleState2(edu.wpi.first.math.kinematics.SwerveModuleState state, double omegaRadPerSecond) Create aSwerveModuleState2based on theSwerveModuleStatewith the radians per second defined. -
Method Summary
Modifier and TypeMethodDescriptionstatic SwerveModuleState2optimize(SwerveModuleState2 desiredState, edu.wpi.first.math.geometry.Rotation2d currentAngle, SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop) Minimize the change in heading the desired swerve module state would require by potentially reversing the direction the wheel spins.edu.wpi.first.math.kinematics.SwerveModuleStateConvert to aSwerveModuleState.Methods inherited from class edu.wpi.first.math.kinematics.SwerveModuleState
compareTo, equals, hashCode, optimize, toString
-
Field Details
-
omegaRadPerSecond
public double omegaRadPerSecondRad per sec
-
-
Constructor Details
-
SwerveModuleState2
public SwerveModuleState2()Constructs a SwerveModuleState with zeros for speed and angle. -
SwerveModuleState2
public SwerveModuleState2(double speedMetersPerSecond, edu.wpi.first.math.geometry.Rotation2d angle, double omegaRadPerSecond) Constructs a SwerveModuleState.- Parameters:
speedMetersPerSecond- The speed of the wheel of the module.angle- The angle of the module.omegaRadPerSecond- The angular velocity of the module.
-
SwerveModuleState2
public SwerveModuleState2(edu.wpi.first.math.kinematics.SwerveModuleState state, double omegaRadPerSecond) Create aSwerveModuleState2based on theSwerveModuleStatewith the radians per second defined.- Parameters:
state- First order kinematic module state.omegaRadPerSecond- Module wheel angular rotation in radians per second.
-
-
Method Details
-
optimize
public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, edu.wpi.first.math.geometry.Rotation2d currentAngle, SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop) Minimize the change in heading the desired swerve module state would require by potentially reversing the direction the wheel spins. If this is used with the PIDController class's continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.- Parameters:
desiredState- The desired state.currentAngle- The current module angle.lastState- The last state of the module.moduleSteerFeedForwardClosedLoop- The module feed forward closed loop for the angle motor.- Returns:
- Optimized swerve module state.
-
toSwerveModuleState
public edu.wpi.first.math.kinematics.SwerveModuleState toSwerveModuleState()Convert to aSwerveModuleState.- Returns:
SwerveModuleStatewith the same angle and speed.
-