Package frc.robot.subsystems.harvester

Class HarvesterSubsystem

java.lang.Object

edu.wpi.first.wpilibj2.command.SubsystemBase

frc.robot.shared.subsystems.AbstractSubsystem<TConfig>

frc.robot.shared.subsystems.AbstractMotorSubsystem<TConfig>

frc.robot.shared.subsystems.AbstractSetAndSeekSubsystem<HarvesterSubsystemConfig>

frc.robot.subsystems.harvester.HarvesterSubsystem

All Implemented Interfaces:

edu.wpi.first.util.sendable.Sendable, edu.wpi.first.wpilibj2.command.Subsystem

public class HarvesterSubsystem
extends AbstractSetAndSeekSubsystem<HarvesterSubsystemConfig>

Harvester subsystem that swings the intake arm between a stowed (upright) position and a deployed (lowered) position using a profiled motor. The superclass handles motion profiling, limits, and logging while this class adds gravity-compensating arm feedforward and exposes convenience methods for the two named positions.

A trapezoidal motion profile is a control technique that limits both the velocity and acceleration of the motor, producing smooth, predictable movements instead of abrupt starts and stops.

Because the arm swings against gravity, this subsystem replaces the parent's simple feedforward with an ArmFeedforward. The arm feedforward adds a gravity term (kG) that is multiplied by the cosine of the arm's angle from horizontal, automatically varying the compensation voltage as the arm moves.

Field Summary

Fields inherited from class frc.robot.shared.subsystems.AbstractSetAndSeekSubsystem

constraints, controller, goalState, setpointState, targetWasClamped

Fields inherited from class frc.robot.shared.subsystems.AbstractMotorSubsystem

feedforward, motor, motorInputs

Fields inherited from class frc.robot.shared.subsystems.AbstractSubsystem

className, config, enabled, kDt, log, verbose

Constructor Summary

Constructors

Constructor	Description
HarvesterSubsystem(HarvesterSubsystemConfig config)	Builds the harvester subsystem with a single profiled motor for arm positioning.

Method Summary

Modifier and Type	Method	Description
protected double	calculateFeedforward(double previousSetpointVelocity, edu.wpi.first.math.trajectory.TrapezoidProfile.State currentSetpoint)	Computes the arm feedforward voltage including gravity compensation for the current profile step.
void	deployArm()	Commands the arm to deploy to the lowered Fuel-collection position.
boolean	isInDeployedMode()	Reports whether the arm's current goal is the deployed position.
protected void	refreshFeedforward()	Re-reads arm feedforward gains from config so live tuning updates affect voltage estimates immediately.
void	stowArm()	Commands the arm to stow in the upright match-start position.
void	sweepArm()	Commands the arm to the sweep position to push Fuel from the back of the hopper toward the shooting array.

Methods inherited from class frc.robot.shared.subsystems.AbstractSetAndSeekSubsystem

applySetpoint, getMeasuredPosition, getMeasuredPositionDegrees, getMeasuredVelocity, handleSeekInterrupted, isProfileSettled, periodic, resetEncoderPosition, seekTarget, setTarget, settleAtCurrentPosition, stop

Methods inherited from class frc.robot.shared.subsystems.AbstractMotorSubsystem

applyVoltage, getMeasuredPositionRadians, getMeasuredVelocityRadiansPerSecond, getSysIdRoutine, logSysIdEnd, logSysIdInterrupted, logSysIdStart, updateAndLogMotorInputs

Methods inherited from class frc.robot.shared.subsystems.AbstractSubsystem

getConfig, isFMSAttached, isSimulation, isSubsystemDisabled, isVerbose, isVerboseLoggingEnabled, logDisabled, reportWarning

Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase

addChild, getName, getSubsystem, initSendable, setName, setSubsystem

Methods inherited from class java.lang.Object

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

Methods inherited from interface edu.wpi.first.wpilibj2.command.Subsystem

defer, getCurrentCommand, getDefaultCommand, idle, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun

Constructor Details

HarvesterSubsystem

public HarvesterSubsystem(HarvesterSubsystemConfig config)

Builds the harvester subsystem with a single profiled motor for arm positioning.

Parameters:

config - harvester configuration bundle loaded from JSON; angles are expressed in degrees

Method Details

deployArm

public void deployArm()

Commands the arm to deploy to the lowered Fuel-collection position.

The motion profile ramps the arm smoothly from its current position to the configured deployed angle. The arm will stay at the target until a new target is set or the command is interrupted.

stowArm

public void stowArm()

Commands the arm to stow in the upright match-start position.

The motion profile ramps the arm smoothly from its current position to the configured stowed angle. This is the default position at the start of a match and keeps the intake inside the robot perimeter.

sweepArm

public void sweepArm()

Commands the arm to the sweep position to push Fuel from the back of the hopper toward the shooting array.

The motion profile ramps the arm smoothly from its current position to the configured sweep angle. This is used during extended firing sequences when the front of the hopper is empty and remaining Fuel needs to be pushed forward.

isInDeployedMode

public boolean isInDeployedMode()

Reports whether the arm's current goal is the deployed position. The hold command uses this to decide whether to monitor for drift or idle passively.

Returns:

true when the profile goal is within one degree of the configured deployed angle

calculateFeedforward

protected double calculateFeedforward(double previousSetpointVelocity,
 edu.wpi.first.math.trajectory.TrapezoidProfile.State currentSetpoint)

Computes the arm feedforward voltage including gravity compensation for the current profile step.

The arm angle from horizontal is computed by adding the configured horizontal offset to the current setpoint position. The gravity term (kG × cos(angle)) varies automatically as the arm moves, producing more voltage when the arm is near horizontal and less when it is vertical.

Overrides:

calculateFeedforward in class AbstractSetAndSeekSubsystem<HarvesterSubsystemConfig>

Parameters:

previousSetpointVelocity - velocity setpoint from the previous cycle in radians per second

currentSetpoint - the setpoint state the profile wants us to follow this cycle

Returns:

feedforward voltage in volts, including gravity compensation

refreshFeedforward

protected void refreshFeedforward()

Re-reads arm feedforward gains from config so live tuning updates affect voltage estimates immediately.

Overrides:

refreshFeedforward in class AbstractMotorSubsystem<HarvesterSubsystemConfig>