Package frc.robot.subsystems.swervedrive2.swervelib

Class SwerveDrive

java.lang.Object

frc.robot.subsystems.swervedrive2.swervelib.SwerveDrive

public class SwerveDrive
extends Object

Swerve Drive class representing and controlling the swerve drive.

Field Summary

Fields

Modifier and Type	Field	Description
private double	angle	The current angle of the robot and last time odometry during simulations.
edu.wpi.first.wpilibj.smartdashboard.Field2d	field	Field object.
private SwerveIMU	imu	Swerve IMU device for sensing the heading of the robot.
SwerveKinematics2	kinematics	Swerve Kinematics object utilizing second order kinematics.
private double	lastTime	The current angle of the robot and last time odometry during simulations.
SwerveController	swerveController	Swerve controller for controlling heading of the robot.
SwerveDriveConfiguration	swerveDriveConfiguration	Swerve drive configuration.
edu.wpi.first.math.estimator.SwerveDrivePoseEstimator	swerveDrivePoseEstimator	Swerve odometry.
private SwerveModule[]	swerveModules	Swerve modules.
private edu.wpi.first.wpilibj.Timer	timer	Time during simulations.

Constructor Summary

Constructors

Constructor	Description
SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig)	Creates a new swerve drivebase subsystem.

Method Summary

Modifier and Type	Method	Description
void	drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)	The primary method for controlling the drivebase.
edu.wpi.first.math.kinematics.ChassisSpeeds	getFieldVelocity()	Gets the current field-relative velocity (x, y and omega) of the robot
edu.wpi.first.math.kinematics.SwerveModulePosition[]	getModulePositions()	Gets the current module positions (azimuth and wheel position (meters))
edu.wpi.first.math.geometry.Rotation2d	getPitch()	Gets the current yaw 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	getRobotVelocity()	Gets the current robot-relative velocity (x, y and omega) of the robot
SwerveModuleState2[]	getStates()	Gets the current module states (azimuth and velocity)
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	replaceSwerveModuleFeedforward(edu.wpi.first.math.controller.SimpleMotorFeedforward feedforward)	Setup the swerve module feedforward.
void	resetOdometry(edu.wpi.first.math.geometry.Pose2d pose)	Resets odometry to the given pose.
void	setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)	Set field-relative chassis speeds with closed-loop velocity control.
void	setDriveBrake()	Point all modules toward the robot center, thus making the robot very difficult to move.
void	setModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)	Set the module states (azimuth and velocity) directly.
void	setMotorBrake(boolean brake)	Sets the drive motors to brake/coast mode.
void	updateOdometry()	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.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

kinematics

public final SwerveKinematics2 kinematics

Swerve Kinematics object utilizing second order kinematics.

swerveDriveConfiguration

public final SwerveDriveConfiguration swerveDriveConfiguration

Swerve drive configuration.

swerveDrivePoseEstimator

public final edu.wpi.first.math.estimator.SwerveDrivePoseEstimator swerveDrivePoseEstimator

Swerve odometry.

swerveModules

private final SwerveModule[] swerveModules

Swerve modules.

field

public edu.wpi.first.wpilibj.smartdashboard.Field2d field

Field object.

swerveController

public SwerveController swerveController

Swerve controller for controlling heading of the robot.

imu

private SwerveIMU imu

Swerve IMU device for sensing the heading of the robot.

angle

private double angle

The current angle of the robot and last time odometry during simulations.

lastTime

private double lastTime

The current angle of the robot and last time odometry during simulations.

timer

private edu.wpi.first.wpilibj.Timer timer

Time during simulations.

Constructor Detail

SwerveDrive

public SwerveDrive(SwerveDriveConfiguration config,
                   SwerveControllerConfiguration controllerConfig)

Creates a new swerve drivebase subsystem. Robot is controlled via the drive() method, or via the setModuleStates() method. The drive() method incorporates kinematics— it takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control. setModuleStates() takes a list of SwerveModuleStates and directly passes them to the modules. This subsystem also handles odometry.

Method Detail

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 a Translation2d 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.

setModuleStates

public void setModuleStates(SwerveModuleState2[] desiredStates,
                            boolean isOpenLoop)

Set the module states (azimuth and velocity) directly. Used primarily for auto pathing.

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 field-relative chassis speeds with closed-loop velocity control.

Parameters:

chassisSpeeds - Field-relative.

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

getStates

public SwerveModuleState2[] 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

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 yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.

Returns:

The heading as a Rotation2d angle

setMotorBrake

public void setMotorBrake(boolean brake)

Sets the drive motors to brake/coast mode.

Parameters:

brake - True to set motors to brake mode, false for coast.

setDriveBrake

public void setDriveBrake()

Point all modules toward the robot center, thus making the robot very difficult to move.

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 feedforward)

Setup the swerve module feedforward.

Parameters:

feedforward - Feedforward for the drive motor on swerve modules.

updateOdometry

public void updateOdometry()

Update odometry should be run every loop.