Skip to content
This repository has been archived by the owner on Jan 10, 2025. It is now read-only.

Commit

Permalink
Refactor StateMachine class to use handler pattern
Browse files Browse the repository at this point in the history
  • Loading branch information
@jonahsnider
jonahsnider committed Nov 1, 2024
1 parent c13e4fd commit cced47e
Show file tree
Hide file tree
Showing 10 changed files with 629 additions and 515 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/arm/ArmAngle.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
package frc.robot.arm;

public class ArmAngle {
public static final double SUBWOOFER = -20.0;
public static final double SUBWOOFER = -20.0;
public static final double DROP = -60.0;
public static final double PODIUM = -40.0;
public static final double IDLE = -77.0;
Expand Down
173 changes: 91 additions & 82 deletions src/main/java/frc/robot/arm/ArmSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,97 @@ public ArmSubsystem(TalonFX leftMotor, TalonFX rightMotor) {
rightMotor.getConfigurator().apply(RobotConfig.get().arm().rightMotorConfig());
RobotConfig.get().arm().feedSpotDistanceToAngle().accept(feedSpotDistanceToAngle);
RobotConfig.get().arm().speakerDistanceToAngle().accept(speakerDistanceToAngle);

createHandler(ArmState.PRE_MATCH_HOMING)
.onEnter(
() -> {
leftMotor.disable();
rightMotor.disable();
});
createHandler(ArmState.IDLE)
.onEnter(
() -> {
leftMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE))));
rightMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE))));
});
createHandler(ArmState.CLIMBING_1_LINEUP)
.onEnter(
() -> {
leftMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP))));
rightMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP))));
});
createHandler(ArmState.CLIMBING_2_HANGING)
.onEnter(
() -> {
leftMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING))));
rightMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING))));
});
createHandler(ArmState.DROP)
.onEnter(
() -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP))));
});
createHandler(ArmState.PODIUM_SHOT)
.onEnter(
() -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM))));
});
createHandler(ArmState.SUBWOOFER_SHOT)
.onEnter(
() -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER))));
});
createHandler(ArmState.AMP)
.onEnter(
() -> {
leftMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP))));
rightMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP))));
});
createHandler(ArmState.PASS)
.onEnter(
() -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS))));
});
createHandler(ArmState.SPEAKER_SHOT)
.withPeriodic(
() -> {
var newAngle =
Units.degreesToRotations(clamp(speakerDistanceToAngle.get(distanceToSpeaker)));
leftMotor.setControl(pidRequest.withPosition(newAngle));
rightMotor.setControl(pidRequest.withPosition(newAngle));
});
createHandler(ArmState.FEEDING)
.withPeriodic(
() -> {
var newAngle =
Units.degreesToRotations(clamp(feedSpotDistanceToAngle.get(distanceToFeedSpot)));
leftMotor.setControl(pidRequest.withPosition(newAngle));
rightMotor.setControl(pidRequest.withPosition(newAngle));
});
}

public void setState(ArmState newState) {
Expand Down Expand Up @@ -98,92 +189,10 @@ protected void collectInputs() {
}
}

@Override
protected void afterTransition(ArmState newState) {
switch (newState) {
case PRE_MATCH_HOMING -> {
leftMotor.disable();
rightMotor.disable();
}
case IDLE -> {
leftMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE))));
rightMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE))));
}
case CLIMBING_1_LINEUP -> {
leftMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP))));
rightMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP))));
}
case CLIMBING_2_HANGING -> {
leftMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING))));
rightMotor.setControl(
motionMagicRequest.withPosition(
Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING))));
}

case DROP -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP))));
}

case PODIUM_SHOT -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM))));
}
case SUBWOOFER_SHOT -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER))));
}

case AMP -> {
leftMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP))));
rightMotor.setControl(
motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP))));
}
case PASS -> {
leftMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS))));
rightMotor.setControl(
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS))));
}
default -> {}
}
}

@Override
public void robotPeriodic() {
super.robotPeriodic();

switch (getState()) {
case SPEAKER_SHOT -> {
var newAngle =
Units.degreesToRotations(clamp(speakerDistanceToAngle.get(distanceToSpeaker)));
leftMotor.setControl(pidRequest.withPosition(newAngle));
rightMotor.setControl(pidRequest.withPosition(newAngle));
}
case FEEDING -> {
double newAngle =
Units.degreesToRotations(clamp(feedSpotDistanceToAngle.get(distanceToFeedSpot)));
leftMotor.setControl(pidRequest.withPosition(newAngle));
rightMotor.setControl(pidRequest.withPosition(newAngle));
}
default -> {}
}

DogLog.log("Arm/Left/StatorCurrent", leftMotor.getStatorCurrent().getValueAsDouble());
DogLog.log("Arm/Left/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble());
DogLog.log(
Expand Down
60 changes: 29 additions & 31 deletions src/main/java/frc/robot/intake/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,35 @@ public IntakeSubsystem(TalonFX mainMotor, CANSparkMax centeringMotor) {
mainMotor.getConfigurator().apply(RobotConfig.get().intake().mainMotorConfig());

RobotConfig.get().intake().centeringMotorConfig().accept(centeringMotor);

createHandler(IntakeState.IDLE)
.onEnter(
() -> {
mainMotor.disable();
centeringMotor.disable();
});
createHandler(IntakeState.INTAKING)
.onEnter(
() -> {
mainMotor.setVoltage(12);
centeringMotor.setVoltage(8);
});
createHandler(IntakeState.OUTTAKING)
.onEnter(
() -> {
mainMotor.setVoltage(-6);
centeringMotor.setVoltage(-10);
});
createHandler(IntakeState.INTAKING_BACK)
.onEnter(
() -> {
mainMotor.setVoltage(-1);
});
createHandler(IntakeState.INTAKING_FORWARD_PUSH)
.onEnter(
() -> {
mainMotor.setVoltage(1);
});
}

public void setState(IntakeState newState) {
Expand All @@ -29,37 +58,6 @@ public double getIntakeRotations() {
return mainMotor.getRotorPosition().getValueAsDouble();
}

@Override
protected IntakeState getNextState(IntakeState currentState) {
return currentState;
}

@Override
protected void afterTransition(IntakeState newState) {
switch (newState) {
case IDLE -> {
mainMotor.disable();
centeringMotor.disable();
}

case INTAKING -> {
mainMotor.setVoltage(12);
centeringMotor.setVoltage(8);
}

case OUTTAKING -> {
mainMotor.setVoltage(-6);
centeringMotor.setVoltage(-10);
}
case INTAKING_BACK -> {
mainMotor.setVoltage(-1);
}
case INTAKING_FORWARD_PUSH -> {
mainMotor.setVoltage(1);
}
}
}

@Override
public void robotPeriodic() {
super.robotPeriodic();
Expand Down
18 changes: 7 additions & 11 deletions src/main/java/frc/robot/localization/LocalizationSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot.localization;

import dev.doglog.DogLog;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
Expand All @@ -23,14 +24,12 @@
import java.util.ArrayList;
import java.util.List;

import dev.doglog.DogLog;

public class LocalizationSubsystem extends StateMachine<LocalizationState> {
private static final Vector<N3> VISION_STD_DEVS =
VecBuilder.fill(
RobotConfig.get().vision().xyStdDev(),
RobotConfig.get().vision().xyStdDev(),
RobotConfig.get().vision().thetaStdDev());
VecBuilder.fill(
RobotConfig.get().vision().xyStdDev(),
RobotConfig.get().vision().xyStdDev(),
RobotConfig.get().vision().thetaStdDev());
private final ImuSubsystem imu;
private final VisionSubsystem vision;
private final SwerveSubsystem swerve;
Expand Down Expand Up @@ -77,17 +76,14 @@ public void robotPeriodic() {
if (visionTimestamp == lastAddedVisionTimestamp) {
// Don't add the same vision pose over and over
} else {
poseEstimator.addVisionMeasurement(
visionPose,
visionTimestamp,
VISION_STD_DEVS);
poseEstimator.addVisionMeasurement(visionPose, visionTimestamp, VISION_STD_DEVS);
lastAddedVisionTimestamp = visionTimestamp;
}

poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
}

DogLog.log("Localization/EstimatedPose", getPose());
DogLog.log("Localization/EstimatedPose", getPose());
}

private void resetGyro(double gyroAngle) {
Expand Down
35 changes: 13 additions & 22 deletions src/main/java/frc/robot/queuer/QueuerSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,19 @@ public QueuerSubsystem(TalonFX motor, DigitalInput sensor) {
this.motor = motor;

motor.getConfigurator().apply(RobotConfig.get().queuer().motorConfig());

createHandler(QueuerState.IDLE).onEnter(() -> motor.disable());
createHandler(QueuerState.SHOOTING).onEnter(() -> motor.setVoltage(10));
createHandler(QueuerState.INTAKING).onEnter(() -> motor.setVoltage(1));
createHandler(QueuerState.OUTTAKING).onEnter(() -> motor.setVoltage(-4));
createHandler(QueuerState.AMPING).onEnter(() -> motor.setVoltage(-10));
createHandler(QueuerState.INTAKING_BACK).onEnter(() -> motor.setVoltage(-1));
createHandler(QueuerState.INTAKING_FORWARD_PUSH)
.onEnter(
() -> {
goalPosition = motorPosition + 2;
motor.setControl(pidRequest.withPosition(goalPosition));
});
}

public void setState(QueuerState newState) {
Expand All @@ -48,32 +61,10 @@ protected void collectInputs() {
motorPosition = motor.getRotorPosition().getValueAsDouble();
}

@Override
protected QueuerState getNextState(QueuerState currentState) {
// State transitions are done by robot manager, not here
return currentState;
}

public boolean hasNote() {
return debouncedSensorHasNote;
}

@Override
protected void afterTransition(QueuerState newState) {
switch (newState) {
case IDLE -> motor.disable();
case SHOOTING -> motor.setVoltage(10);
case INTAKING -> motor.setVoltage(1);
case OUTTAKING -> motor.setVoltage(-4);
case AMPING -> motor.setVoltage(-10);
case INTAKING_BACK -> motor.setVoltage(-1);
case INTAKING_FORWARD_PUSH -> {
goalPosition = motorPosition + 2;
motor.setControl(pidRequest.withPosition(goalPosition));
}
}
}

@Override
public void robotPeriodic() {
super.robotPeriodic();
Expand Down
Loading

0 comments on commit cced47e

Please sign in to comment.