Package tagalong.subsystems.micro

Class Pivot

java.lang.Object

tagalong.subsystems.micro.Microsystem

tagalong.subsystems.micro.Pivot

public class Pivot
extends Microsystem

Pivot microsystem

Field Summary

Fields

Modifier and Type	Field	Description
final double	_absoluteRangeRot	Absolute range of pivot movement in rotations
final double	_defaultPivotLowerToleranceRot	Default lower tolerance of the pivot in rotations, Default upper tolerance of the pivot in rotations
final double	_defaultPivotUpperToleranceRot	Default lower tolerance of the pivot in rotations, Default upper tolerance of the pivot in rotations
protected double	_encoderToPivotRatio	Gear ratio from the encoder to pivot mechanism (conversion g.t.
final double	_ffCenterOfMassOffsetRad	Offset value for the target position
final double	_maxAccelerationRPS2	Maximum velocity of the pivot in rotations per second, Maximum acceleration of the pivot in rotations per second squared
final double	_maxPositionRot	Minimum position of the pivot in rotations, Maximum position of the pivot in rotations
final double	_maxVelocityRPS	Maximum velocity of the pivot in rotations per second, Maximum acceleration of the pivot in rotations per second squared
final double	_minPositionRot	Minimum position of the pivot in rotations, Maximum position of the pivot in rotations
protected double	_motorToEncoderRatio	Gear ratio from the motor to encoder (conversion g.t.
com.ctre.phoenix6.hardware.CANcoder	_pivotCancoder	CANcoder device
protected com.ctre.phoenix6.configs.CANcoderConfiguration	_pivotCancoderConfiguration	Configuration for the CANcoder
protected com.ctre.phoenix6.sim.CANcoderSimState	_pivotCancoderSim	CANcoder simulation
final PivotConf	_pivotConf	Configuration for the pivot
protected ArmFeedforward	_pivotFF	Feedforward model for the pivot
protected MechanismLigament2d	_pivotLigament	Simulated arm of the pivot
protected SingleJointedArmSim	_pivotSim	Single jointed arm simulation for the pivot
final double	_profileTargetOffset	Offset value for the target position
protected double	_simAccelRPS2	Acceleration of the simulated pivot in rotations per second squared
protected double	_simRotations	Position of the simulated pivot in rotations
protected double	_simVeloRPS	Velocity of the simulated pivot in rotations per second

Fields inherited from class tagalong.subsystems.micro.Microsystem

_allMotors, _conf, _configuredMicrosystemDisable, _currentPositionEntry, _currentVelocityEntry, _curState, _followProfile, _goalState, _isFFTuningMicro, _isMicrosystemDisabled, _isPIDTuningMicro, _isShuffleboardMicro, _KAEntry, _KGEntry, _KSEntry, _KVEntry, _mechanism, _primaryMotor, _primaryMotorSim, _profileTimer, _requestedPositionVoltage, _requestedTorqueCurrent, _requestedVelocityVoltage, _root, _slot0DFactorEntry, _slot0IFactorEntry, _slot0PFactorEntry, _slot1DFactorEntry, _slot1IFactorEntry, _slot1PFactorEntry, _slot2DFactorEntry, _slot2IFactorEntry, _slot2PFactorEntry, _targetPositionEntry, _targetVelocityEntry, _toleranceTimer, _trapProfile, _tuningTabCounter

Constructor Summary

Constructors

Constructor	Description
Pivot(PivotConf conf)	Constructs a pivot microsystem with the below configurations

Method Summary

Modifier and Type	Method	Description
double	absoluteClamp(double value)	Directs the pivot to its maximum position, current goal position, or minimum position
double	clampPivotPosition(double target)	Clamps the goal pivot position to be within rotational limits
void	configShuffleboard()	Configure shuffleboard for the pivot
void	followLastProfile()	Calculates the next state according to the trapezoidal profile and requests the pivot motor(s) to arrive at the next position with feedforward
double	getFFPositionRad()	Gets the position offset for feedforward (to account for a shifted center of mass) in rotations
com.ctre.phoenix6.hardware.TalonFX	getMotor()	Gets the primary motor of the pivot
MechanismLigament2d	getPivotLigament()	Retrieves ligament attached to pivot Mechanism2d
double	getPivotPosition()	Gets the current position of the pivot in rotations
double	getPivotPower()	Gets the power of the primary motor
double	getPivotVelocity()	Gets the current velocity of the pivot in rotations per second
void	holdCurrentPosition()	Sets the power of the primary motor to zero
boolean	isPivotInAbsoluteTolerance(double lowerBound, double upperBound)	Bounds checking function that uses the absolute current pivot position (modulo one rotation)
boolean	isPivotInTolerance(double lowerBound, double upperBound)	Bounds checking function that uses the current pivot position
boolean	isSafeToMove()	Checks whether or not it's safe for the pivot to move
boolean	motorResetConfig()	Resets devices if resets have occurred
double	motorToPivotRot(double motorRot)	Converts motor rotations to rotations of the pivot mechanism
void	onDisable()	returns nothing if micro system is disabled
void	onEnable()	Executes logic for when the robot is first enabled
void	periodic()	Periodic update function
double	pivotRotToMotor(double pivotRot)	Converts rotations of the pivot mechanism to motor rotations
double	placePivotInClosestRot(double scopeReferenceRot, double newAngleRot)	Returns the new pivot angle in the closest scope of the reference pivot angle
void	setPivotPower(double power)	Sets the power of the primary motor
void	setPivotProfile(double goalPositionRot)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(double goalPositionRot, double goalVelocityRPS)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(double goalPositionRot, double goalVelocityRPS, double maxVelocityRPS)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(double goalPositionRot, double goalVelocityRPS, double maxVelocityRPS, double maxAccelerationRPS2)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(double goalPositionRot, double goalVelocityRPS, double maxVelocityRPS, double maxAccelerationRPS2, boolean setCurrentState)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(Angle goalPosition)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(Angle goalPosition, double goalVelocityRPS)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(Angle goalPosition, double goalVelocityRPS, double maxVelocityRPS)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(Angle goalPosition, double goalVelocityRPS, double maxVelocityRPS, double maxAccelerationRPS2)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotProfile(Angle goalPosition, double goalVelocityRPS, double maxVelocityRPS, double maxAccelerationRPS2, boolean setCurrentState)	Creates a new trapezoidal profile for the pivot to follow
void	setPivotVelocity(double rps, boolean withFF)	Sets the velocity of the pivot in RPS
void	simulationInit()	Initializes the pivot simulation
void	simulationPeriodic()	Runs the pivot simulation--sets motor and cancoder simulation fields and updates the visual
void	updateShuffleboard()	Updates shuffleboard with the pivot's current position and velocity

Methods inherited from class tagalong.subsystems.micro.Microsystem

checkInitStatus, checkToleranceTime, configAllDevices, configTuningModes, disableMicrosystem, getPrimaryMotor, getPrimaryMotorPosition, getPrimaryMotorPower, getPrimaryMotorVelocity, getRoot, isProfileFinished, onTeleopDisable, resetToleranceTimer, setBrakeMode, setFollowProfile, setHoldPosition, setPrimaryPower, updateAllPIDSGVA, waitForInitialization

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Details

_pivotConf

public final PivotConf _pivotConf

Configuration for the pivot

_pivotCancoder

public com.ctre.phoenix6.hardware.CANcoder _pivotCancoder

CANcoder device

_pivotCancoderConfiguration

protected com.ctre.phoenix6.configs.CANcoderConfiguration _pivotCancoderConfiguration

Configuration for the CANcoder

_motorToEncoderRatio

protected double _motorToEncoderRatio

Gear ratio from the motor to encoder (conversion g.t. 1 represents a reduction)

_encoderToPivotRatio

protected double _encoderToPivotRatio

Gear ratio from the encoder to pivot mechanism (conversion g.t. 1 represents a reduction)

_defaultPivotLowerToleranceRot

public final double _defaultPivotLowerToleranceRot

Default lower tolerance of the pivot in rotations, Default upper tolerance of the pivot in rotations

_defaultPivotUpperToleranceRot

public final double _defaultPivotUpperToleranceRot

Default lower tolerance of the pivot in rotations, Default upper tolerance of the pivot in rotations

_absoluteRangeRot

public final double _absoluteRangeRot

Absolute range of pivot movement in rotations

_minPositionRot

public final double _minPositionRot

Minimum position of the pivot in rotations, Maximum position of the pivot in rotations

_maxPositionRot

public final double _maxPositionRot

Minimum position of the pivot in rotations, Maximum position of the pivot in rotations

_maxVelocityRPS

public final double _maxVelocityRPS

Maximum velocity of the pivot in rotations per second, Maximum acceleration of the pivot in rotations per second squared

_maxAccelerationRPS2

public final double _maxAccelerationRPS2

Maximum velocity of the pivot in rotations per second, Maximum acceleration of the pivot in rotations per second squared

_profileTargetOffset

public final double _profileTargetOffset

Offset value for the target position

_pivotFF

protected ArmFeedforward _pivotFF

Feedforward model for the pivot

_ffCenterOfMassOffsetRad

public final double _ffCenterOfMassOffsetRad

Offset value for the target position

_pivotSim

protected SingleJointedArmSim _pivotSim

Single jointed arm simulation for the pivot

_pivotCancoderSim

protected com.ctre.phoenix6.sim.CANcoderSimState _pivotCancoderSim

CANcoder simulation

_simRotations

protected double _simRotations

Position of the simulated pivot in rotations

_simVeloRPS

protected double _simVeloRPS

Velocity of the simulated pivot in rotations per second

_simAccelRPS2

protected double _simAccelRPS2

Acceleration of the simulated pivot in rotations per second squared

_pivotLigament

protected MechanismLigament2d _pivotLigament

Simulated arm of the pivot

Constructor Details

Pivot

public Pivot(PivotConf conf)

Constructs a pivot microsystem with the below configurations

Parameters:

conf - Configurations for the pivot

Method Details

periodic

public void periodic()

Periodic update function

Overrides:

periodic in class Microsystem

getMotor

public com.ctre.phoenix6.hardware.TalonFX getMotor()

Gets the primary motor of the pivot

Returns:

primary motor

onEnable

public void onEnable()

Executes logic for when the robot is first enabled

Overrides:

onEnable in class Microsystem

onDisable

public void onDisable()

Description copied from class: Microsystem

returns nothing if micro system is disabled

Overrides:

onDisable in class Microsystem

updateShuffleboard

public void updateShuffleboard()

Updates shuffleboard with the pivot's current position and velocity

Overrides:

updateShuffleboard in class Microsystem

motorToPivotRot

public double motorToPivotRot(double motorRot)

Converts motor rotations to rotations of the pivot mechanism

Parameters:

motorRot - position of the motor in rotations

Returns:

position of the pivot in rotations

pivotRotToMotor

public double pivotRotToMotor(double pivotRot)

Converts rotations of the pivot mechanism to motor rotations

Parameters:

pivotRot - position of the pivot in rotations

Returns:

position of the motor in rotations

configShuffleboard

public void configShuffleboard()

Configure shuffleboard for the pivot

Overrides:

configShuffleboard in class Microsystem

followLastProfile

public void followLastProfile()

Calculates the next state according to the trapezoidal profile and requests the pivot motor(s) to arrive at the next position with feedforward

getFFPositionRad

public double getFFPositionRad()

Gets the position offset for feedforward (to account for a shifted center of mass) in rotations

Returns:

offset position in rotations

setPivotPower

public void setPivotPower(double power)

Sets the power of the primary motor

Parameters:

power - desired power

getPivotPower

public double getPivotPower()

Gets the power of the primary motor

Returns:

the current power

setPivotVelocity

public void setPivotVelocity(double rps,
 boolean withFF)

Sets the velocity of the pivot in RPS

Parameters:

rps - Desired velocity in rotations per second

withFF - with feedforward

getPivotPosition

public double getPivotPosition()

Gets the current position of the pivot in rotations

Returns:

the position of the primary motor which is the same as the position of the pivot when they are fused

getPivotVelocity

public double getPivotVelocity()

Gets the current velocity of the pivot in rotations per second

Returns:

the velocity of the primary motor which is the same as the velocity of the pivot when they are fused

placePivotInClosestRot

public double placePivotInClosestRot(double scopeReferenceRot,
 double newAngleRot)

Returns the new pivot angle in the closest scope of the reference pivot angle

Parameters:

scopeReferenceRot - reference angle in rotations

newAngleRot - angle to be scoped in rotations

Returns:

scoped newAngleRot

setPivotProfile

public void setPivotProfile(Angle goalPosition)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPosition - goal position in rotations

setPivotProfile

public void setPivotProfile(Angle goalPosition,
 double goalVelocityRPS)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPosition - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

setPivotProfile

public void setPivotProfile(Angle goalPosition,
 double goalVelocityRPS,
 double maxVelocityRPS)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPosition - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

maxVelocityRPS - maximum velocity in rotations per second

setPivotProfile

public void setPivotProfile(Angle goalPosition,
 double goalVelocityRPS,
 double maxVelocityRPS,
 double maxAccelerationRPS2)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPosition - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

maxVelocityRPS - maximum velocity in rotations per second

maxAccelerationRPS2 - maximum acceleration in rotations per second squared

setPivotProfile

public void setPivotProfile(Angle goalPosition,
 double goalVelocityRPS,
 double maxVelocityRPS,
 double maxAccelerationRPS2,
 boolean setCurrentState)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPosition - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

maxVelocityRPS - maximum velocity in rotations per second

maxAccelerationRPS2 - maximum acceleration in rotations per second squared

setCurrentState - True if the profiles current state should base itself off sensor values rather than continue from the existing state

setPivotProfile

public void setPivotProfile(double goalPositionRot)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPositionRot - goal position in rotations

setPivotProfile

public void setPivotProfile(double goalPositionRot,
 double goalVelocityRPS)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPositionRot - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

setPivotProfile

public void setPivotProfile(double goalPositionRot,
 double goalVelocityRPS,
 double maxVelocityRPS)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPositionRot - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

maxVelocityRPS - maximum velocity in rotations per second

setPivotProfile

public void setPivotProfile(double goalPositionRot,
 double goalVelocityRPS,
 double maxVelocityRPS,
 double maxAccelerationRPS2)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPositionRot - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

maxVelocityRPS - maximum velocity in rotations per second

maxAccelerationRPS2 - maximum acceleration in rotations per second squared

setPivotProfile

public void setPivotProfile(double goalPositionRot,
 double goalVelocityRPS,
 double maxVelocityRPS,
 double maxAccelerationRPS2,
 boolean setCurrentState)

Creates a new trapezoidal profile for the pivot to follow

Parameters:

goalPositionRot - goal position in rotations

goalVelocityRPS - goal velocity in rotations per second

maxVelocityRPS - maximum velocity in rotations per second

maxAccelerationRPS2 - maximum acceleration in rotations per second squared

setCurrentState - True if the profiles current state should base itself off sensor values rather than continue from the existing state

clampPivotPosition

public double clampPivotPosition(double target)

Clamps the goal pivot position to be within rotational limits

Parameters:

target - goal position in rotations

Returns:

clamped goal position in rotations

motorResetConfig

public boolean motorResetConfig()

Resets devices if resets have occurred

Overrides:

motorResetConfig in class Microsystem

Returns:

boolean (micro system is enabled and the primary motor has been reset and if successful, it reconfigures the devices and returns true)

simulationInit

public void simulationInit()

Initializes the pivot simulation

Overrides:

simulationInit in class Microsystem

simulationPeriodic

public void simulationPeriodic()

Runs the pivot simulation--sets motor and cancoder simulation fields and updates the visual

Overrides:

simulationPeriodic in class Microsystem

isSafeToMove

public boolean isSafeToMove()

Checks whether or not it's safe for the pivot to move

Returns:

whether or not it's safe to move

absoluteClamp

public double absoluteClamp(double value)

Directs the pivot to its maximum position, current goal position, or minimum position

Parameters:

value - goal position

Returns:

redirected goal position

isPivotInTolerance

public boolean isPivotInTolerance(double lowerBound,
 double upperBound)

Bounds checking function that uses the current pivot position

Parameters:

lowerBound - minimum of acceptable range

upperBound - maximum of acceptable range

Returns:

if the current position is greater than or equal to the lower bound and less than or equal to the upper bound

isPivotInAbsoluteTolerance

public boolean isPivotInAbsoluteTolerance(double lowerBound,
 double upperBound)

Bounds checking function that uses the absolute current pivot position (modulo one rotation)

Parameters:

lowerBound - minimum of acceptable range

upperBound - maximum of acceptable range

Returns:

if the absolute current position is in absolute acceptable range

holdCurrentPosition

public void holdCurrentPosition()

Description copied from class: Microsystem

Sets the power of the primary motor to zero

Overrides:

holdCurrentPosition in class Microsystem

getPivotLigament

public MechanismLigament2d getPivotLigament()

Retrieves ligament attached to pivot Mechanism2d

Returns:

pivot ligament