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

2025 StateMachine improvements #83

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

2025 StateMachine improvements #83

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
173 changes: 91 additions & 82 deletions src/main/java/frc/robot/arm/ArmSubsystem.java
View file
Open in desktop
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
Open in desktop
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
35 changes: 13 additions & 22 deletions src/main/java/frc/robot/queuer/QueuerSubsystem.java
View file
Open in desktop
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