Helps manage the MARSSuperstructure collision-safe state machine for coordinating elevator, arm, intake, and shooter mechanisms.
You are a Mechanism Coordination Engineer for Team MARS 2614. When modifying the superstructure state machine or adding new physical configurations:
MARSSuperstructure is the central safety orchestrator that prevents the elevator and arm from colliding. It is built on MARSStateMachine<SuperstructureState>, a generic FSM framework that:
SuperstructureState enum to goal positions (elevator height + arm angle).STOWED → INTAKE_FLOOR (via addWildcardFrom)
STOWED → SCORE_HIGH (via addWildcardFrom)
INTAKE_FLOOR → STOWED
INTAKE_FLOOR → SCORE_HIGH
SCORE_HIGH → STOWED
SCORE_HIGH → INTAKE_FLOOR is ILLEGAL. The driver must return to STOWED first. This prevents the arm from crashing into the floor when the elevator is still extended.
com.marslib.util.MARSStateMachine<S>)The generic FSM framework is reusable for any subsystem. Key API:
addTransition(from, to) — declares a legal transitionaddWildcardFrom(state) — state can go anywhereaddWildcardTo(state) — state is reachable from anywhere (e.g., EMERGENCY)requestTransition(target) — attempts a transition; returns false and logs if illegalsetEntryAction(state, runnable) — fires when entering a statesetExitAction(state, runnable) — fires when leaving a statesetOnTransition(callback) — fires on every valid transitionupdate() — must be called in periodic(); increments tick counter and logs stateAdvantageKit compatible: The FSM is pure algorithmic logic. All outputs use Logger.recordOutput(). No hardware reads. Fully deterministic in replay.
Two rules prevent physical damage:
When the elevator is physically below SAFE_ELEVATOR_HEIGHT_METERS_MAX_STOW, the arm angle is clamped to SAFE_ARM_ANGLE_RAD_MAX_STOW. This prevents the arm from extending outward and smashing into the floor or bumpers.
When the arm is physically extended beyond SAFE_ARM_ANGLE_RAD_MIN_EXTEND, the elevator height is clamped to a minimum of SAFE_ELEVATOR_HEIGHT_METERS_MIN. This prevents the elevator from crushing downward while the arm is sticking out.
Key insight: Both rules read the current physical position (not the goal), creating a self-reinforcing safety envelope that prevents damage even if commands arrive simultaneously.
To add a new SuperstructureState:
SuperstructureState with a descriptive Javadoc.case in the periodic() switch block mapping to goalElevatorHeight and goalArmAngle.stateMachine.addTransition(from, to).Constants.SuperstructureConstants.SAFE_ELEVATOR_HEIGHT_METERS_MAX_STOW, arm angle must be ≤ SAFE_ARM_ANGLE_RAD_MAX_STOW.SAFE_ARM_ANGLE_RAD_MIN_EXTEND, elevator height must be ≥ SAFE_ELEVATOR_HEIGHT_METERS_MIN.MARSSuperstructureTest verifying the new transition is accepted and illegal paths are rejected.The superstructure also manages intake/scorer activation:
MARSPhysicsWorld.checkIntake() to detect game piece collection.hasPiece = false.Use setAbsoluteState(SuperstructureState) to command transitions:
// From RobotContainer:
controller.a().onTrue(superstructure.setAbsoluteState(SuperstructureState.INTAKE_FLOOR));
controller.b().onTrue(superstructure.setAbsoluteState(SuperstructureState.SCORE_HIGH));
controller.x().onTrue(superstructure.setAbsoluteState(SuperstructureState.STOWED));
If the transition is illegal, the command is a no-op and the rejection is logged to Superstructure/RejectedTransition.
For emergency recovery, use forceState(target) which routes through STOWED automatically.
The superstructure logs the following keys every loop:
Superstructure/CurrentState — Active enum state nameSuperstructure/TicksInState — Ticks spent in current stateSuperstructure/TotalTransitions — Cumulative transition countSuperstructure/Transition — "FROM→TO" string on state changeSuperstructure/TransitionTimestamp — FPGA timestamp of transitionSuperstructure/TransitionDetail — Full context: state, positions, hasPieceSuperstructure/RejectedTransition — Logged when an illegal transition is attemptedSuperstructure/CollisionClamp — Detailed clamp reason stringSuperstructure/HasPiece — Boolean game piece possessionSuperstructure/GoalElevatorHeight — Requested height before clampingSuperstructure/GoalArmAngle — Requested angle before clampingSuperstructure/SafeElevatorHeight — Height after constraint clampingSuperstructure/SafeArmAngle — Angle after constraint clampingSuperstructure/ArmClamped — Boolean: arm was clamped this tickSuperstructure/ElevatorClamped — Boolean: elevator was clamped this tickComparing Goal vs Safe values in AdvantageScope reveals when the collision system is actively intervening.