Package swervelib
Class SwerveDrive
java.lang.Object
swervelib.SwerveDrive
Swerve Drive class representing and controlling the swerve drive.
-
Field Summary
Modifier and TypeFieldDescriptionprivate double
The absolute max speed the robot can reach while rotating radians per second.private double
The absolute max speed that your robot can reach while translating in meters per second.boolean
Correct chassis velocity indrive(Translation2d, double, boolean, boolean)
using 254's correction.edu.wpi.first.wpilibj.smartdashboard.Field2d
Field object.private double
Deadband for speeds in heading correction.boolean
Whether to correct heading when driving translationally.private SwerveIMU
Swerve IMU device for sensing the heading of the robot.final Cache<edu.wpi.first.math.geometry.Rotation3d>
IMU reading cache for robot readings.final edu.wpi.first.math.kinematics.SwerveDriveKinematics
Swerve Kinematics object.private double
The last heading set in radians.private double
Maximum speed of the robot in meters per second.private int
Counter to synchronize the modules relative encoder with absolute encoder when not moving.private final Lock
Odometry lock to ensure thread safety.private final edu.wpi.first.wpilibj.Notifier
WPILibNotifier
to keep odometry up to date.private SwerveIMUSimulation
Simulation of the swerve drive.Swerve controller for controlling heading of the robot.final SwerveDriveConfiguration
Swerve drive configuration.final edu.wpi.first.math.estimator.SwerveDrivePoseEstimator
Swerve odometry.private final SwerveModule[]
Swerve modules.private final Alert
Alert to recommend Tuner X if the configuration is compatible. -
Constructor Summary
ConstructorDescriptionSwerveDrive
(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS) Creates a new swerve drivebase subsystem. -
Method Summary
Modifier and TypeMethodDescriptionvoid
addVisionMeasurement
(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp) Add a vision measurement to theSwerveDrivePoseEstimator
and update theSwerveIMU
gyro reading with the given timestamp of the vision measurement.void
addVisionMeasurement
(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3, edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs) Add a vision measurement to theSwerveDrivePoseEstimator
and update theSwerveIMU
gyro reading with the given timestamp of the vision measurement.private void
Check all components to ensure that Tuner X Swerve Generator is recommended instead.void
drive
(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) The primary method for controlling the drivebase.void
drive
(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) The primary method for controlling the drivebase.void
drive
(edu.wpi.first.math.kinematics.ChassisSpeeds velocity) Secondary method for controlling the drivebase.void
drive
(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) The primary method for controlling the drivebase.void
drive
(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) Secondary method for controlling the drivebase.void
driveFieldOriented
(edu.wpi.first.math.kinematics.ChassisSpeeds velocity) Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.void
driveFieldOriented
(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.Optional<edu.wpi.first.math.geometry.Translation3d>
getAccel()
Gets current acceleration of the robot in m/s/s.edu.wpi.first.math.kinematics.ChassisSpeeds
Gets the current field-relative velocity (x, y and omega) of the robotgetGyro()
Getter for theSwerveIMU
.edu.wpi.first.math.geometry.Rotation3d
Gets the current gyroRotation3d
of the robot, as reported by the imu.double
Get the maximum angular velocity, eitherattainableMaxRotationalVelocityRadiansPerSecond
orSwerveControllerConfiguration.maxAngularVelocity
.double
Get the maximum velocity fromattainableMaxTranslationalSpeedMetersPerSecond
ormaxSpeedMPS
whichever is higher.Get theSwerveModule
's as aHashMap
where the key is the swerve module configuration name.edu.wpi.first.math.kinematics.SwerveModulePosition[]
Gets the current module positions (azimuth and wheel position (meters)).Get theSwerveModule
s associated with theSwerveDrive
.edu.wpi.first.math.geometry.Rotation2d
Fetch the latest odometry heading, should be trusted overgetYaw()
.edu.wpi.first.math.geometry.Rotation2d
getPitch()
Gets the current pitch angle of the robot, as reported by the imu.edu.wpi.first.math.geometry.Pose2d
getPose()
Gets the current pose (position and rotation) of the robot, as reported by odometry.edu.wpi.first.math.kinematics.ChassisSpeeds
Gets the current robot-relative velocity (x, y and omega) of the robotedu.wpi.first.math.geometry.Rotation2d
getRoll()
Gets the current roll angle of the robot, as reported by the imu.edu.wpi.first.math.kinematics.SwerveModuleState[]
Gets the current module states (azimuth and velocity)Helper function to get theswerveController
for theSwerveDrive
which can be used to generateChassisSpeeds
for the robot to orient it correctly given axis or angles, and applySlewRateLimiter
to given inputs.edu.wpi.first.math.geometry.Pose2d[]
getSwerveModulePoses
(edu.wpi.first.math.geometry.Pose2d robotPose) Get the swerve module poses and on the field relative to the robot.edu.wpi.first.math.geometry.Rotation2d
getYaw()
Gets the current yaw angle of the robot, as reported by the imu.void
lockPose()
Point all modules toward the robot center, thus making the robot very difficult to move.void
postTrajectory
(edu.wpi.first.math.trajectory.Trajectory trajectory) Post the trajectory to the fieldvoid
Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type.void
replaceSwerveModuleFeedforward
(edu.wpi.first.math.controller.SimpleMotorFeedforward driveFeedforward) Setup the swerve module feedforward.void
Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in autonomous.void
resetOdometry
(edu.wpi.first.math.geometry.Pose2d pose) Resets odometry to the given pose.void
Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0void
setAngleMotorConversionFactor
(double conversionFactor) Set the conversion factor for the angle/azimuth motor controller.void
setChassisSpeeds
(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds) Set chassis speeds with closed-loop velocity control.void
setCosineCompensator
(boolean enabled) Enable or disable theSwerveModuleConfiguration.useCosineCompensator
for allSwerveModule
's in the swerve drive.void
setDriveMotorConversionFactor
(double conversionFactor) Set the conversion factor for the drive motor controller.void
setGyro
(edu.wpi.first.math.geometry.Rotation3d gyro) Set the expected gyroscope angle using aRotation3d
object.void
setGyroOffset
(edu.wpi.first.math.geometry.Rotation3d offset) Set the gyro scope offset to a desired known rotation.void
setHeadingCorrection
(boolean state) Set the heading correction capabilities of YAGSL.void
setHeadingCorrection
(boolean state, double deadband) Set the heading correction capabilities of YAGSL.void
setMaximumSpeed
(double maximumSpeed) Set the maximum speed of the drive motors, modifiedmaxSpeedMPS
which is used for thesetRawModuleStates(SwerveModuleState[], boolean)
function andSwerveController.getTargetSpeeds(double, double, double, double, double)
functions.void
setMaximumSpeed
(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage) Set the maximum speed of the drive motors, modifiedmaxSpeedMPS
which is used for thesetRawModuleStates(SwerveModuleState[], boolean)
function andSwerveController.getTargetSpeeds(double, double, double, double, double)
functions.void
setMaximumSpeeds
(double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) Set the maximum speeds for desaturation.void
setModuleStates
(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop) Set the module states (azimuth and velocity) directly.void
setMotorIdleMode
(boolean brake) Sets the drive motors to brake/coast mode.void
setOdometryPeriod
(double period) Set the odometry update period in seconds.private void
setRawModuleStates
(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop) Set the module states (azimuth and velocity) directly.void
Stop the odometry thread in favor of manually updating odometry.void
Synchronize angle motor integrated encoders with data from absolute encoders.void
updateCacheValidityPeriods
(long imu, long driveMotor, long absoluteEncoder) Update the cache validity period for the robot.void
Update odometry should be run every loop.void
zeroGyro()
Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
-
Field Details
-
kinematics
public final edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematicsSwerve Kinematics object. -
swerveDriveConfiguration
Swerve drive configuration. -
swerveDrivePoseEstimator
public final edu.wpi.first.math.estimator.SwerveDrivePoseEstimator swerveDrivePoseEstimatorSwerve odometry. -
imuReadingCache
IMU reading cache for robot readings. -
swerveModules
Swerve modules. -
odometryThread
private final edu.wpi.first.wpilibj.Notifier odometryThreadWPILibNotifier
to keep odometry up to date. -
odometryLock
Odometry lock to ensure thread safety. -
tunerXRecommendation
Alert to recommend Tuner X if the configuration is compatible. -
field
public edu.wpi.first.wpilibj.smartdashboard.Field2d fieldField object. -
swerveController
Swerve controller for controlling heading of the robot. -
chassisVelocityCorrection
public boolean chassisVelocityCorrectionCorrect chassis velocity indrive(Translation2d, double, boolean, boolean)
using 254's correction. -
headingCorrection
public boolean headingCorrectionWhether to correct heading when driving translationally. Set to true to enable. -
HEADING_CORRECTION_DEADBAND
private double HEADING_CORRECTION_DEADBANDDeadband for speeds in heading correction. -
imu
Swerve IMU device for sensing the heading of the robot. -
simIMU
Simulation of the swerve drive. -
moduleSynchronizationCounter
private int moduleSynchronizationCounterCounter to synchronize the modules relative encoder with absolute encoder when not moving. -
lastHeadingRadians
private double lastHeadingRadiansThe last heading set in radians. -
attainableMaxTranslationalSpeedMetersPerSecond
private double attainableMaxTranslationalSpeedMetersPerSecondThe absolute max speed that your robot can reach while translating in meters per second. -
attainableMaxRotationalVelocityRadiansPerSecond
private double attainableMaxRotationalVelocityRadiansPerSecondThe absolute max speed the robot can reach while rotating radians per second. -
maxSpeedMPS
private double maxSpeedMPSMaximum speed of the robot in meters per second.
-
-
Constructor Details
-
SwerveDrive
public SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS) Creates a new swerve drivebase subsystem. Robot is controlled via thedrive(edu.wpi.first.math.kinematics.ChassisSpeeds)
method, or via thesetRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[], boolean)
method. Thedrive(edu.wpi.first.math.kinematics.ChassisSpeeds)
method incorporates kinematics-- it takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control.setRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[], boolean)
takes a list of SwerveModuleStates and directly passes them to the modules. This subsystem also handles odometry.- Parameters:
config
- TheSwerveDriveConfiguration
configuration to base the swerve drive off of.controllerConfig
- TheSwerveControllerConfiguration
to use when creating theSwerveController
.maxSpeedMPS
- Maximum speed in meters per second, remember to useUnits.feetToMeters(double)
if you have feet per second!
-
-
Method Details
-
updateCacheValidityPeriods
public void updateCacheValidityPeriods(long imu, long driveMotor, long absoluteEncoder) Update the cache validity period for the robot.- Parameters:
imu
- IMU reading cache validity period in milliseconds.driveMotor
- Drive motor reading cache in milliseconds.absoluteEncoder
- Absolute encoder reading cache in milliseconds.
-
checkIfTunerXCompatible
private void checkIfTunerXCompatible()Check all components to ensure that Tuner X Swerve Generator is recommended instead. -
setOdometryPeriod
public void setOdometryPeriod(double period) Set the odometry update period in seconds.- Parameters:
period
- period in seconds.
-
stopOdometryThread
public void stopOdometryThread()Stop the odometry thread in favor of manually updating odometry. -
setAngleMotorConversionFactor
public void setAngleMotorConversionFactor(double conversionFactor) Set the conversion factor for the angle/azimuth motor controller.- Parameters:
conversionFactor
- Angle motor conversion factor for PID, should be generated fromSwerveMath.calculateDegreesPerSteeringRotation(double, double)
or calculated.
-
setDriveMotorConversionFactor
public void setDriveMotorConversionFactor(double conversionFactor) Set the conversion factor for the drive motor controller.- Parameters:
conversionFactor
- Drive motor conversion factor for PID, should be generated fromSwerveMath.calculateMetersPerRotation(double, double, double)
or calculated.
-
getOdometryHeading
public edu.wpi.first.math.geometry.Rotation2d getOdometryHeading()Fetch the latest odometry heading, should be trusted overgetYaw()
.- Returns:
Rotation2d
of the robot heading.
-
setHeadingCorrection
public void setHeadingCorrection(boolean state) Set the heading correction capabilities of YAGSL.- Parameters:
state
-headingCorrection
state.
-
setHeadingCorrection
public void setHeadingCorrection(boolean state, double deadband) Set the heading correction capabilities of YAGSL.- Parameters:
state
-headingCorrection
state.deadband
-HEADING_CORRECTION_DEADBAND
deadband.
-
driveFieldOriented
public void driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity) Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.- Parameters:
velocity
- Velocity of the robot desired.
-
driveFieldOriented
public void driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.- Parameters:
velocity
- Velocity of the robot desired.centerOfRotationMeters
- The center of rotation in meters, 0 is the center of the robot.
-
drive
public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity) Secondary method for controlling the drivebase. Given a simpleChassisSpeeds
set the swerve module states, to achieve the goal.- Parameters:
velocity
- The desired robot-orientedChassisSpeeds
for the robot to achieve.
-
drive
public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) Secondary method for controlling the drivebase. Given a simpleChassisSpeeds
set the swerve module states, to achieve the goal.- Parameters:
velocity
- The desired robot-orientedChassisSpeeds
for the robot to achieve.centerOfRotationMeters
- The center of rotation in meters, 0 is the center of the robot.
-
drive
public void drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) The primary method for controlling the drivebase. Takes aTranslation2d
and a rotation rate, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.- Parameters:
translation
-Translation2d
that is the commanded linear velocity of the robot, in meters per second. In robot-relative mode, positive x is torwards the bow (front) and positive y is torwards port (left). In field-relative mode, positive x is away from the alliance wall (field North) and positive y is torwards the left wall when looking through the driver station glass (field West).rotation
- Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot relativity.fieldRelative
- Drive mode. True for field-relative, false for robot-relative.isOpenLoop
- Whether to use closed-loop velocity control. Set to true to disable closed-loop.centerOfRotationMeters
- The center of rotation in meters, 0 is the center of the robot.
-
drive
public void drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) The primary method for controlling the drivebase. Takes aTranslation2d
and a rotation rate, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.- Parameters:
translation
-Translation2d
that is the commanded linear velocity of the robot, in meters per second. In robot-relative mode, positive x is torwards the bow (front) and positive y is torwards port (left). In field-relative mode, positive x is away from the alliance wall (field North) and positive y is torwards the left wall when looking through the driver station glass (field West).rotation
- Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot relativity.fieldRelative
- Drive mode. True for field-relative, false for robot-relative.isOpenLoop
- Whether to use closed-loop velocity control. Set to true to disable closed-loop.
-
drive
public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters) The primary method for controlling the drivebase. Takes aChassisSpeeds
, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Applies heading correction if enabled and necessary.- Parameters:
velocity
- The chassis speeds to set the robot to achieve.isOpenLoop
- Whether to use closed-loop velocity control. Set to true to disable closed-loop.centerOfRotationMeters
- The center of rotation in meters, 0 is the center of the robot.
-
setMaximumSpeeds
public void setMaximumSpeeds(double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) Set the maximum speeds for desaturation.- Parameters:
attainableMaxModuleSpeedMetersPerSecond
- The absolute max speed that a module can reach in meters per second.attainableMaxTranslationalSpeedMetersPerSecond
- The absolute max speed that your robot can reach while translating in meters per second.attainableMaxRotationalVelocityRadiansPerSecond
- The absolute max speed the robot can reach while rotating in radians per second.
-
getMaximumVelocity
public double getMaximumVelocity()Get the maximum velocity fromattainableMaxTranslationalSpeedMetersPerSecond
ormaxSpeedMPS
whichever is higher.- Returns:
- Maximum speed in meters/second.
-
getMaximumAngularVelocity
public double getMaximumAngularVelocity()Get the maximum angular velocity, eitherattainableMaxRotationalVelocityRadiansPerSecond
orSwerveControllerConfiguration.maxAngularVelocity
.- Returns:
- Maximum angular velocity in radians per second.
-
setRawModuleStates
private void setRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop) Set the module states (azimuth and velocity) directly.- Parameters:
desiredStates
- A list of SwerveModuleStates to send to the modules.isOpenLoop
- Whether to use closed-loop velocity control. Set to true to disable closed-loop.
-
setModuleStates
public void setModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop) Set the module states (azimuth and velocity) directly. Used primarily for auto paths.- Parameters:
desiredStates
- A list of SwerveModuleStates to send to the modules.isOpenLoop
- Whether to use closed-loop velocity control. Set to true to disable closed-loop.
-
setChassisSpeeds
public void setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds) Set chassis speeds with closed-loop velocity control.- Parameters:
chassisSpeeds
- Chassis speeds to set.
-
getPose
public edu.wpi.first.math.geometry.Pose2d getPose()Gets the current pose (position and rotation) of the robot, as reported by odometry.- Returns:
- The robot's pose
-
getFieldVelocity
public edu.wpi.first.math.kinematics.ChassisSpeeds getFieldVelocity()Gets the current field-relative velocity (x, y and omega) of the robot- Returns:
- A ChassisSpeeds object of the current field-relative velocity
-
getRobotVelocity
public edu.wpi.first.math.kinematics.ChassisSpeeds getRobotVelocity()Gets the current robot-relative velocity (x, y and omega) of the robot- Returns:
- A ChassisSpeeds object of the current robot-relative velocity
-
resetOdometry
public void resetOdometry(edu.wpi.first.math.geometry.Pose2d pose) Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this method. However, if either gyro angle or module position is reset, this must be called in order for odometry to keep working.- Parameters:
pose
- The pose to set the odometry to
-
postTrajectory
public void postTrajectory(edu.wpi.first.math.trajectory.Trajectory trajectory) Post the trajectory to the field- Parameters:
trajectory
- the trajectory to post.
-
getStates
public edu.wpi.first.math.kinematics.SwerveModuleState[] getStates()Gets the current module states (azimuth and velocity)- Returns:
- A list of SwerveModuleStates containing the current module states
-
getModulePositions
public edu.wpi.first.math.kinematics.SwerveModulePosition[] getModulePositions()Gets the current module positions (azimuth and wheel position (meters)).- Returns:
- A list of SwerveModulePositions containg the current module positions
-
getGyro
Getter for theSwerveIMU
.- Returns:
- generated
SwerveIMU
-
setGyro
public void setGyro(edu.wpi.first.math.geometry.Rotation3d gyro) Set the expected gyroscope angle using aRotation3d
object. To reset gyro, set to a newRotation3d
subtracted from the current gyroscopic readingsSwerveIMU.getRotation3d()
.- Parameters:
gyro
- expected gyroscope angle asRotation3d
.
-
zeroGyro
public void zeroGyro()Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. -
getYaw
public edu.wpi.first.math.geometry.Rotation2d getYaw()Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.- Returns:
- The yaw as a
Rotation2d
angle
-
getPitch
public edu.wpi.first.math.geometry.Rotation2d getPitch()Gets the current pitch angle of the robot, as reported by the imu.- Returns:
- The heading as a
Rotation2d
angle
-
getRoll
public edu.wpi.first.math.geometry.Rotation2d getRoll()Gets the current roll angle of the robot, as reported by the imu.- Returns:
- The heading as a
Rotation2d
angle
-
getGyroRotation3d
public edu.wpi.first.math.geometry.Rotation3d getGyroRotation3d()Gets the current gyroRotation3d
of the robot, as reported by the imu.- Returns:
- The heading as a
Rotation3d
angle
-
getAccel
Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty.- Returns:
- acceleration of the robot as a
Translation3d
-
setMotorIdleMode
public void setMotorIdleMode(boolean brake) Sets the drive motors to brake/coast mode.- Parameters:
brake
- True to set motors to brake mode, false for coast.
-
setMaximumSpeed
public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage) Set the maximum speed of the drive motors, modifiedmaxSpeedMPS
which is used for thesetRawModuleStates(SwerveModuleState[], boolean)
function andSwerveController.getTargetSpeeds(double, double, double, double, double)
functions. This function overrides what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates.- Parameters:
maximumSpeed
- Maximum speed for the drive motors in meters / second.updateModuleFeedforward
- Update the swerve module feedforward to account for the new maximum speed. This should be true unless you have replaced the drive motor feedforward withreplaceSwerveModuleFeedforward(SimpleMotorFeedforward)
optimalVoltage
- Optimal voltage to use for the feedforward.
-
setMaximumSpeed
public void setMaximumSpeed(double maximumSpeed) Set the maximum speed of the drive motors, modifiedmaxSpeedMPS
which is used for thesetRawModuleStates(SwerveModuleState[], boolean)
function andSwerveController.getTargetSpeeds(double, double, double, double, double)
functions. This function overrides what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites theSwerveModule.setFeedforward(SimpleMotorFeedforward)
.- Parameters:
maximumSpeed
- Maximum speed for the drive motors in meters / second.
-
lockPose
public void lockPose()Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep the current pose. -
getSwerveModulePoses
public edu.wpi.first.math.geometry.Pose2d[] getSwerveModulePoses(edu.wpi.first.math.geometry.Pose2d robotPose) Get the swerve module poses and on the field relative to the robot.- Parameters:
robotPose
- Robot pose.- Returns:
- Swerve module poses.
-
replaceSwerveModuleFeedforward
public void replaceSwerveModuleFeedforward(edu.wpi.first.math.controller.SimpleMotorFeedforward driveFeedforward) Setup the swerve module feedforward.- Parameters:
driveFeedforward
- Feedforward for the drive motor on swerve modules.
-
updateOdometry
public void updateOdometry()Update odometry should be run every loop. Synchronizes module absolute encoders with relative encoders periodically. In simulation mode will also post the pose of each module. Updates SmartDashboard with module encoder readings and states. -
synchronizeModuleEncoders
public void synchronizeModuleEncoders()Synchronize angle motor integrated encoders with data from absolute encoders. -
setGyroOffset
public void setGyroOffset(edu.wpi.first.math.geometry.Rotation3d offset) Set the gyro scope offset to a desired known rotation. UnlikesetGyro(Rotation3d)
it DOES NOT take the current rotation into account.- Parameters:
offset
-Rotation3d
known offset of the robot for gyroscope to use.
-
addVisionMeasurement
public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3, edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs) Add a vision measurement to theSwerveDrivePoseEstimator
and update theSwerveIMU
gyro reading with the given timestamp of the vision measurement.- Parameters:
robotPose
- RobotPose2d
as measured by vision.timestamp
- Timestamp the measurement was taken as time since startup, should be taken fromTimer.getFPGATimestamp()
or similar sources.visionMeasurementStdDevs
- Vision measurement standard deviation that will be sent to theSwerveDrivePoseEstimator
.The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more points and fit a line to it with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get * vision accurate to inches instead of feet.
-
addVisionMeasurement
public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp) Add a vision measurement to theSwerveDrivePoseEstimator
and update theSwerveIMU
gyro reading with the given timestamp of the vision measurement.- Parameters:
robotPose
- RobotPose2d
as measured by vision.timestamp
- Timestamp the measurement was taken as time since startup, should be taken fromTimer.getFPGATimestamp()
or similar sources.
-
getSwerveController
Helper function to get theswerveController
for theSwerveDrive
which can be used to generateChassisSpeeds
for the robot to orient it correctly given axis or angles, and applySlewRateLimiter
to given inputs. Important functions to look at areSwerveController.getTargetSpeeds(double, double, double, double, double)
,SwerveController.addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter)
,SwerveController.getRawTargetSpeeds(double, double, double)
.- Returns:
SwerveController
for theSwerveDrive
.
-
getModules
Get theSwerveModule
s associated with theSwerveDrive
.- Returns:
SwerveModule
array specified by configurations.
-
getModuleMap
Get theSwerveModule
's as aHashMap
where the key is the swerve module configuration name.- Returns:
HashMap
(Module Name, SwerveModule)
-
resetDriveEncoders
public void resetDriveEncoders()Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in autonomous. -
pushOffsetsToControllers
public void pushOffsetsToControllers()Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the internal offsets to prevent double offsetting. -
restoreInternalOffset
public void restoreInternalOffset()Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0 -
setCosineCompensator
public void setCosineCompensator(boolean enabled) Enable or disable theSwerveModuleConfiguration.useCosineCompensator
for allSwerveModule
's in the swerve drive. The cosine compensator will slow down or speed up modules that are close to their desired state in theory.- Parameters:
enabled
- Usage of the cosine compensator.
-