Package frc.alotobots.reefscape.subsystems.wrist

Class WristSubsystem

java.lang.Object

edu.wpi.first.wpilibj2.command.SubsystemBase

frc.alotobots.reefscape.subsystems.wrist.WristSubsystem

All Implemented Interfaces:

Sendable, Subsystem

public class WristSubsystem
extends SubsystemBase

Subsystem for controlling the robot's wrist mechanism. Handles both position and velocity control of the wrist joint with dynamic limits based on elevator height zones.

Constructor Summary

Constructors

Constructor	Description
WristSubsystem(WristIO io)	Creates a new WristSubsystem.

Method Summary

Modifier and Type	Method	Description
Angle	getCurrentAngle()	Retrieves the current angle of the wrist.
boolean	isAtTargetAngle()	Checks if the wrist is stably at its target angle for a minimum duration.
void	periodic()
void	runAtPercentOutput(double percentOutput)	Runs the wrist using direct percent output (open-loop control).
void	runToTargetAngle(Angle angle)	Commands the wrist to move to a target angle using closed-loop control.
void	runToTargetVelocity(AngularVelocity velocity)	Controls the wrist to move to a specified velocity using closed-loop velocity control.
void	stop()	Stops all wrist movement.

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, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun

Constructor Details

WristSubsystem

public WristSubsystem(WristIO io)

Creates a new WristSubsystem.

Parameters:

io - The hardware abstraction interface for the wrist

elevatorHeightSupplier - Supplier function that provides the current elevator height

Method Details

periodic

public void periodic()

runToTargetAngle

public void runToTargetAngle(Angle angle)

Commands the wrist to move to a target angle using closed-loop control. Target angle is dynamically clamped based on current elevator height zone.

Parameters:

angle - The target angle for the wrist

runToTargetVelocity

public void runToTargetVelocity(AngularVelocity velocity)

Controls the wrist to move to a specified velocity using closed-loop velocity control. Dynamic limits based on current elevator height are passed to the IO layer.

Parameters:

velocity - Target velocity in degrees per second

runAtPercentOutput

public void runAtPercentOutput(double percentOutput)

Runs the wrist using direct percent output (open-loop control). Dynamic limits based on current elevator height are passed to the IO layer.

Parameters:

percentOutput - The motor output as a percentage (-1.0 to 1.0)

stop

public void stop()

Stops all wrist movement.

getCurrentAngle

public Angle getCurrentAngle()

Retrieves the current angle of the wrist.

Returns:

The current angle as an Angle object

isAtTargetAngle

public boolean isAtTargetAngle()

Checks if the wrist is stably at its target angle for a minimum duration.

Returns:

true if the wrist has maintained its target angle within tolerance