Skip to content
This repository has been archived by the owner on Apr 20, 2024. It is now read-only.

Commit

Permalink
Refactor robot state homed & has note
Browse files Browse the repository at this point in the history
  • Loading branch information
@jordanjcoderman
jordanjcoderman committed Feb 17, 2024
1 parent f51f125 commit d91fdd9
Show file tree
Hide file tree
Showing 3 changed files with 69 additions and 93 deletions.
11 changes: 6 additions & 5 deletions src/main/java/frc/robot/elevator/ElevatorSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.StaticBrake;
import com.ctre.phoenix6.hardware.TalonFX;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
Expand All @@ -24,8 +23,7 @@ public class ElevatorSubsystem extends LifecycleSubsystem {

private final StaticBrake brakeNeutralRequest = new StaticBrake();
private final CoastOut coastNeutralRequest = new CoastOut();
private final PositionVoltage positionRequest =
new PositionVoltage(ElevatorPositions.STOWED);
private final PositionVoltage positionRequest = new PositionVoltage(ElevatorPositions.STOWED);
private final LoggedDashboardNumber ntDistance =
new LoggedDashboardNumber("Elevator/DistanceOverride", -1);

Expand Down Expand Up @@ -90,7 +88,9 @@ public void robotPeriodic() {
Logger.recordOutput("Elevator/UsedGoalPosition", usedGoalPosition);

motor.setControl(
positionRequest.withSlot(slot).withPosition(inchesToRotations(usedGoalPosition).getRotations()));
positionRequest
.withSlot(slot)
.withPosition(inchesToRotations(usedGoalPosition).getRotations()));

break;
case MID_MATCH_HOMING:
Expand Down Expand Up @@ -132,6 +132,7 @@ private Rotation2d inchesToRotations(double inches) {
}

private static double clampHeight(double height) {
return MathUtil.clamp(height, RobotConfig.get().elevator().minHeight(), RobotConfig.get().elevator().maxHeight());
return MathUtil.clamp(
height, RobotConfig.get().elevator().minHeight(), RobotConfig.get().elevator().maxHeight());
}
}
81 changes: 22 additions & 59 deletions src/main/java/frc/robot/robot_manager/RobotManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,42 +30,9 @@
import frc.robot.vision.VisionSubsystem;
import frc.robot.wrist.WristPositions;
import frc.robot.wrist.WristSubsystem;
import java.util.EnumSet;
import java.util.Set;
import org.littletonrobotics.junction.Logger;

public class RobotManager extends LifecycleSubsystem {
private static final Set<RobotState> HAS_GP_STATES =
EnumSet.of(
RobotState.WAITING_FLOOR_SHOT,
RobotState.PREPARE_FLOOR_SHOT,
RobotState.FLOOR_SHOOT,
RobotState.WAITING_SUBWOOFER_SHOT,
RobotState.PREPARE_SUBWOOFER_SHOT,
RobotState.SUBWOOFER_SHOOT,
RobotState.TRAP_OUTTAKE,
RobotState.WAITING_SPEAKER_SHOT,
RobotState.PREPARE_SPEAKER_SHOT,
RobotState.SPEAKER_SHOOT,
RobotState.WAITING_AMP_SHOT,
RobotState.QUEUER_TO_INTAKE_FOR_AMP,
RobotState.CONVEYOR_TO_INTAKE_FOR_SHOOTER,
RobotState.INTAKE_TO_CONVEYOR_FOR_AMP,
RobotState.INTAKE_TO_QUEUER_FOR_SHOOTER,
RobotState.AMP_SHOT,
RobotState.IDLE_WITH_GP,
RobotState.WAITING_CLIMBER_RAISED,
RobotState.PREPARE_CLIMBER_RAISED,
RobotState.CLIMBER_RAISED,
RobotState.PREPARE_CLIMBER_HANGING,
RobotState.CLIMBER_HANGING);
private static final Set<RobotState> HOMED_STATES = EnumSet.allOf(RobotState.class);

{
HOMED_STATES.remove(RobotState.HOMING);
HOMED_STATES.remove(RobotState.UNHOMED);
}

public final WristSubsystem wrist;
public final IntakeSubsystem intake;
public final ElevatorSubsystem elevator;
Expand Down Expand Up @@ -176,35 +143,35 @@ public void robotPeriodic() {
}
if (groundIntakeFlag) {
intake.setState(IntakeState.IDLE);
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.GROUND_INTAKING;
}
}
if (stowFlag) {
if (HOMED_STATES.contains(state)) {
if (HAS_GP_STATES.contains(state)) {
if (state.homed) {
if (state.hasNote) {
state = RobotState.IDLE_WITH_GP;
} else {
state = RobotState.IDLE_NO_GP;
}
}
}
if (stowAfterIntakeFlag) {
if (HOMED_STATES.contains(state) && state != RobotState.GROUND_INTAKING) {
if (HAS_GP_STATES.contains(state)) {
if (state.homed && state != RobotState.GROUND_INTAKING) {
if (state.hasNote) {
state = RobotState.IDLE_WITH_GP;
} else {
state = RobotState.IDLE_NO_GP;
}
}
}
if (waitingClimberRaisedFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.WAITING_CLIMBER_RAISED;
}
}
if (raiseClimberFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.PREPARE_CLIMBER_RAISED;
}
}
Expand All @@ -214,67 +181,67 @@ public void robotPeriodic() {
}
}
if (waitSpeakerShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.WAITING_SPEAKER_SHOT;
}
}
if (waitSubwooferShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.WAITING_SUBWOOFER_SHOT;
}
}
if (outtakeIntakeFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.OUTTAKING_INTAKE;
}
}
if (outtakeShooterFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.OUTTAKING_SHOOTER;
}
}
if (speakerShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.PREPARE_SPEAKER_SHOT;
}
}
if (ampShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.AMP_SHOT;
}
}
if (passNoteToQueuerFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.CONVEYOR_TO_INTAKE_FOR_SHOOTER;
}
}
if (passNoteToConveyorFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.QUEUER_TO_INTAKE_FOR_AMP;
}
}
if (trapShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.TRAP_OUTTAKE;
}
}
if (subwooferShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.PREPARE_SUBWOOFER_SHOT;
}
}
if (preloadNoteRequest) {
if (HOMED_STATES.contains(state) && !HAS_GP_STATES.contains(state)) {
if (state.homed && !state.hasNote) {
state = RobotState.IDLE_WITH_GP;
}
}
if (waitFloorShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.WAITING_FLOOR_SHOT;
}
}
if (floorShotFlag) {
if (HOMED_STATES.contains(state)) {
if (state.homed) {
state = RobotState.PREPARE_FLOOR_SHOT;
}
}
Expand Down Expand Up @@ -350,7 +317,7 @@ public void robotPeriodic() {
case FLOOR_SHOOT:
case SUBWOOFER_SHOOT:
case SPEAKER_SHOOT:
if (queuer.atGoal(QueuerState.PASS_TO_SHOOTER) && !getCollectiveHasNote()) {
if (queuer.atGoal(QueuerState.PASS_TO_SHOOTER) && !state.hasNote) {
state = RobotState.IDLE_NO_GP;
}
break;
Expand Down Expand Up @@ -419,7 +386,7 @@ public void robotPeriodic() {
case HOMING:
wrist.startMidMatchHoming();
climber.startHoming();
elevator.startMidMatchHoming();
elevator.setGoalHeight(ElevatorPositions.STOWED);
conveyor.setMode(ConveyorMode.IDLE);
queuer.setState(QueuerState.IDLE);
intake.setState(IntakeState.IDLE);
Expand Down Expand Up @@ -776,10 +743,6 @@ public Command waitForStateCommand(RobotState goalState) {
return Commands.waitUntil(() -> this.state == goalState);
}

public boolean getCollectiveHasNote() {
return intake.getHasNote() || queuer.getHasNote() || conveyor.hasNote();
}

public RobotState getState() {
return state;
}
Expand Down
70 changes: 41 additions & 29 deletions src/main/java/frc/robot/robot_manager/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,55 +5,67 @@
package frc.robot.robot_manager;

public enum RobotState {
UNHOMED,
HOMING,
UNHOMED(false, false),
HOMING(false, false),

IDLE_NO_GP,
IDLE_WITH_GP,
IDLE_NO_GP(false),
IDLE_WITH_GP(true),

GROUND_INTAKING,
GROUND_INTAKING(false),

OUTTAKING_SHOOTER,
OUTTAKING_INTAKE,
OUTTAKING_SHOOTER(true),
OUTTAKING_INTAKE(true),

QUEUER_TO_INTAKE_FOR_AMP,
CONVEYOR_TO_INTAKE_FOR_SHOOTER,
INTAKE_TO_QUEUER_FOR_SHOOTER,
INTAKE_TO_CONVEYOR_FOR_AMP,
QUEUER_TO_INTAKE_FOR_AMP(true),
CONVEYOR_TO_INTAKE_FOR_SHOOTER(true),
INTAKE_TO_QUEUER_FOR_SHOOTER(true),
INTAKE_TO_CONVEYOR_FOR_AMP(true),

WAITING_FLOOR_SHOT,
PREPARE_FLOOR_SHOT,
FLOOR_SHOOT,
WAITING_FLOOR_SHOT(true),
PREPARE_FLOOR_SHOT(true),
FLOOR_SHOOT(true),

/**
* Get ready for subwoofer shot, wait for driver/operator (?) to confirm, then go to
* PREPARE_SUBWOOFER_SHOT.
*/
WAITING_SUBWOOFER_SHOT,
WAITING_SUBWOOFER_SHOT(true),
/** Get ready for subwoofer shot, automatically go to SUBWOOFER_SHOOT when ready. */
PREPARE_SUBWOOFER_SHOT,
SUBWOOFER_SHOOT,
PREPARE_SUBWOOFER_SHOT(true),
SUBWOOFER_SHOOT(true),

PREPARE_TRAP_OUTTAKE,
TRAP_OUTTAKE,
PREPARE_TRAP_OUTTAKE(true),
TRAP_OUTTAKE(true),

/** Get ready for speaker shot, wait for driver to confirm, then go to PREPARE_SPEAKER_SHOT. */
WAITING_SPEAKER_SHOT,
WAITING_SPEAKER_SHOT(true),
/** Get ready for speaker shot, automatically go to SPEAKER_SHOOT when ready. */
PREPARE_SPEAKER_SHOT,
SPEAKER_SHOOT,
PREPARE_SPEAKER_SHOT(true),
SPEAKER_SHOOT(true),

WAITING_AMP_SHOT,
AMP_SHOT,
WAITING_AMP_SHOT(true),
AMP_SHOT(true),

/** Arm moves to chain height, climber hooks are touching the chain */
WAITING_CLIMBER_RAISED,
WAITING_CLIMBER_RAISED(true),

/** On the ground, arm goes up */
PREPARE_CLIMBER_RAISED,
CLIMBER_RAISED,
PREPARE_CLIMBER_RAISED(true),
CLIMBER_RAISED(true),

/** Actually climbing then hanging */
PREPARE_CLIMBER_HANGING,
CLIMBER_HANGING;
PREPARE_CLIMBER_HANGING(true),
CLIMBER_HANGING(true);

public final boolean hasNote;
public final boolean homed;

RobotState(boolean hasNote) {
this(hasNote, true);
}

RobotState(boolean hasNote, boolean homed) {
this.hasNote = hasNote;
this.homed = homed;
}
}

0 comments on commit d91fdd9

Please sign in to comment.