2023-Robot
Robot code for 2023 FRC season by Argos, FRC team #1756
Loading...
Searching...
No Matches
SwerveDriveSubsystem Class Reference

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< SwerveTrapezoidalProfileSegmentm_pActiveSwerveProfile
 Profile to execute.
 
std::unique_ptr< SwerveTrapezoidalSplinem_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

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
homeAngleCurrent orientation of the robot
updateOdometryAlso 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
velocitiesDesired 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
angleCurrent 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
angleCurrent 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
currentPoseField-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
newProfileProfile 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
newProfileProfile 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
fwVelocityPercent speed forward. Range [-1.0, 1.0] where positive 1.0 is full speed forward
sideVelocityPercent speed perpendicular to the robots front. Range [-1.0, 1.0] where positive 1.0 is full speed left
rotVelocityPercent 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
velAngleAngle of velocity vector, [0, 360] with 0 degrees being field-centric home
velocityMagnitude 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
velAngleAngle of velocity vector, [0, 360] with 0 degrees being field-centric home
velocityMagnitude of velocity on [0, 1] to apply
rotVelocityPercent 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
kPProportional gain
kIIntegral gain
kDDerivative 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
constraintsRotational 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
kPProportional gain
kIIntegral gain
kDDerivative 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: