Package frc.robot.subsystems

Class MARSSuperstructure

java.lang.Object

edu.wpi.first.wpilibj2.command.SubsystemBase

frc.robot.subsystems.MARSSuperstructure

All Implemented Interfaces:

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

public class MARSSuperstructure
extends edu.wpi.first.wpilibj2.command.SubsystemBase

Superstructure orchestrator using a validated finite state machine.

Coordinates the cowl, intake pivot, floor intake, shooter, and feeder subsystems through safe state transitions. Automatically enters BEACHED mode when the robot tilt exceeds 25°, disabling all mechanisms to prevent damage during field obstacle traversal.

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
static enum	MARSSuperstructure.SuperstructureState	The set of valid superstructure states.

Constructor Summary

Constructors

Constructor	Description
MARSSuperstructure(MARSCowl cowl, MARSIntakePivot intakePivot, MARSShooter floorIntake, MARSShooter shooter, MARSShooter feeder, Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Supplier<Optional<edu.wpi.first.math.geometry.Translation2d>> visionTargetSupplier, Supplier<Double> tiltRadiansSupplier)	Constructs the superstructure orchestrator.

Method Summary

Modifier and Type	Method	Description
void	forceState(MARSSuperstructure.SuperstructureState targetState)	Forces the superstructure into the specified state by routing through STOWED first.
MARSSuperstructure.SuperstructureState	getCurrentState()	Returns the current superstructure state.
MARSStateMachine<MARSSuperstructure.SuperstructureState>	getStateMachine()	Returns the underlying state machine for testing and diagnostics.
void	periodic()
edu.wpi.first.wpilibj2.command.Command	setAbsoluteState(MARSSuperstructure.SuperstructureState targetState)	Creates a command that requests a transition to the given superstructure state.

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

MARSSuperstructure

public MARSSuperstructure(MARSCowl cowl,
 MARSIntakePivot intakePivot,
 MARSShooter floorIntake,
 MARSShooter shooter,
 MARSShooter feeder,
 Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier,
 Supplier<Optional<edu.wpi.first.math.geometry.Translation2d>> visionTargetSupplier,
 Supplier<Double> tiltRadiansSupplier)

Constructs the superstructure orchestrator.

Parameters:

cowl - The cowl (hood) rotary mechanism.

intakePivot - The intake pivot rotary mechanism.

floorIntake - The floor intake flywheel (as MARSShooter).

shooter - The main shooter flywheel.

feeder - The feeder flywheel.

poseSupplier - Supplier for the current robot field pose.

visionTargetSupplier - Supplier for the latest vision target translation.

tiltRadiansSupplier - Supplier for the current robot tilt in radians.

Method Details

setAbsoluteState

public edu.wpi.first.wpilibj2.command.Command setAbsoluteState(MARSSuperstructure.SuperstructureState targetState)

Creates a command that requests a transition to the given superstructure state. If the transition is illegal, it is rejected and logged — no mechanism movement occurs.

Parameters:

targetState - The desired superstructure state.

Returns:

A command that executes the transition request.

forceState

public void forceState(MARSSuperstructure.SuperstructureState targetState)

Forces the superstructure into the specified state by routing through STOWED first. This bypasses normal transition validation and should only be used for safety overrides (e.g., automatic beaching on tilt detection).

Parameters:

targetState - The state to force.

periodic

public void periodic()

getCurrentState

public MARSSuperstructure.SuperstructureState getCurrentState()

Returns the current superstructure state.

Returns:

The active MARSSuperstructure.SuperstructureState.

getStateMachine

public MARSStateMachine<MARSSuperstructure.SuperstructureState> getStateMachine()

Returns the underlying state machine for testing and diagnostics.

Returns:

The MARSStateMachine instance.