Package frc.robot.subsystems.elevator_wrist

Class ElevatorWrist

java.lang.Object

edu.wpi.first.wpilibj2.command.SubsystemBase

frc.robot.subsystems.elevator_wrist.ElevatorWrist

All Implemented Interfaces:

Sendable, Subsystem

public class ElevatorWrist
extends SubsystemBase

Elevator and Wrist Subsystem

Field Summary

Fields

Modifier and Type	Field	Description
Trigger	elevatorAtAmp
frc.robot.subsystems.elevator_wrist.ElevatorWristInputsAutoLogged	inputs
ElevatorWristIO	io

Constructor Summary

Constructors

Constructor	Description
ElevatorWrist(ElevatorWristIO io, CommandXboxController operator, PumbaaViz viz)	Create new ElevatorWrist.

Method Summary

Modifier and Type	Method	Description
Command	ampPosition()	Set elevator and wrist to amp position.
Boolean	atGoal()	Get if at elevator + wrist PID goal
Command	climbPosition()	Set elevator and wrist to climbing position.
boolean	elevatorAtAmp()	Check if the elevator is at the AMP position
boolean	elevatorAtHome()	Check if the elevator is at the home position
double	elevatorDistanceTraveled()	Get the height in meters of the elevator based on the rotations of the motor
Command	followPosition(DoubleSupplier height, Supplier<Rotation2d> angle)	Command to continuously move the Elevator and Wrist to an ever changing position
Rotation2d	getAngleFromDistance(Pose2d position)	Determine the angle of wrist based on the distance from the shooter
double	getHeight()	Calculate elevator height from raw encoder value.
Rotation2d	getWristAngle()
Rotation2d	getWristAngleMeasurement()	Calculate wrist angle from raw encoder value.
Command	goToPosition(double height, Rotation2d angle)	Command to move Elevator and Wrist to set positions
Command	homePosition()	Set elevator to home position which can fit under the stage.
void	periodic()
void	setWristAngle(Rotation2d angle)	Set the Wrist to a specified angle
Command	speakerPreset()	Set elevator to home and angle the wrist at a set angle to shoot into the speaker from directly in front.

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

Field Details

io

public ElevatorWristIO io

inputs

public frc.robot.subsystems.elevator_wrist.ElevatorWristInputsAutoLogged inputs

elevatorAtAmp

public Trigger elevatorAtAmp

Constructor Details

ElevatorWrist

public ElevatorWrist(ElevatorWristIO io,
 CommandXboxController operator,
 PumbaaViz viz)

Create new ElevatorWrist.

Method Details

periodic

public void periodic()

getHeight

public double getHeight()

Calculate elevator height from raw encoder value.

getWristAngleMeasurement

public Rotation2d getWristAngleMeasurement()

Calculate wrist angle from raw encoder value.

getWristAngle

public Rotation2d getWristAngle()

getAngleFromDistance

public Rotation2d getAngleFromDistance(Pose2d position)

Determine the angle of wrist based on the distance from the shooter

Parameters:

position - Position of the robot

Returns:

Rotation of the wrist

setWristAngle

public void setWristAngle(Rotation2d angle)

Set the Wrist to a specified angle

Parameters:

angle - Angle of the wrist

speakerPreset

public Command speakerPreset()

Set elevator to home and angle the wrist at a set angle to shoot into the speaker from directly in front.

ampPosition

public Command ampPosition()

Set elevator and wrist to amp position. Performs two steps to avoid colliding with electronics box.

climbPosition

public Command climbPosition()

Set elevator and wrist to climbing position. Performs two steps to avoid colliding with electronics box.

homePosition

public Command homePosition()

Set elevator to home position which can fit under the stage. Performs two steps to avoid colliding with electronics box.

goToPosition

public Command goToPosition(double height,
 Rotation2d angle)

Command to move Elevator and Wrist to set positions

Parameters:

height - The height of the elevator in meters

angle - The angle of the wrist in Rotation2d

Returns:

A Command

followPosition

public Command followPosition(DoubleSupplier height,
 Supplier<Rotation2d> angle)

Command to continuously move the Elevator and Wrist to an ever changing position

Parameters:

height - A DoubleSupplier to provide the height of the elevator in meters

angle - A Supplier of Rotation2d of angle of the wrist in Rotation2d

Returns:

A Command

elevatorDistanceTraveled

public double elevatorDistanceTraveled()

Get the height in meters of the elevator based on the rotations of the motor

Returns:

Height of elevator in meters

atGoal

public Boolean atGoal()

Get if at elevator + wrist PID goal

Returns:

boolean representing if the elevator and wrist PID controllers are at their goals

elevatorAtHome

public boolean elevatorAtHome()

Check if the elevator is at the home position

Returns:

True if the elevator is home

elevatorAtAmp

public boolean elevatorAtAmp()

Check if the elevator is at the AMP position

Returns:

True if the elevator is AMP