SwerveModule Class Reference

Public Member Functions
	SwerveModule (int driveMotorChannel, int turningMotorChannel, GetPulseWidthCallback pulseWidthCallback, CANifier::PWMChannel pwmChannel, bool driveEncoderReversed, double offSet, const string &name)
void	Periodic ()
SwerveModuleState	GetState ()
void	SetDesiredState (SwerveModuleState &state)
void	ResetEncoders ()
	Resets the drive motor encoders to 0. More...
void	ResetRelativeToAbsolute ()
	Resync the relative NEO turn encoder to the absolute encoder. More...

Private Types
using	radians_per_second_squared_t = compound_unit< radians, inverse< squared< second >>>

Private Member Functions
double	CalcAbsoluteAngle ()
	Calculate the absolute angle, in radians, based on the pulse widths from m_pulseWidthCallback. More...
double	MinTurnRads (double init, double final, bool &bOutputReverse)
meters_per_second_t	CalcMetersPerSec ()
double	CalcTicksPer100Ms (meters_per_second_t speed)

Private Attributes
double	m_offset
	The offset, in pulse widths, for syncing the relative encoder to the absolute encoder. More...
string	m_name
	String used to identify each pod, used for SmartDashboard prints. More...
TalonFX	m_driveMotor
	Falon 500 that drives the pod. More...
PIDLoaderFalcon	m_drivePIDLoader
	PID param loader for the TalonFX. More...
PIDLoaderNEO	m_turnPIDLoader
	PID param loader for the CANSparkMax. More...
GetPulseWidthCallback	m_pulseWidthCallback
CANifier::PWMChannel	m_pwmChannel
Timer	m_timer
	Timer used to sync absolute and relative encoders on robot turn on. More...
NEO that turns the pod, controls angle with relative encoder and PID
CANSparkMax	m_turningMotor
CANEncoder	m_turnRelativeEncoder
CANPIDController	m_turnPIDController = m_turningMotor.GetPIDController()

Detailed Description

Definition at line 51 of file SwerveModule.h.

Member Typedef Documentation

using SwerveModule::radians_per_second_squared_t = compound_unit<radians, inverse<squared<second>>>

private

Definition at line 53 of file SwerveModule.h.

Constructor & Destructor Documentation

SwerveModule::SwerveModule	(	int	driveMotorChannel,
		int	turningMotorChannel,
		GetPulseWidthCallback	pulseWidthCallback,
		CANifier::PWMChannel	pwmChannel,
		bool	driveEncoderReversed,
		double	offSet,
		const string &	name
	)

Definition at line 11 of file SwerveModule.cpp.

References ModuleConstants::kMotorCurrentLimit, ModuleConstants::kTurnMotorRevsPerWheelRev, PIDLoaderFalcon::Load(), PIDLoaderNEO::Load(), m_driveMotor, m_drivePIDLoader, m_timer, m_turningMotor, m_turnPIDController, m_turnPIDLoader, and m_turnRelativeEncoder.

    : m_offset(offset)
    , m_name(name)
    , m_driveMotor(driveMotorChannel)
    , m_turningMotor(turningMotorChannel, CANSparkMax::MotorType::kBrushless)
    , m_drivePIDLoader("SM", kDriveAdjust, kDriveP, kDriveI, kDriveD, kDriveFF)
    , m_turnPIDLoader("SM", kTurnAdjust, kTurnP, kTurnI, kTurnD, kTurnIZ, kTurnIA)
    , m_pulseWidthCallback(pulseWidthCallback)
    , m_pwmChannel(pwmChannel)
{
    StatorCurrentLimitConfiguration statorLimit { true, kMotorCurrentLimit, kMotorCurrentLimit, 2 };
    m_driveMotor.ConfigStatorCurrentLimit(statorLimit);
    SupplyCurrentLimitConfiguration supplyLimit { true, kMotorCurrentLimit, kMotorCurrentLimit, 2 };
    m_driveMotor.ConfigSupplyCurrentLimit(supplyLimit);
    m_turningMotor.SetSmartCurrentLimit(kMotorCurrentLimit);

    // Set up GetVelocity() to return meters per sec instead of RPM
    m_turnRelativeEncoder.SetPositionConversionFactor(2.0 * wpi::math::pi / kTurnMotorRevsPerWheelRev);
    
    m_driveMotor.SetInverted(driveMotorReversed ? TalonFXInvertType::CounterClockwise : TalonFXInvertType::Clockwise);
    m_turningMotor.SetInverted(false);
    m_driveMotor.ConfigSelectedFeedbackSensor(TalonFXFeedbackDevice::IntegratedSensor);
    m_turnPIDController.SetFeedbackDevice(m_turnRelativeEncoder);
    
    m_turningMotor.SetIdleMode(CANSparkMax::IdleMode::kBrake);
    m_driveMotor.SetNeutralMode(NeutralMode::Brake);

    m_drivePIDLoader.Load(m_driveMotor);
    m_turnPIDLoader.Load(m_turnPIDController);

    m_timer.Reset();
    m_timer.Start();
}

Member Function Documentation

void SwerveModule::Periodic

(

)

Definition at line 51 of file SwerveModule.cpp.

References CalcAbsoluteAngle(), m_name, m_timer, m_turnRelativeEncoder, and ResetRelativeToAbsolute().

{
    if (m_timer.Get() < 5)
        ResetRelativeToAbsolute();

    double absAngle = CalcAbsoluteAngle();
    SmartDashboard::PutNumber("D_SM_Rel " + m_name, m_turnRelativeEncoder.GetPosition());
    SmartDashboard::PutNumber("D_SM_Abs " + m_name, absAngle);
    SmartDashboard::PutNumber("D_SM_AbsDiff " + m_name, m_turnRelativeEncoder.GetPosition() - absAngle);
    // SmartDashboard::PutNumber("D_SM_MPS " + m_name, CalcMetersPerSec().to<double>());
    // SmartDashboard::PutNumber("D_SM_IError " + m_name, m_turnPIDController.GetIAccum());
    // SmartDashboard::PutNumber("D_SM_TP100MS " + m_name, m_driveMotor.GetSelectedSensorVelocity());
}

SwerveModuleState SwerveModule::GetState

(

)

Get the state for the swerve module pod

Returns

The state (vector with speed and angle) representig the current module state

Todo:

Currently GetState uses the absolute angle instead of the relative angle that we should be using

Why do we find the absolute angle here instead of the relative angle?

Definition at line 65 of file SwerveModule.cpp.

References CalcAbsoluteAngle(), and CalcMetersPerSec().

{
    return { CalcMetersPerSec(), Rotation2d(radian_t(CalcAbsoluteAngle()))};
}

void SwerveModule::SetDesiredState

(

SwerveModuleState &

state

)

Set the desired state for the swerve module pod

Parameters

state

The state (vector with speed and angle) representing the desired module state

Definition at line 71 of file SwerveModule.cpp.

References CalcTicksPer100Ms(), PIDLoaderFalcon::LoadFromNetworkTable(), PIDLoaderNEO::LoadFromNetworkTable(), m_driveMotor, m_drivePIDLoader, m_turnPIDController, m_turnPIDLoader, m_turnRelativeEncoder, and MinTurnRads().

{
    // Retrieving PID values from SmartDashboard if enabled
    m_drivePIDLoader.LoadFromNetworkTable(m_driveMotor);
    m_turnPIDLoader.LoadFromNetworkTable(m_turnPIDController);

    // Get relative encoder position
    double currentPosition = m_turnRelativeEncoder.GetPosition();

    // Calculate new turn position given current and desired position
    bool bOutputReverse = false;
    double minTurnRads = MinTurnRads(currentPosition, state.angle.Radians().to<double>(), bOutputReverse);
    double direction = 1.0; // Sent to TalonFX
    if (bOutputReverse)
        direction = -1.0;

    // Set position reference of turnPIDController
    double newPosition = currentPosition + minTurnRads;

    if (state.speed != 0_mps) {
        #ifdef DISABLE_DRIVE
        m_driveMotor.Set(TalonFXControlMode::Velocity, 0.0);
        #else
        m_driveMotor.Set(TalonFXControlMode::Velocity, direction * CalcTicksPer100Ms(state.speed));
        #endif
    }
    else
        m_driveMotor.Set(TalonFXControlMode::PercentOutput, 0.0);

    // Set the angle unless module is coming to a full stop
    if (state.speed.to<double>() != 0.0)
        m_turnPIDController.SetReference(newPosition, ControlType::kPosition);

    // SmartDashboard::PutNumber("D_SM_SetpointMPS " + m_name, state.speed.to<double>());
    // SmartDashboard::PutNumber("D_SM_Error " + m_name, newPosition - m_turnRelativeEncoder.GetPosition());
}

void SwerveModule::ResetEncoders

(

)

Resets the drive motor encoders to 0.

Definition at line 108 of file SwerveModule.cpp.

References m_driveMotor.

{
    m_driveMotor.SetSelectedSensorPosition(0.0);
}

void SwerveModule::ResetRelativeToAbsolute

(

)

Resync the relative NEO turn encoder to the absolute encoder.

Definition at line 123 of file SwerveModule.cpp.

References CalcAbsoluteAngle(), and m_turnRelativeEncoder.

{
    m_turnRelativeEncoder.SetPosition(CalcAbsoluteAngle());
}

double SwerveModule::CalcAbsoluteAngle ( )

private

Calculate the absolute angle, in radians, based on the pulse widths from m_pulseWidthCallback.

Definition at line 113 of file SwerveModule.cpp.

References DriveConstants::kPulseWidthToRadians, m_offset, m_pulseWidthCallback, and m_pwmChannel.

{
    double pulseWidth = m_pulseWidthCallback(m_pwmChannel);
    // Pulse Width per rotation is not equal for all encoders. Some are 0 - 3865, some are 0 - 4096
    return fmod((pulseWidth - m_offset) * DriveConstants::kPulseWidthToRadians + 2.0 * wpi::math::pi, 2.0 * wpi::math::pi);
    // SmartDashboard::PutNumber("D_SM_PW " + m_name, pulseWidth);
    // SmartDashboard::PutNumber("D_SM_AA " + m_name, absAngle);
    // Convert CW to CCW? absAngle = 2.0 * wpi::math::pi - absAngle;
}

double SwerveModule::MinTurnRads ( double  init, double  final, bool &  bOutputReverse  )

private

Determine the smallest magnitude delta angle that can be added to initial angle that will result in an angle equivalent (but not necessarily equal) to final angle. All angles in radians Currently final - init difference is always chosen regardless of angle

Definition at line 133 of file SwerveModule.cpp.

References Util::NegPiToPiRads(), and Util::ZeroTo2PiRads().

{
    init = Util::ZeroTo2PiRads(init);
    final = Util::ZeroTo2PiRads(final);

    // The shortest turn angle may be acheived by reversing the motor output direction
    double angle1 = final - init;
    double angle2 = final + wpi::math::pi - init;

    angle1 = Util::NegPiToPiRads(angle1);
    angle2 = Util::NegPiToPiRads(angle2);

    // Choose the smallest angle and determine reverse flag
    //TODO: FINISHED ROBOT TUNING
    // Eventually prefer angle 1 always during high speed to prevent 180s
    // if (fabs(angle1) <= 2 * fabs(angle2))
    // {
        bOutputReverse = false;

        return angle1;
    // } 
    // else
    // {
    //     bOutputReverse = true;

    //     return angle2;
    // }
}

meters_per_second_t SwerveModule::CalcMetersPerSec ( )

private

Calculate the MPS of the drive motor based on its current encoder tick velocity

Returns

Meters per second

Definition at line 162 of file SwerveModule.cpp.

References ModuleConstants::kDriveEncoderMetersPerSec, and m_driveMotor.

{
   double ticksPer100ms = m_driveMotor.GetSelectedSensorVelocity();
   return meters_per_second_t(kDriveEncoderMetersPerSec * ticksPer100ms);
}

double SwerveModule::CalcTicksPer100Ms ( meters_per_second_t  speed )

private

Calculate drive motor encoder ticks based on the MPS

Parameters

speed

Meters per second

Returns

Encoder ticks

Definition at line 168 of file SwerveModule.cpp.

References ModuleConstants::kDriveEncoderMetersPerSec.

{
   return speed.to<double>() / kDriveEncoderMetersPerSec;
}

Member Data Documentation

double SwerveModule::m_offset

private

The offset, in pulse widths, for syncing the relative encoder to the absolute encoder.

Definition at line 96 of file SwerveModule.h.

string SwerveModule::m_name

private

String used to identify each pod, used for SmartDashboard prints.

Definition at line 98 of file SwerveModule.h.

TalonFX SwerveModule::m_driveMotor

private

Falon 500 that drives the pod.

Definition at line 101 of file SwerveModule.h.

CANSparkMax SwerveModule::m_turningMotor

private

Definition at line 104 of file SwerveModule.h.

CANEncoder SwerveModule::m_turnRelativeEncoder

private

Initial value:

= m_turningMotor.GetAlternateEncoder(CANEncoder::AlternateEncoderType::kQuadrature, 
                                                                          ModuleConstants::kTurnEncoderCPR)

Definition at line 105 of file SwerveModule.h.

CANPIDController SwerveModule::m_turnPIDController = m_turningMotor.GetPIDController()

private

Definition at line 107 of file SwerveModule.h.

PIDLoaderFalcon SwerveModule::m_drivePIDLoader

private

PID param loader for the TalonFX.

Definition at line 111 of file SwerveModule.h.

PIDLoaderNEO SwerveModule::m_turnPIDLoader

private

PID param loader for the CANSparkMax.

Definition at line 113 of file SwerveModule.h.

GetPulseWidthCallback SwerveModule::m_pulseWidthCallback

private

Callback used to determine the pulse width

Parameters

pwmChannel

Channel to decide which absolute encoder's pulse width values are retrieved

Definition at line 117 of file SwerveModule.h.

CANifier::PWMChannel SwerveModule::m_pwmChannel

private

Definition at line 118 of file SwerveModule.h.

Timer SwerveModule::m_timer

private

Timer used to sync absolute and relative encoders on robot turn on.

Definition at line 121 of file SwerveModule.h.

---

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

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