Subsystem for controlling the swerve drivetrain of the robot. More...

#include <swerve_drive_subsystem.h>

Inheritance diagram for SwerveDriveSubsystem:

Classes
struct	Velocities
	A struct for holding the 3 different input velocities, for organization. More...

Public Types
enum	DriveControlMode { fieldCentricControl , robotCentricControl }
	Enumerator for either field-centric or robot centric drive modes. More...

Public Member Functions
	SwerveDriveSubsystem (std::shared_ptr< NetworkTablesWrapper > networkTable, const argos_lib::RobotInstance instance)

void	Disable ()
	Handle the robot disabling. More...

void	SwerveDrive (const double &fwVelocity, const double &sideVelocity, const double &rotVelocity)
	Main drive function for the robot. More...

void	StopDrive ()
	Stop all motors. More...

void	Home (const units::degree_t &angle)
	Save homes to persistent storage and updates module motors. More...

void	FieldHome (units::degree_t homeAngle=0_deg, bool updateOdometry=true)
	Tell the robot it's in it's correct field-oriented "Front". More...

void	InitializeOdometry (const frc::Pose2d &currentPose)
	Set current robot position. Useful for initializing initial position before autonomous. More...

frc::Rotation2d	GetContinuousOdometryAngle ()

frc::Pose2d	GetContinuousOdometry ()

frc::Pose2d	UpdateOdometry ()
	Reads module states & gyro, updates odometry, and returns latest pose estimate. More...

units::degree_t	GetFieldCentricAngle () const
	Get the field-centric angle of the robot based on gyro and saved reference orientation. More...

frc::Pose2d	GetPoseEstimate ()
	Get the latest pose estimate. More...

void	SetControlMode (SwerveDriveSubsystem::DriveControlMode controlMode)

void	InitializeMotors ()
	Initialize motors from persistent storage. More...

void	UpdateFollowerLinearPIDParams (double kP, double kI, double kD)
	Change PID parameters for linear follower. These adjust velocities based on distance error from path goal. More...

void	UpdateFollowerRotationalPIDParams (double kP, double kI, double kD)
	Change PID parameters for rotational follower. These adjust velocities based on angle error from path goal. More...

void	UpdateFollowerRotationalPIDConstraints (frc::TrapezoidProfile< units::degrees >::Constraints constraints)
	Update constraints to rotate robot along profiled path. More...

void	StartDrivingProfile (SwerveTrapezoidalProfileSegment newProfile)
	Start driving a new profile. This also resets the finished flag. More...

void	CancelDrivingProfile ()
	Cancel the current driving profile without marking it complete. More...

bool	ProfileIsComplete () const
	Check if a driving profile path has been completed. More...

frc::ChassisSpeeds	GetChassisVelocity ()
	Get the robot velocity in chassis frame (x toward intake, y toward left) based on GetCurrentModuleStates() output. More...

Private Member Functions
wpi::array< frc::SwerveModuleState, 4 >	GetRawModuleStates (SwerveDriveSubsystem::Velocities velocities)
	Get the Raw Module States object and switch between robot-centric and field-centric. More...

wpi::array< frc::SwerveModuleState, 4 >	GetCurrentModuleStates ()
	Get the active states of all swerve modules. More...

void	HomeToFS (const units::degree_t &angle)
	Save homes to a file. More...

void	InitializeMotorsFromFS ()
	Initialize motors from saved file. More...

units::degree_t	GetIMUYaw () const

void	ResetIMUYaw ()

Private Attributes
argos_lib::RobotInstance	m_instance

DriveControlMode	m_controlMode
	Active control mode. More...

SwerveModule	m_frontLeft
	Front left swerve module. More...

SwerveModule	m_frontRight
	Front right swerve module. More...

SwerveModule	m_backRight
	Back right swerve module. More...

SwerveModule	m_backLeft
	Back left swerve module. More...

frc::ADIS16448_IMU	m_imu

Pigeon2	m_pigeonIMU

units::degree_t	m_fieldHomeOffset
	Offset from IMU angle to 0 field angle (intake away from driver station) More...

frc::SwerveDriveKinematics< 4 >	m_swerveDriveKinematics
	Kinematics model for swerve drive system. More...

frc::SwerveDriveOdometry< 4 >	m_odometry
	Odometry to track robot. More...

units::degree_t	m_prevOdometryAngle
	Last odometry angle used for continuous calculations. More...

units::degree_t	m_continuousOdometryOffset
	Offset to convert [-180,180] odometry angle to continuous angle. More...

std::shared_ptr< NetworkTablesWrapper >	m_pNetworkTable
	Instance of network table class. More...

FileSystemHomingStorage	m_fsStorage
	Roborio filesystem access for homes. More...

bool	m_followingProfile
	True when an incomplete drive profile is being run. More...

bool	m_profileComplete
	True once a drive profile has been completed. More...

std::unique_ptr< SwerveTrapezoidalProfileSegment >	m_pActiveSwerveProfile
	Profile to execute. More...

std::chrono::time_point< std::chrono::steady_clock >	m_swerveProfileStartTime
	Time when active profile began. More...

frc2::PIDController	m_linearPID
	Correction parameters for x/y error when following drive profile. More...

frc::ProfiledPIDController< units::radians >	m_rotationalPID
	Correction parameters for rotational error when following drive profile. More...

frc::HolonomicDriveController	m_followerController
	Controller to follow drive profile. More...

argos_lib::NTMotorPIDTuner	m_driveMotorPIDTuner
	Utility to tune drive motors. More...

Detailed Description

Subsystem for controlling the swerve drivetrain of the robot.

Member Enumeration Documentation

◆ DriveControlMode

enum SwerveDriveSubsystem::DriveControlMode

Enumerator for either field-centric or robot centric drive modes.

Enumerator
fieldCentricControl
robotCentricControl

Constructor & Destructor Documentation

◆ SwerveDriveSubsystem()

SwerveDriveSubsystem::SwerveDriveSubsystem	(	std::shared_ptr< NetworkTablesWrapper >	networkTable,
		const argos_lib::RobotInstance	instance
	)

explicit

Member Function Documentation

◆ CancelDrivingProfile()

void SwerveDriveSubsystem::CancelDrivingProfile ( )

Cancel the current driving profile without marking it complete.

◆ Disable()

void SwerveDriveSubsystem::Disable ( )

Handle the robot disabling.

◆ FieldHome()

void SwerveDriveSubsystem::FieldHome	(	units::degree_t	homeAngle = `0_deg`,
		bool	updateOdometry = `true`
	)

Tell the robot it's in it's correct field-oriented "Front".

Parameters

homeAngle	Current orientation of the robot
updateOdometry	Also update odometry field-centric angle

◆ GetChassisVelocity()

frc::ChassisSpeeds SwerveDriveSubsystem::GetChassisVelocity ( )

Get the robot velocity in chassis frame (x toward intake, y toward left) based on GetCurrentModuleStates() output.

Returns: frc::ChassisSpeeds Velocity based on module states

◆ GetContinuousOdometry()

frc::Pose2d SwerveDriveSubsystem::GetContinuousOdometry ( )

◆ GetContinuousOdometryAngle()

frc::Rotation2d SwerveDriveSubsystem::GetContinuousOdometryAngle ( )

◆ GetCurrentModuleStates()

wpi::array< frc::SwerveModuleState, 4 > SwerveDriveSubsystem::GetCurrentModuleStates ( )

private

Get the active states of all swerve modules.

Returns: Active module states

◆ GetFieldCentricAngle()

units::degree_t SwerveDriveSubsystem::GetFieldCentricAngle ( ) const

Get the field-centric angle of the robot based on gyro and saved reference orientation.

Returns: Field-centric angle of the robot where 0 degrees is intake oriented toward opposing alliance operator station positive CCW.

◆ GetIMUYaw()

units::degree_t SwerveDriveSubsystem::GetIMUYaw ( ) const

private

◆ GetPoseEstimate()

frc::Pose2d SwerveDriveSubsystem::GetPoseEstimate ( )

Get the latest pose estimate.

Returns: Latest pose

◆ GetRawModuleStates()

wpi::array< frc::SwerveModuleState, 4 > SwerveDriveSubsystem::GetRawModuleStates ( SwerveDriveSubsystem::Velocities velocities )

private

Get the Raw Module States object and switch between robot-centric and field-centric.

Parameters

velocities Desired velocity

Returns: wpi::array<frc::SwerveModuleState, 4>

◆ Home()

void SwerveDriveSubsystem::Home ( const units::degree_t & angle )

Save homes to persistent storage and updates module motors.

Parameters

angle Current angle of all the swerve modules. They should all be oriented the same during homing.

◆ HomeToFS()

void SwerveDriveSubsystem::HomeToFS ( const units::degree_t & angle )

private

Save homes to a file.

Parameters

angle Current angle of all the swerve modules. They should all be oriented the same during homing.

◆ InitializeMotors()

void SwerveDriveSubsystem::InitializeMotors ( )

Initialize motors from persistent storage.

◆ InitializeMotorsFromFS()

void SwerveDriveSubsystem::InitializeMotorsFromFS ( )

private

Initialize motors from saved file.

◆ InitializeOdometry()

void SwerveDriveSubsystem::InitializeOdometry ( const frc::Pose2d & currentPose )

Set current robot position. Useful for initializing initial position before autonomous.

Parameters

currentPose Field-centric pose of the robot

◆ ProfileIsComplete()

bool SwerveDriveSubsystem::ProfileIsComplete ( ) const

Check if a driving profile path has been completed.

Returns: true when a path is completed and not canceled

◆ ResetIMUYaw()

void SwerveDriveSubsystem::ResetIMUYaw ( )

private

◆ SetControlMode()

void SwerveDriveSubsystem::SetControlMode ( SwerveDriveSubsystem::DriveControlMode controlMode )

◆ StartDrivingProfile()

void SwerveDriveSubsystem::StartDrivingProfile ( SwerveTrapezoidalProfileSegment newProfile )

Start driving a new profile. This also resets the finished flag.

Parameters

newProfile Profile to follow with t=0 being the time this function is called

◆ StopDrive()

void SwerveDriveSubsystem::StopDrive ( )

Stop all motors.

◆ SwerveDrive()

void SwerveDriveSubsystem::SwerveDrive	(	const double &	fwVelocity,
		const double &	sideVelocity,
		const double &	rotVelocity
	)

Main drive function for the robot.

Parameters

fwVelocity	Percent speed forward. Range [-1.0, 1.0] where positive 1.0 is full speed forward
sideVelocity	Percent speed perpendicular to the robots front. Range [-1.0, 1.0] where positive 1.0 is full speed left
rotVelocity	Percent speed of rotation of the chassis. Range [-1.0, 1.0] where positive 1.0 is full speed counterclockwise

◆ UpdateFollowerLinearPIDParams()

void SwerveDriveSubsystem::UpdateFollowerLinearPIDParams	(	double	kP,
		double	kI,
		double	kD
	)

Change PID parameters for linear follower. These adjust velocities based on distance error from path goal.

Parameters

kP	Proportional gain
kI	Integral gain
kD	Derivative gain

◆ UpdateFollowerRotationalPIDConstraints()

void SwerveDriveSubsystem::UpdateFollowerRotationalPIDConstraints ( frc::TrapezoidProfile< units::degrees >::Constraints constraints )

Update constraints to rotate robot along profiled path.

Parameters

constraints Rotational velocity and acceleration constraints

◆ UpdateFollowerRotationalPIDParams()

void SwerveDriveSubsystem::UpdateFollowerRotationalPIDParams	(	double	kP,
		double	kI,
		double	kD
	)

Change PID parameters for rotational follower. These adjust velocities based on angle error from path goal.

Parameters

kP	Proportional gain
kI	Integral gain
kD	Derivative gain

◆ UpdateOdometry()

frc::Pose2d SwerveDriveSubsystem::UpdateOdometry ( )

Reads module states & gyro, updates odometry, and returns latest pose estimate.

Returns: Estimate of robot pose

Member Data Documentation

◆ m_backLeft

SwerveModule SwerveDriveSubsystem::m_backLeft

private

Back left swerve module.

◆ m_backRight

SwerveModule SwerveDriveSubsystem::m_backRight

private

Back right swerve module.

◆ m_continuousOdometryOffset

units::degree_t SwerveDriveSubsystem::m_continuousOdometryOffset

private

Offset to convert [-180,180] odometry angle to continuous angle.

◆ m_controlMode

DriveControlMode SwerveDriveSubsystem::m_controlMode

private

Active control mode.

◆ m_driveMotorPIDTuner

argos_lib::NTMotorPIDTuner SwerveDriveSubsystem::m_driveMotorPIDTuner

private

Utility to tune drive motors.

◆ m_fieldHomeOffset

units::degree_t SwerveDriveSubsystem::m_fieldHomeOffset

private

Offset from IMU angle to 0 field angle (intake away from driver station)

◆ m_followerController

frc::HolonomicDriveController SwerveDriveSubsystem::m_followerController

private

Controller to follow drive profile.

◆ m_followingProfile

bool SwerveDriveSubsystem::m_followingProfile

private

True when an incomplete drive profile is being run.

◆ m_frontLeft

SwerveModule SwerveDriveSubsystem::m_frontLeft

private

Front left swerve module.

◆ m_frontRight

SwerveModule SwerveDriveSubsystem::m_frontRight

private

Front right swerve module.

◆ m_fsStorage

FileSystemHomingStorage SwerveDriveSubsystem::m_fsStorage

private

Roborio filesystem access for homes.

◆ m_imu

frc::ADIS16448_IMU SwerveDriveSubsystem::m_imu

private

◆ m_instance

argos_lib::RobotInstance SwerveDriveSubsystem::m_instance

private

◆ m_linearPID

frc2::PIDController SwerveDriveSubsystem::m_linearPID

private

Correction parameters for x/y error when following drive profile.

◆ m_odometry

frc::SwerveDriveOdometry<4> SwerveDriveSubsystem::m_odometry

private

Odometry to track robot.

◆ m_pActiveSwerveProfile

std::unique_ptr<SwerveTrapezoidalProfileSegment> SwerveDriveSubsystem::m_pActiveSwerveProfile

private

Profile to execute.

◆ m_pigeonIMU

Pigeon2 SwerveDriveSubsystem::m_pigeonIMU

private

◆ m_pNetworkTable

std::shared_ptr<NetworkTablesWrapper> SwerveDriveSubsystem::m_pNetworkTable

private

Instance of network table class.

◆ m_prevOdometryAngle

units::degree_t SwerveDriveSubsystem::m_prevOdometryAngle

private

Last odometry angle used for continuous calculations.

◆ m_profileComplete

bool SwerveDriveSubsystem::m_profileComplete

private

True once a drive profile has been completed.

◆ m_rotationalPID

frc::ProfiledPIDController<units::radians> SwerveDriveSubsystem::m_rotationalPID

private

Correction parameters for rotational error when following drive profile.

◆ m_swerveDriveKinematics

frc::SwerveDriveKinematics<4> SwerveDriveSubsystem::m_swerveDriveKinematics

private

Kinematics model for swerve drive system.

◆ m_swerveProfileStartTime

std::chrono::time_point<std::chrono::steady_clock> SwerveDriveSubsystem::m_swerveProfileStartTime

private

Time when active profile began.

The documentation for this class was generated from the following files:

src/main/include/subsystems/swerve_drive_subsystem.h
src/main/cpp/subsystems/swerve_drive_subsystem.cpp

Classes

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ DriveControlMode

Constructor & Destructor Documentation

◆ SwerveDriveSubsystem()

Member Function Documentation

◆ CancelDrivingProfile()

◆ Disable()

◆ FieldHome()

◆ GetChassisVelocity()

◆ GetContinuousOdometry()

◆ GetContinuousOdometryAngle()

◆ GetCurrentModuleStates()

◆ GetFieldCentricAngle()

◆ GetIMUYaw()

◆ GetPoseEstimate()

◆ GetRawModuleStates()

◆ Home()

◆ HomeToFS()

◆ InitializeMotors()

◆ InitializeMotorsFromFS()

◆ InitializeOdometry()

◆ ProfileIsComplete()

◆ ResetIMUYaw()

◆ SetControlMode()

◆ StartDrivingProfile()

◆ StopDrive()

◆ SwerveDrive()

◆ UpdateFollowerLinearPIDParams()

◆ UpdateFollowerRotationalPIDConstraints()

◆ UpdateFollowerRotationalPIDParams()

◆ UpdateOdometry()

Member Data Documentation

◆ m_backLeft

◆ m_backRight

◆ m_continuousOdometryOffset

◆ m_controlMode

◆ m_driveMotorPIDTuner

◆ m_fieldHomeOffset

◆ m_followerController

◆ m_followingProfile

◆ m_frontLeft

◆ m_frontRight

◆ m_fsStorage

◆ m_imu

◆ m_instance

◆ m_linearPID

◆ m_odometry

◆ m_pActiveSwerveProfile

◆ m_pigeonIMU

◆ m_pNetworkTable

◆ m_prevOdometryAngle

◆ m_profileComplete

◆ m_rotationalPID

◆ m_swerveDriveKinematics

◆ m_swerveProfileStartTime