Package frc.alotobots.library.subsystems.swervedrive

Class SwerveDriveSubsystem

java.lang.Object

edu.wpi.first.wpilibj2.command.SubsystemBase

frc.alotobots.library.subsystems.swervedrive.SwerveDriveSubsystem

All Implemented Interfaces:

Sendable, Subsystem

public class SwerveDriveSubsystem
extends SubsystemBase

The SwerveDriveSubsystem class manages the robot's swerve drive system, handling odometry, module control, and autonomous path following capabilities.

Constructor Summary

Constructors

Constructor	Description
SwerveDriveSubsystem(GyroIO gyroIO, ModuleIO flModuleIO, ModuleIO frModuleIO, ModuleIO blModuleIO, ModuleIO brModuleIO)	Constructs a new SwerveDriveSubsystem.

Method Summary

Modifier and Type	Method	Description
void	addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)	Adds vision measurement for pose estimation.
ChassisSpeeds	getChassisSpeeds()	Gets measured chassis speeds.
double	getFFCharacterizationVelocity()	Gets average module velocity for feedforward characterization.
double	getMaxAngularSpeedRadPerSec()	Gets maximum angular speed capability.
double	getMaxLinearSpeedMetersPerSec()	Gets maximum linear speed capability.
Pose2d	getPose()	Gets current odometry pose.
Rotation2d	getRotation()	Gets current odometry rotation.
double[]	getWheelRadiusCharacterizationPositions()	Gets wheel positions for radius characterization.
void	periodic()	Periodic update function handling odometry updates and module states.
void	runCharacterization(double output)	Runs drive characterization with specified output.
void	runVelocity(ChassisSpeeds speeds)	Runs the drive at the desired velocity.
void	setPose(Pose2d pose)	Resets odometry to specified pose.
void	stop()	Stops all drive modules.
void	stopWithX()	Stops drive and positions modules in X pattern to resist movement.
Command	sysIdDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)	Creates command for dynamic system identification.
Command	sysIdQuasistatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)	Creates command for quasistatic system identification.

Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase

addChild, getName, getSubsystem, initSendable, setName, setSubsystem

Methods inherited from class java.lang.Object

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

Methods inherited from interface edu.wpi.first.wpilibj2.command.Subsystem

defer, getCurrentCommand, getDefaultCommand, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun

Constructor Details

SwerveDriveSubsystem

public SwerveDriveSubsystem(GyroIO gyroIO,
 ModuleIO flModuleIO,
 ModuleIO frModuleIO,
 ModuleIO blModuleIO,
 ModuleIO brModuleIO)

Constructs a new SwerveDriveSubsystem.

Parameters:

gyroIO - Interface for gyro hardware

flModuleIO - Front left module interface

frModuleIO - Front right module interface

blModuleIO - Back left module interface

brModuleIO - Back right module interface

Method Details

periodic

public void periodic()

Periodic update function handling odometry updates and module states.

runVelocity

public void runVelocity(ChassisSpeeds speeds)

Runs the drive at the desired velocity.

Parameters:

speeds - Desired chassis speeds in meters/sec

runCharacterization

public void runCharacterization(double output)

Runs drive characterization with specified output.

Parameters:

output - Characterization output value

stop

public void stop()

Stops all drive modules.

stopWithX

public void stopWithX()

Stops drive and positions modules in X pattern to resist movement.

sysIdQuasistatic

public Command sysIdQuasistatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)

Creates command for quasistatic system identification.

Parameters:

direction - Test direction

Returns:

Command for quasistatic test

sysIdDynamic

public Command sysIdDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)

Creates command for dynamic system identification.

Parameters:

direction - Test direction

Returns:

Command for dynamic test

getChassisSpeeds

public ChassisSpeeds getChassisSpeeds()

Gets measured chassis speeds.

Returns:

Current chassis speeds

getWheelRadiusCharacterizationPositions

public double[] getWheelRadiusCharacterizationPositions()

Gets wheel positions for radius characterization.

Returns:

Array of wheel positions in radians

getFFCharacterizationVelocity

public double getFFCharacterizationVelocity()

Gets average module velocity for feedforward characterization.

Returns:

Average velocity in rotations/sec (Phoenix native units)

getPose

public Pose2d getPose()

Gets current odometry pose.

Returns:

Current robot pose

getRotation

public Rotation2d getRotation()

Gets current odometry rotation.

Returns:

Current robot rotation

setPose

public void setPose(Pose2d pose)

Resets odometry to specified pose.

Parameters:

pose - New robot pose

addVisionMeasurement

public void addVisionMeasurement(Pose2d visionRobotPoseMeters,
 double timestampSeconds,
 Matrix<N3,N1> visionMeasurementStdDevs)

Adds vision measurement for pose estimation.

Parameters:

visionRobotPoseMeters - Vision-measured robot pose

timestampSeconds - Timestamp of measurement

visionMeasurementStdDevs - Standard deviations of vision measurements

getMaxLinearSpeedMetersPerSec

public double getMaxLinearSpeedMetersPerSec()

Gets maximum linear speed capability.

Returns:

Maximum speed in meters per second

getMaxAngularSpeedRadPerSec

public double getMaxAngularSpeedRadPerSec()

Gets maximum angular speed capability.

Returns:

Maximum angular speed in radians per second