Package tagalong.subsystems.micro
Class Microsystem
java.lang.Object
tagalong.subsystems.micro.Microsystem
Base class for all Tagalong Microsystems
-
Field Summary
FieldsModifier and TypeFieldDescriptionprotected com.ctre.phoenix6.hardware.TalonFX[]Array of all microsystem motors, the first in the arrayfinal MicrosystemConfConfiguration common to all microsystemsfinal booleanTrue if the microsystem is configured to be disabled by a null in the conf filesprotected GenericPublisherLogging entries that write robot status values to shuffleboardprotected GenericPublisherLogging entries that write robot status values to shuffleboardprotected TrapezoidProfile.StateTrapezoidal profile follower helper states that track current and goal stateprotected booleanTrue if the microsystem should follow the a profile during each periodic loopprotected TrapezoidProfile.StateTrapezoidal profile follower helper states that track current and goal stateprotected booleanTrue if the microsystem is configured for shuffleboard based FeedForward tuningprotected booleanDisablement state, for dynamic disablement caused by hardware disconnects or on the fly disablement by robot codeprotected booleanTrue if the microsystem is configured for shuffleboard based PID tuningprotected booleanTrue if the microsystem is configured for shuffleboard based logging, also true if in PID or FF tuning modesprotected GenericSubscriberFeedForward tuning entries that read FeedForward constants from shuffleboardprotected GenericSubscriberFeedForward tuning entries that read FeedForward constants from shuffleboardprotected GenericSubscriberFeedForward tuning entries that read FeedForward constants from shuffleboardprotected GenericSubscriberFeedForward tuning entries that read FeedForward constants from shuffleboardprotected Mechanism2dVisual representation of the elevatorprotected com.ctre.phoenix6.hardware.TalonFXThe primary motor for the microsystem, all others will follow this motorprotected com.ctre.phoenix6.sim.TalonFXSimStateSimulation for the primary motorprotected TimerTimer used for trapezoidal state timing and trackingprotected com.ctre.phoenix6.controls.PositionVoltageShared motor positional voltage request, we put positional PIDSGVA constants into slot 0protected com.ctre.phoenix6.controls.TorqueCurrentFOCShared motor torque current requestprotected com.ctre.phoenix6.controls.VelocityVoltageShared motor velocity voltage request, we put velocity PIDSGVA constants into slot 1protected MechanismRoot2dThe root of the simulation animationprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericSubscriberPID tuning entries that read PID from shuffleboardprotected GenericPublisherLogging entries that write robot status values to shuffleboardprotected GenericPublisherLogging entries that write robot status values to shuffleboardprotected TimerTimer used for tracking how long a system is in toleranceprotected TrapezoidProfileTrapezoidal profile currently being followedstatic intMicrosystem variable for tiling the shuffleboard entries -
Constructor Summary
ConstructorsConstructorDescriptionMicrosystem(MicrosystemConf conf) Constructs a microsystem with the below configurations -
Method Summary
Modifier and TypeMethodDescriptionbooleanchecks to see if micro system is enabled and primary motor is alivebooleancheckToleranceTime(boolean inTolerance, double requiredDurationS) Ran each time the isFinished command is run when in tolerance duration is required.voidConfigures devicesvoidConfigures a user interface on the Shuffleboard for the specified micro systemvoidconfigures the tuning states of the micro system based on whether it is selected for tuning, and whether it is in PID or feedforward tuning modevoiddisableMicrosystem(boolean disable) Sets microsystem disablementcom.ctre.phoenix6.hardware.TalonFXdoubledoubledoublegetRoot()voidSets the power of the primary motor to zerobooleanChecks if the trapezoidal profile has reached its goalbooleanvoidreturns nothing if micro system is disabledvoidonEnable()Sets break mode for all motors If system is in PID tuning mode is updates all PID related settings If the system is in Feedforward Tuning mode, it sets the primary motor to coast mode.voidreturns nothing if micro system is disabled else it sets followProfile to false which stops any movementvoidperiodic()Periodic update functionvoidResets the tolerance timer, must be ran as part of command initializationvoidsetBrakeMode(boolean enabled) Sets all motors on enable brake modevoidsetFollowProfile(boolean followProfile) control whether the robot or subsystem should follow a predefined motion pathvoidsetHoldPosition(boolean holdPosition) alias for setFollowProfilevoidsetPrimaryPower(double power) exits method if micro system is disabled, if enabled sets primary motor to specified valuevoidInitializes simulationvoidPeriodic function during simulationvoidvoidUpdates shuffleboardprotected voidChecks initialize status
-
Field Details
-
_conf
Configuration common to all microsystems -
_configuredMicrosystemDisable
True if the microsystem is configured to be disabled by a null in the conf files -
_isMicrosystemDisabled
Disablement state, for dynamic disablement caused by hardware disconnects or on the fly disablement by robot code -
_tuningTabCounter
Microsystem variable for tiling the shuffleboard entries -
_isShuffleboardMicro
True if the microsystem is configured for shuffleboard based logging, also true if in PID or FF tuning modes -
_isPIDTuningMicro
True if the microsystem is configured for shuffleboard based PID tuning -
_isFFTuningMicro
True if the microsystem is configured for shuffleboard based FeedForward tuning -
_slot0PFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot0IFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot0DFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot1PFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot1IFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot1DFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot2PFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot2IFactorEntry
PID tuning entries that read PID from shuffleboard -
_slot2DFactorEntry
PID tuning entries that read PID from shuffleboard -
_KSEntry
FeedForward tuning entries that read FeedForward constants from shuffleboard -
_KGEntry
FeedForward tuning entries that read FeedForward constants from shuffleboard -
_KVEntry
FeedForward tuning entries that read FeedForward constants from shuffleboard -
_KAEntry
FeedForward tuning entries that read FeedForward constants from shuffleboard -
_targetPositionEntry
Logging entries that write robot status values to shuffleboard -
_targetVelocityEntry
Logging entries that write robot status values to shuffleboard -
_currentPositionEntry
Logging entries that write robot status values to shuffleboard -
_currentVelocityEntry
Logging entries that write robot status values to shuffleboard -
_primaryMotor
The primary motor for the microsystem, all others will follow this motor -
_allMotors
Array of all microsystem motors, the first in the array -
_requestedPositionVoltage
Shared motor positional voltage request, we put positional PIDSGVA constants into slot 0 -
_requestedVelocityVoltage
Shared motor velocity voltage request, we put velocity PIDSGVA constants into slot 1 -
_requestedTorqueCurrent
Shared motor torque current request -
_followProfile
True if the microsystem should follow the a profile during each periodic loop -
_curState
Trapezoidal profile follower helper states that track current and goal state -
_goalState
Trapezoidal profile follower helper states that track current and goal state -
_trapProfile
Trapezoidal profile currently being followed -
_profileTimer
Timer used for trapezoidal state timing and tracking -
_toleranceTimer
Timer used for tracking how long a system is in tolerance -
_primaryMotorSim
Simulation for the primary motor -
_root
The root of the simulation animation -
_mechanism
Visual representation of the elevator
-
-
Constructor Details
-
Microsystem
Constructs a microsystem with the below configurations- Parameters:
conf- Configuration for the microsystem
-
-
Method Details
-
configAllDevices
Configures devices -
setBrakeMode
Sets all motors on enable brake mode- Parameters:
enabled- whether or not the robot is enabled
-
updateAllPIDSGVA
-
onEnable
Sets break mode for all motors If system is in PID tuning mode is updates all PID related settings If the system is in Feedforward Tuning mode, it sets the primary motor to coast mode. -
waitForInitialization
Checks initialize status -
checkInitStatus
checks to see if micro system is enabled and primary motor is alive- Returns:
- boolean
-
getPrimaryMotor
- Returns:
- _primaryMotor(if system is enabled)
-
getPrimaryMotorPosition
- Returns:
- position of the primary motor, 0.0 if system is disabled
-
getPrimaryMotorVelocity
- Returns:
- velocity of the primary motor, 0.0 if system is disabled
-
setPrimaryPower
exits method if micro system is disabled, if enabled sets primary motor to specified value- Parameters:
power- desired power
-
getPrimaryMotorPower
- Returns:
- double(0.0 if micro system is disabled or actual power of primary motor)
-
configTuningModes
configures the tuning states of the micro system based on whether it is selected for tuning, and whether it is in PID or feedforward tuning mode -
configShuffleboard
Configures a user interface on the Shuffleboard for the specified micro system -
onDisable
returns nothing if micro system is disabled -
onTeleopDisable
returns nothing if micro system is disabled else it sets followProfile to false which stops any movement -
setFollowProfile
control whether the robot or subsystem should follow a predefined motion path- Parameters:
followProfile- whether or not to follow the trapezoidal profile
-
setHoldPosition
alias for setFollowProfile- Parameters:
holdPosition- whether or not to hold position
-
motorResetConfig
- Returns:
- boolean (micro system is enabled and the primary motor has been reset and if successful, it reconfigures the devices and returns true)
-
isProfileFinished
Checks if the trapezoidal profile has reached its goal- Returns:
- whether or not the profile has finished
-
disableMicrosystem
Sets microsystem disablement- Parameters:
disable- whether or not the microsystem should be disabled
-
holdCurrentPosition
Sets the power of the primary motor to zero -
updateShuffleboard
Updates shuffleboard -
periodic
Periodic update function -
simulationInit
Initializes simulation -
simulationPeriodic
Periodic function during simulation -
resetToleranceTimer
Resets the tolerance timer, must be ran as part of command initialization -
checkToleranceTime
Ran each time the isFinished command is run when in tolerance duration is required. Resets or starts the timer accordingly and evaluates if required duration has been met- Parameters:
inTolerance- True if the microsystem is currently in tolerancerequiredDurationS- Require in tolerance duration in seconds- Returns:
- True if the microsystem has been in tolerance the required duration
-
getRoot
- Returns:
- sim root
-