DriveSubsystem Class Reference

Inheritance diagram for DriveSubsystem:

Public Types
using	SwerveModuleStates = wpi::array< SwerveModuleState, DriveConstants::kNumSwerveModules >
	Readable alias for array of swerve modules. More...

Public Member Functions
	DriveSubsystem (Gyro *gyro)
void	Periodic () override
	Will be called periodically whenever the CommandScheduler runs. More...
void	Drive (meters_per_second_t xSpeed, meters_per_second_t ySpeed, radians_per_second_t rot, bool fieldRelative)
void	RotationDrive (meters_per_second_t xSpeed, meters_per_second_t ySpeed, radian_t rot, bool fieldRelative)
void	RotationDrive (meters_per_second_t xSpeed, meters_per_second_t ySpeed, double xRot, double yRot, bool fieldRelative)
void	HeadingDrive (meters_per_second_t xSpeed, meters_per_second_t ySpeed, radians_per_second_t rot, bool fieldRelative)
void	UpdateLastHeading ()
	Updates the last heading set for Heading Drive. Needs to be called if transitioning from other Drive functions to HeadingDrive. More...
void	ResetEncoders ()
	Resets the drive encoders to currently read a position of 0. More...
void	SetModuleStates (SwerveModuleStates desiredStates)
	Sets the drive SpeedControllers to a power from -1 to 1. More...
Pose2d	GetPose ()
double	PWMToPulseWidth (CANifier::PWMChannel pwmChannel)
void	ResetOdometry (Pose2d pose)
void	WheelsForward ()
	Set all 4 wheels to the zero position. More...
void	ResetRelativeToAbsolute ()
	Resync all relative NEO turn encoders to the absolute encoders. More...

Public Attributes
SwerveDriveKinematics< kNumSwerveModules >	kDriveKinematics
	The kinematics object converts inputs into 4 individual swerve module turn angle and wheel speeds. More...

Private Member Functions
SwerveModuleStates	getCurrentWheelSpeeds ()
	Get all 4 swerve module wheel speed to update the odometry with. More...

Private Attributes
CANifier	m_canifier
	Reads the absolute encoder pulse widths. More...
Gyro *	m_gyro
	Gyro to determine field relative driving, from RobotContainer. More...
SwerveDriveOdometry< DriveConstants::kNumSwerveModules >	m_odometry
	Odometry class for tracking robot pose. More...
frc2::PIDController	m_rotationPIDController
	PID to control overall robot chassis rotation. More...
double	m_lastHeading
	Last maintained heading, used for HeadingDrive. More...
bool	m_rotationalInput
	Whether or not rotation input was provided, used for HeadingDrive. More...
Swerve Modules
The drive subsystem owns all 4 swerve modules
SwerveModule	m_frontLeft
SwerveModule	m_frontRight
SwerveModule	m_rearRight
SwerveModule	m_rearLeft

Detailed Description

Definition at line 37 of file DriveSubsystem.h.

Member Typedef Documentation

using DriveSubsystem::SwerveModuleStates = wpi::array<SwerveModuleState, DriveConstants::kNumSwerveModules>

Readable alias for array of swerve modules.

Definition at line 101 of file DriveSubsystem.h.

Member Enumeration Documentation

enum DriveSubsystem::ModuleLocation

< Order as returned by kDriveKinematics.ToSwerveModuleStates

Enumerator
kFrontLeft
kFrontRight
kRearLeft
kRearRight

Definition at line 40 of file DriveSubsystem.h.

    {
        kFrontLeft,
        kFrontRight,
        kRearLeft,
        kRearRight
    };

Constructor & Destructor Documentation

DriveSubsystem::DriveSubsystem

(

Gyro *

gyro

)

Definition at line 11 of file DriveSubsystem.cpp.

References Gyro::GetHeadingAsRot2d(), DriveConstants::kCanifierID, kDriveKinematics, DriveConstants::kFrontLeftDriveMotorPort, DriveConstants::kFrontLeftDriveMotorReversed, DriveConstants::kFrontLeftOffset, DriveConstants::kFrontLeftPWM, DriveConstants::kFrontLeftTurningMotorPort, DriveConstants::kFrontRightDriveMotorPort, DriveConstants::kFrontRightDriveMotorReversed, DriveConstants::kFrontRightOffset, DriveConstants::kFrontRightPWM, DriveConstants::kFrontRightTurningMotorPort, DriveConstants::kRearLeftDriveMotorPort, DriveConstants::kRearLeftDriveMotorReversed, DriveConstants::kRearLeftOffset, DriveConstants::kRearLeftPWM, DriveConstants::kRearLeftTurningMotorPort, DriveConstants::kRearRightDriveMotorPort, DriveConstants::kRearRightDriveMotorReversed, DriveConstants::kRearRightOffset, DriveConstants::kRearRightPWM, DriveConstants::kRearRightTurningMotorPort, DriveConstants::kRotationDriveIMaxRange, DriveConstants::kRotationDriveTolerance, m_canifier, m_frontRight, m_gyro, m_lastHeading, m_odometry, m_rearLeft, m_rearRight, m_rotationalInput, m_rotationPIDController, and PWMToPulseWidth().

    : m_frontLeft
      {
          kFrontLeftDriveMotorPort
        , kFrontLeftTurningMotorPort
        , [this](CANifier::PWMChannel channel){ return PWMToPulseWidth(channel); } 
        , kFrontLeftPWM
        , kFrontLeftDriveMotorReversed
        , kFrontLeftOffset
        , string("FrontLeft")
      }
    , m_frontRight
      {
          kFrontRightDriveMotorPort
        , kFrontRightTurningMotorPort
        , [this](CANifier::PWMChannel channel){ return PWMToPulseWidth(channel); } 
        , kFrontRightPWM
        , kFrontRightDriveMotorReversed
        , kFrontRightOffset
        , string("FrontRight")
      }
    , m_rearRight
      {
          kRearRightDriveMotorPort
        , kRearRightTurningMotorPort
        , [this](CANifier::PWMChannel channel){ return PWMToPulseWidth(channel); } 
        , kRearRightPWM
        , kRearRightDriveMotorReversed
        , kRearRightOffset
        , string("RearRight")
      }
    , m_rearLeft
      {
          kRearLeftDriveMotorPort
        , kRearLeftTurningMotorPort
        , [this](CANifier::PWMChannel channel){ return PWMToPulseWidth(channel); } 
        , kRearLeftPWM
        , kRearLeftDriveMotorReversed
        , kRearLeftOffset
        , string("RearLeft")
      }
    , m_canifier(kCanifierID)
    , m_gyro(gyro)
    , m_odometry{kDriveKinematics, m_gyro->GetHeadingAsRot2d(), Pose2d()}
{

    #ifdef MANUAL_MODULE_STATES
    SmartDashboard::PutNumber("T_D_MFL", 0);
    SmartDashboard::PutNumber("T_D_MFR", 0);
    SmartDashboard::PutNumber("T_D_MRR", 0);
    SmartDashboard::PutNumber("T_D_MRL", 0);
    SmartDashboard::PutNumber("T_D_MFLV", 0);
    SmartDashboard::PutNumber("T_D_MFRV", 0);
    SmartDashboard::PutNumber("T_D_MRRV", 0);
    SmartDashboard::PutNumber("T_D_MRLV", 0);
    #endif

    #ifdef TUNE_ROTATION_DRIVE
    SmartDashboard::PutNumber("T_D_RP", 0);
    SmartDashboard::PutNumber("T_D_RI", 0);
    SmartDashboard::PutNumber("T_D_RD", 0);
    SmartDashboard::PutNumber("T_D_RMax", 0);
    SmartDashboard::PutNumber("T_D_RTMax", 0);
    #endif

    m_rotationPIDController.SetTolerance(kRotationDriveTolerance);
    m_rotationPIDController.SetIntegratorRange(0, kRotationDriveIMaxRange);

    m_lastHeading = 0;
    m_rotationalInput = true;
}

Member Function Documentation

void DriveSubsystem::Periodic ( )

override

Will be called periodically whenever the CommandScheduler runs.

Definition at line 83 of file DriveSubsystem.cpp.

References Gyro::GetHeadingAsRot2d(), SwerveModule::GetState(), m_frontLeft, m_frontRight, m_gyro, m_odometry, m_rearLeft, m_rearRight, and SwerveModule::Periodic().

{
    m_odometry.Update(m_gyro->GetHeadingAsRot2d()
                    , m_frontLeft.GetState()
                    , m_frontRight.GetState()
                    , m_rearLeft.GetState()
                    , m_rearRight.GetState());

    m_frontLeft.Periodic();
    m_frontRight.Periodic();
    m_rearRight.Periodic();
    m_rearLeft.Periodic();
}

void DriveSubsystem::Drive	(	meters_per_second_t	xSpeed,
		meters_per_second_t	ySpeed,
		radians_per_second_t	rot,
		bool	fieldRelative
	)

Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear speeds have no effect on the angular speed.

Parameters

xSpeed	Speed of the robot in the x direction (forward/backwards).
ySpeed	Speed of the robot in the y direction (sideways).
rot	Angular rate of the robot.
fieldRelative	Whether the provided x and y speeds are relative to the field.
isAuto	Whether the bot is using function for auto or not. False by default.

Definition at line 177 of file DriveSubsystem.cpp.

References Gyro::GetHeadingAsRot2d(), kDriveKinematics, DriveConstants::kDriveSpeed, kFrontLeft, kFrontRight, kRearLeft, kRearRight, m_frontLeft, m_frontRight, m_gyro, m_rearLeft, m_rearRight, and SwerveModule::SetDesiredState().

{
    ChassisSpeeds chassisSpeeds;
    if (fieldRelative)
        chassisSpeeds = ChassisSpeeds::FromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro->GetHeadingAsRot2d());
    else
        chassisSpeeds = ChassisSpeeds{xSpeed, ySpeed, rot};

    auto states = kDriveKinematics.ToSwerveModuleStates(chassisSpeeds);

    kDriveKinematics.NormalizeWheelSpeeds(&states, kDriveSpeed);
    
    #ifdef MANUAL_MODULE_STATES
    states[kFrontLeft].angle = Rotation2d(radian_t(SmartDashboard::GetNumber("T_D_MFL", 0.0)));
    states[kFrontRight].angle = Rotation2d(radian_t(SmartDashboard::GetNumber("T_D_MFR", 0.0)));
    states[kRearRight].angle = Rotation2d(radian_t(SmartDashboard::GetNumber("T_D_MRR", 0.0)));
    states[kRearLeft].angle = Rotation2d(radian_t(SmartDashboard::GetNumber("T_D_MRL", 0.0)));
    states[kFrontLeft].speed = SmartDashboard::GetNumber("T_D_MFLV", 0.0) * 1_mps;
    states[kFrontRight].speed = SmartDashboard::GetNumber("T_D_MFRV", 0.0) * 1_mps;
    states[kRearRight].speed = SmartDashboard::GetNumber("T_D_MRRV", 0.0) * 1_mps;
    states[kRearLeft].speed = SmartDashboard::GetNumber("T_D_MRLV", 0.0) * 1_mps;
    #endif

    m_frontLeft.SetDesiredState(states[kFrontLeft]);
    m_frontRight.SetDesiredState(states[kFrontRight]);
    m_rearLeft.SetDesiredState(states[kRearLeft]);
    m_rearRight.SetDesiredState(states[kRearRight]);
}

void DriveSubsystem::RotationDrive	(	meters_per_second_t	xSpeed,
		meters_per_second_t	ySpeed,
		radian_t	rot,
		bool	fieldRelative
	)

Parameters

xSpeed	Speed of the robot in the x direction (forward/backwards).
ySpeed	Speed of the robot in the y direction (sideways).
rot	Angle of the robot in radians
fieldRelative	Whether the provided translational speeds are relative to the field.

Definition at line 97 of file DriveSubsystem.cpp.

References Drive(), Gyro::GetHeadingAsRot2d(), Gyro::GetTurnRate(), DriveConstants::kRotationDriveDirectionLimit, DriveConstants::kRotationDriveMaxSpeed, m_gyro, m_rotationPIDController, and Util::NegPiToPiRads().

{  
    double error = rot.to<double>() - m_gyro->GetHeadingAsRot2d().Radians().to<double>();
    double desiredSet = Util::NegPiToPiRads(error);
    double max = kRotationDriveMaxSpeed;
    double maxTurn = kRotationDriveDirectionLimit;

    #ifdef TUNE_ROTATION_DRIVE
    double P = SmartDashboard::GetNumber("T_D_RP", 0);
    double I = SmartDashboard::GetNumber("T_D_RI", 0);
    double D = SmartDashboard::GetNumber("T_D_RD", 0);
    double m = SmartDashboard::GetNumber("T_D_RMax", 0);
    double mTurn = SmartDashboard::GetNumber("T_D_RTMax", 0);

    m_rotationPIDController.SetP(P);
    m_rotationPIDController.SetI(I);
    m_rotationPIDController.SetD(D);
    max = m;
    maxTurn = mTurn;
    #endif

    double desiredTurnRate = m_rotationPIDController.Calculate(0, desiredSet);

    double currentTurnRate = m_gyro->GetTurnRate() * wpi::math::pi / 180;

    // Prevent sharp turning if already fast going in the opposite direction
    if ((abs(currentTurnRate) >= maxTurn) && (signbit(desiredTurnRate) != signbit(currentTurnRate)))
        desiredTurnRate *= -1.0;

    // Power limiting
    if (abs(desiredTurnRate) > max)
        desiredTurnRate = signbit(desiredTurnRate) ? max * -1.0 : max;

    Drive(xSpeed, ySpeed, radians_per_second_t(desiredTurnRate), fieldRelative);
}

void DriveSubsystem::RotationDrive	(	meters_per_second_t	xSpeed,
		meters_per_second_t	ySpeed,
		double	xRot,
		double	yRot,
		bool	fieldRelative
	)

Parameters

xSpeed	Speed of the robot in the x direction (forward/backwards).
ySpeed	Speed of the robot in the y direction (sideways).
xRot	Angle of the robot on the x axis
yRot	Angle of the robot on the y axis
fieldRelative	Whether the provided translational speeds are relative to the field.

Definition at line 136 of file DriveSubsystem.cpp.

References Drive(), m_rotationalInput, and RotationDrive().

{
    if (xRot != 0 || yRot != 0)
    {
        m_rotationalInput = true;
        RotationDrive(xSpeed, ySpeed, radian_t(atan2f(yRot, xRot)), fieldRelative);
    }
    else
        Drive(xSpeed, ySpeed, radians_per_second_t(0), fieldRelative);
    
}

void DriveSubsystem::HeadingDrive	(	meters_per_second_t	xSpeed,
		meters_per_second_t	ySpeed,
		radians_per_second_t	rot,
		bool	fieldRelative
	)

Drives the robot and maintains robot angle with no rotational input

Parameters

xSpeed	Speed of the robot in the x direction (forward/backwards).
ySpeed	Speed of the robot in the y direction (sideways).
rot	Angular rate of the robot.
fieldRelative	Whether the provided x and y speeds are relative to the field.

Definition at line 152 of file DriveSubsystem.cpp.

References Drive(), m_lastHeading, m_rotationalInput, RotationDrive(), and UpdateLastHeading().

{
    if (xSpeed.to<double>() == 0 && ySpeed.to<double>() == 0 &&   rot.to<double>() == 0)
    {
        Drive(xSpeed, ySpeed, rot, fieldRelative);
        return;
    }

    if (rot.to<double>() == 0 && m_rotationalInput)
    {
        m_rotationalInput = false;
        UpdateLastHeading();
    }
    else if (rot.to<double>() != 0)
        m_rotationalInput = true;
    
    if (!m_rotationalInput && (xSpeed.to<double>() != 0 || ySpeed.to<double>() != 0))
        RotationDrive(xSpeed, ySpeed, radian_t(m_lastHeading), fieldRelative);
    else
        Drive(xSpeed, ySpeed, rot, fieldRelative);
}

void DriveSubsystem::UpdateLastHeading

(

)

Updates the last heading set for Heading Drive. Needs to be called if transitioning from other Drive functions to HeadingDrive.

Definition at line 218 of file DriveSubsystem.cpp.

References Gyro::GetHeadingAsRot2d(), m_gyro, and m_lastHeading.

{
    m_lastHeading = m_gyro->GetHeadingAsRot2d().Radians().to<double>();
}

void DriveSubsystem::ResetEncoders

(

)

Resets the drive encoders to currently read a position of 0.

Definition at line 223 of file DriveSubsystem.cpp.

References m_frontLeft, m_frontRight, m_rearLeft, m_rearRight, and SwerveModule::ResetEncoders().

{
    m_frontLeft.ResetEncoders();
    m_frontRight.ResetEncoders();
    m_rearRight.ResetEncoders();
    m_rearLeft.ResetEncoders();
}

void DriveSubsystem::SetModuleStates

(

SwerveModuleStates

desiredStates

)

Sets the drive SpeedControllers to a power from -1 to 1.

Definition at line 209 of file DriveSubsystem.cpp.

References kDriveKinematics, kFrontLeft, kFrontRight, AutoConstants::kMaxSpeed, kRearLeft, kRearRight, m_frontLeft, m_frontRight, m_rearLeft, m_rearRight, and SwerveModule::SetDesiredState().

{
    kDriveKinematics.NormalizeWheelSpeeds(&desiredStates, AutoConstants::kMaxSpeed);
    m_frontLeft.SetDesiredState(desiredStates[kFrontLeft]);
    m_frontRight.SetDesiredState(desiredStates[kFrontRight]);
    m_rearRight.SetDesiredState(desiredStates[kRearRight]);
    m_rearLeft.SetDesiredState(desiredStates[kRearLeft]);
}

Pose2d DriveSubsystem::GetPose

(

)

Returns the currently-estimated pose of the robot.

Returns

The pose.

Definition at line 231 of file DriveSubsystem.cpp.

References m_odometry.

{
    return m_odometry.GetPose();
}

double DriveSubsystem::PWMToPulseWidth

(

CANifier::PWMChannel

pwmChannel

)

Converts PWM input on the CANifier to a pulse width

Parameters

pwmChannel

The PWM channel to pass in

Returns

The pulse width of the PWM channel

Definition at line 236 of file DriveSubsystem.cpp.

References DriveConstants::kPulseWidthToZeroOne, and m_canifier.

{
    double dutyCycleAndPeriod[2];
    m_canifier.GetPWMInput(pwmChannel, dutyCycleAndPeriod);
    return dutyCycleAndPeriod[0] * dutyCycleAndPeriod[1] / kPulseWidthToZeroOne;
}

void DriveSubsystem::ResetOdometry

(

Pose2d

pose

)

Resets the odometry to the specified pose.

Parameters

pose	The pose to which to set the odometry.

Definition at line 243 of file DriveSubsystem.cpp.

References Gyro::GetHeadingAsRot2d(), m_gyro, and m_odometry.

{
    m_odometry.ResetPosition(pose, m_gyro->GetHeadingAsRot2d());
}

void DriveSubsystem::WheelsForward

(

)

Set all 4 wheels to the zero position.

Definition at line 256 of file DriveSubsystem.cpp.

References m_frontLeft, m_frontRight, m_rearLeft, m_rearRight, and SwerveModule::SetDesiredState().

{
    static SwerveModuleState zeroState { 0_mps, 0_deg };
    printf("DriveSubsystem::WheelsForward() called");
    m_frontLeft.SetDesiredState(zeroState);
    m_frontRight.SetDesiredState(zeroState);
    m_rearRight.SetDesiredState(zeroState);
    m_rearLeft.SetDesiredState(zeroState);
}

void DriveSubsystem::ResetRelativeToAbsolute

(

)

Resync all relative NEO turn encoders to the absolute encoders.

Definition at line 248 of file DriveSubsystem.cpp.

References m_frontLeft, m_frontRight, m_rearLeft, m_rearRight, and SwerveModule::ResetRelativeToAbsolute().

{
    m_frontLeft.ResetRelativeToAbsolute();
    m_frontRight.ResetRelativeToAbsolute();
    m_rearRight.ResetRelativeToAbsolute();
    m_rearLeft.ResetRelativeToAbsolute();
}

SwerveModuleStates DriveSubsystem::getCurrentWheelSpeeds ( )

inlineprivate

Get all 4 swerve module wheel speed to update the odometry with.

Definition at line 133 of file DriveSubsystem.h.

    {
        SwerveModuleStates sms({
            m_frontLeft.GetState(),
            m_frontRight.GetState(),
            m_rearLeft.GetState(),
            m_rearRight.GetState()
        });
        return sms;
    }

Member Data Documentation

SwerveDriveKinematics<kNumSwerveModules> DriveSubsystem::kDriveKinematics

Initial value:

{
        Translation2d( kWheelBase / 2,  kTrackWidth / 2),    
        Translation2d( kWheelBase / 2, -kTrackWidth / 2),    
        Translation2d(-kWheelBase / 2,  kTrackWidth / 2),    
        Translation2d(-kWheelBase / 2, -kTrackWidth / 2)}

The kinematics object converts inputs into 4 individual swerve module turn angle and wheel speeds.

Definition at line 125 of file DriveSubsystem.h.

SwerveModule DriveSubsystem::m_frontLeft

private

Definition at line 147 of file DriveSubsystem.h.

SwerveModule DriveSubsystem::m_frontRight

private

Definition at line 148 of file DriveSubsystem.h.

SwerveModule DriveSubsystem::m_rearRight

private

Definition at line 149 of file DriveSubsystem.h.

SwerveModule DriveSubsystem::m_rearLeft

private

Definition at line 150 of file DriveSubsystem.h.

CANifier DriveSubsystem::m_canifier

private

Reads the absolute encoder pulse widths.

Definition at line 154 of file DriveSubsystem.h.

Gyro* DriveSubsystem::m_gyro

private

Gyro to determine field relative driving, from RobotContainer.

Definition at line 156 of file DriveSubsystem.h.

SwerveDriveOdometry<DriveConstants::kNumSwerveModules> DriveSubsystem::m_odometry

private

Odometry class for tracking robot pose.

Definition at line 158 of file DriveSubsystem.h.

frc2::PIDController DriveSubsystem::m_rotationPIDController

private

Initial value:

{
        DriveConstants::kRotationDriveP,
        DriveConstants::kRotationDriveI,
        DriveConstants::kRotationDriveD
    }

PID to control overall robot chassis rotation.

Definition at line 161 of file DriveSubsystem.h.

double DriveSubsystem::m_lastHeading

private

Last maintained heading, used for HeadingDrive.

Definition at line 167 of file DriveSubsystem.h.

bool DriveSubsystem::m_rotationalInput

private

Whether or not rotation input was provided, used for HeadingDrive.

Definition at line 169 of file DriveSubsystem.h.

---

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

• src/main/include/subsystems/DriveSubsystem.h
• src/main/cpp/subsystems/DriveSubsystem.cpp