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 (const argos_lib::RobotInstance instance)

void	Disable ()
	Handle the robot disabling.

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

void	SwerveDrive (const units::degree_t &velAngle, const double &velocity, const double &rotVelocity)
	Same as polar swerve drive function, but also takes in a rotational velocity to apply ONLY USE IN FIELD-CENTRIC.

void	SwerveDrive (const units::degree_t &velAngle, const double &velocity)
	Takes in speeds as a polar vector, and calculates the forward and side velocity to apply ONLY USE IN FIELD-CENTRIC.

void	StopDrive ()
	Stop all motors.

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

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

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

frc::Rotation2d	GetContinuousOdometryAngle ()

frc::Rotation2d	GetNearestSquareAngle ()

frc::Pose2d	GetContinuousOdometry ()

frc::Pose2d	UpdateEstimatedPose ()
	Reads module states & gyro, updates pose estimator, and returns latest pose estimate.

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

frc::Pose2d	GetPoseEstimate (const frc::Pose2d &robotPose, const units::millisecond_t &latency)
	Get the latest pose estimate.

void	SetControlMode (SwerveDriveSubsystem::DriveControlMode controlMode)

void	InitializeMotors ()
	Initialize motors from persistent storage.

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

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

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

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

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

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

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

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

bool	IsFollowingProfile () const
	Check if drivetrain is following a profile.

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

void	LockWheels ()
	Put the robot wheels in an x shape where it locks the movement of it.

bool	GetManualOverride ()

units::degree_t	GetRobotPitch () const
	Get the robot pitch as determined by the pigeon IMU.

units::degrees_per_second_t	GetRobotPitchRate ()
	Get the rate of robot pitch.

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.

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

wpi::array< frc::SwerveModulePosition, 4 >	GetCurrentModulePositions ()
	Get the active positions of all swerve modules.

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

void	InitializeMotorsFromFS ()
	Initialize motors from saved file.

units::degree_t	GetIMUYaw () const

void	ResetIMUYaw ()

Private Attributes
argos_lib::RobotInstance	m_instance

DriveControlMode	m_controlMode
	Active control mode.

SwerveModule	m_frontLeft
	Front left swerve module.

SwerveModule	m_frontRight
	Front right swerve module.

SwerveModule	m_backRight
	Back right swerve module.

SwerveModule	m_backLeft
	Back left swerve module.

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)

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

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

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

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

frc::SwerveDrivePoseEstimator< 4 >	m_poseEstimator
	accounts vision-based measurements for odometry

argos_lib::SwerveFSHomingStorage	m_fsStorage
	Roborio filesystem access for homes.

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

bool	m_profileComplete
	True once a drive profile has been completed.

bool	m_manualOverride

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

std::unique_ptr< SwerveTrapezoidalSpline >	m_pActiveSwerveSplineProfile
	Profile to execute.

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

frc::ProfiledPIDController< units::radians >::Constraints	m_rotationalPIDConstraints

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

frc::HolonomicDriveController	m_followerController
	Controller to follow drive profile.

argos_lib::NTMotorPIDTuner	m_driveMotorPIDTuner
	Utility to tune drive motors.

argos_lib::NTSubscriber	m_linearFollowerTuner_P

argos_lib::NTSubscriber	m_linearFollowerTuner_I

argos_lib::NTSubscriber	m_linearFollowerTuner_D

argos_lib::NTSubscriber	m_rotationalFollowerTuner_P

argos_lib::NTSubscriber	m_rotationalFollowerTuner_I

argos_lib::NTSubscriber	m_rotationalFollowerTuner_D

argos_lib::NTSubscriber	m_rotationalFollowerConstraintTuner_vel

argos_lib::NTSubscriber	m_rotationalFollowerConstraintTuner_accel

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

◆ GetCurrentModulePositions()

wpi::array< frc::SwerveModulePosition, 4 > SwerveDriveSubsystem::GetCurrentModulePositions ( )

private

Get the active positions of all swerve modules.

Returns: Active module positions

◆ 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

◆ GetManualOverride()

bool SwerveDriveSubsystem::GetManualOverride ( )

◆ GetNearestSquareAngle()

frc::Rotation2d SwerveDriveSubsystem::GetNearestSquareAngle ( )

◆ GetPoseEstimate()

frc::Pose2d SwerveDriveSubsystem::GetPoseEstimate	(	const frc::Pose2d &	robotPose,
		const units::millisecond_t &	latency
	)

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>

◆ GetRobotPitch()

units::degree_t SwerveDriveSubsystem::GetRobotPitch ( ) const

inline

Get the robot pitch as determined by the pigeon IMU.

Returns: pitch in unit degrees

◆ GetRobotPitchRate()

units::degrees_per_second_t SwerveDriveSubsystem::GetRobotPitchRate ( )

Get the rate of robot pitch.

Returns: pitch rate in unit degrees per second

◆ 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

◆ IsFollowingProfile()

bool SwerveDriveSubsystem::IsFollowingProfile ( ) const

Check if drivetrain is following a profile.

Returns: true when robot is following profile

◆ LockWheels()

void SwerveDriveSubsystem::LockWheels ( )

Put the robot wheels in an x shape where it locks the movement of it.

◆ 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() [1/2]

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

◆ StartDrivingProfile() [2/2]

void SwerveDriveSubsystem::StartDrivingProfile ( SwerveTrapezoidalSpline 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() [1/3]

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

◆ SwerveDrive() [2/3]

void SwerveDriveSubsystem::SwerveDrive	(	const units::degree_t &	velAngle,
		const double &	velocity
	)

Takes in speeds as a polar vector, and calculates the forward and side velocity to apply ONLY USE IN FIELD-CENTRIC.

Parameters

velAngle	Angle of velocity vector, [0, 360] with 0 degrees being field-centric home
velocity	Magnitude of velocity on [0, 1] to apply

◆ SwerveDrive() [3/3]

void SwerveDriveSubsystem::SwerveDrive	(	const units::degree_t &	velAngle,
		const double &	velocity,
		const double &	rotVelocity
	)

Same as polar swerve drive function, but also takes in a rotational velocity to apply ONLY USE IN FIELD-CENTRIC.

Parameters

velAngle	Angle of velocity vector, [0, 360] with 0 degrees being field-centric home
velocity	Magnitude of velocity on [0, 1] to apply
rotVelocity	Percent speed of rotation of the chassis. Range [-1.0, 1.0] where positive 1.0 is full speed counterclockwise

◆ UpdateEstimatedPose()

frc::Pose2d SwerveDriveSubsystem::UpdateEstimatedPose ( )

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

Returns: Estimate of robot pose

◆ 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() [1/2]

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

◆ UpdateFollowerRotationalPIDConstraints() [2/2]

void SwerveDriveSubsystem::UpdateFollowerRotationalPIDConstraints ( frc::TrapezoidProfile< units::radians >::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

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

argos_lib::SwerveFSHomingStorage 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_linearFollowerTuner_D

argos_lib::NTSubscriber SwerveDriveSubsystem::m_linearFollowerTuner_D

private

◆ m_linearFollowerTuner_I

argos_lib::NTSubscriber SwerveDriveSubsystem::m_linearFollowerTuner_I

private

◆ m_linearFollowerTuner_P

argos_lib::NTSubscriber SwerveDriveSubsystem::m_linearFollowerTuner_P

private

◆ m_linearPID

frc2::PIDController SwerveDriveSubsystem::m_linearPID

private

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

◆ m_manualOverride

bool SwerveDriveSubsystem::m_manualOverride

private

◆ 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_pActiveSwerveSplineProfile

std::unique_ptr<SwerveTrapezoidalSpline> SwerveDriveSubsystem::m_pActiveSwerveSplineProfile

private

Profile to execute.

◆ m_pigeonIMU

Pigeon2 SwerveDriveSubsystem::m_pigeonIMU

private

◆ m_poseEstimator

frc::SwerveDrivePoseEstimator<4> SwerveDriveSubsystem::m_poseEstimator

private

accounts vision-based measurements for odometry

◆ 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_rotationalFollowerConstraintTuner_accel

argos_lib::NTSubscriber SwerveDriveSubsystem::m_rotationalFollowerConstraintTuner_accel

private

◆ m_rotationalFollowerConstraintTuner_vel

argos_lib::NTSubscriber SwerveDriveSubsystem::m_rotationalFollowerConstraintTuner_vel

private

◆ m_rotationalFollowerTuner_D

argos_lib::NTSubscriber SwerveDriveSubsystem::m_rotationalFollowerTuner_D

private

◆ m_rotationalFollowerTuner_I

argos_lib::NTSubscriber SwerveDriveSubsystem::m_rotationalFollowerTuner_I

private

◆ m_rotationalFollowerTuner_P

argos_lib::NTSubscriber SwerveDriveSubsystem::m_rotationalFollowerTuner_P

private

◆ m_rotationalPIDConstraints

frc::ProfiledPIDController<units::radians>::Constraints SwerveDriveSubsystem::m_rotationalPIDConstraints

private

◆ 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()

◆ GetCurrentModulePositions()

◆ GetCurrentModuleStates()

◆ GetFieldCentricAngle()

◆ GetIMUYaw()

◆ GetManualOverride()

◆ GetNearestSquareAngle()

◆ GetPoseEstimate()

◆ GetRawModuleStates()

◆ GetRobotPitch()

◆ GetRobotPitchRate()

◆ Home()

◆ HomeToFS()

◆ InitializeMotors()

◆ InitializeMotorsFromFS()

◆ InitializeOdometry()

◆ IsFollowingProfile()

◆ LockWheels()

◆ ProfileIsComplete()

◆ ResetIMUYaw()

◆ SetControlMode()

◆ StartDrivingProfile() [1/2]

◆ StartDrivingProfile() [2/2]

◆ StopDrive()

◆ SwerveDrive() [1/3]

◆ SwerveDrive() [2/3]

◆ SwerveDrive() [3/3]

◆ UpdateEstimatedPose()

◆ UpdateFollowerLinearPIDParams()

◆ UpdateFollowerRotationalPIDConstraints() [1/2]

◆ UpdateFollowerRotationalPIDConstraints() [2/2]

◆ UpdateFollowerRotationalPIDParams()

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_linearFollowerTuner_D

◆ m_linearFollowerTuner_I

◆ m_linearFollowerTuner_P

◆ m_linearPID

◆ m_manualOverride

◆ m_odometry

◆ m_pActiveSwerveProfile

◆ m_pActiveSwerveSplineProfile

◆ m_pigeonIMU

◆ m_poseEstimator

◆ m_prevOdometryAngle

◆ m_profileComplete

◆ m_rotationalFollowerConstraintTuner_accel

◆ m_rotationalFollowerConstraintTuner_vel

◆ m_rotationalFollowerTuner_D

◆ m_rotationalFollowerTuner_I

◆ m_rotationalFollowerTuner_P

◆ m_rotationalPIDConstraints