Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

States motion planner -> superstructure #31

Open
wants to merge 18 commits into
base: core
Choose a base branch
from
Open

States motion planner -> superstructure #31

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
b2abb48
maya- working on superstructure , statesMotionPlanner
maya1414 Oct 17, 2024
2159abc
maya- spotless
maya1414 Oct 17, 2024
841a04a
maya- work on CR
maya1414 Oct 17, 2024
85ce02b
maya- work on CR
maya1414 Oct 17, 2024
86e22b4
Maya- merge from superstructure
maya1414 Oct 17, 2024
2039d8f
maya- work on CR
maya1414 Oct 17, 2024
f12baca
maya- work on CR
maya1414 Oct 18, 2024
44c3294
maya- work on CR
maya1414 Oct 18, 2024
e969b16
maya- work on CR
maya1414 Oct 18, 2024
6969ee6
maya- merge
maya1414 Oct 18, 2024
b4a3486
maya- work on CR
maya1414 Oct 18, 2024
189595b
maya- return amp, speaker
maya1414 Oct 18, 2024
c61b8df
Maya- work on CR
maya1414 Oct 19, 2024
30313b0
Maya- fixing
maya1414 Oct 19, 2024
1867268
Maya- work on CR
maya1414 Oct 19, 2024
2ad8714
Maya- work on CR
maya1414 Oct 19, 2024
db8a6a1
Maya- work on CR
maya1414 Oct 19, 2024
5076d2f
Maya- work on CR
maya1414 Oct 19, 2024
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
27 changes: 27 additions & 0 deletions src/main/java/frc/robot/superstructure/StatesMotionPlanner.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
package frc.robot.superstructure;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;

public class StatesMotionPlanner {

private final Superstructure superstructure;

public StatesMotionPlanner(Superstructure superstructure) {
this.superstructure = superstructure;
}

public Command setState(RobotState state) {
return switch (state) {
case INTAKE, PRE_SPEAKER, PRE_AMP, TRANSFER_ELEVATOR_SHOOTER, TRANSFER_SHOOTER_ELEVATOR ->
new SequentialCommandGroup(
superstructure.enableChangeStateAutomatically(false),
superstructure.setState(state).until(superstructure::isEnableChangeStateAutomatically),
superstructure.setState(RobotState.IDLE)
);
case INTAKE_OUTTAKE, SPEAKER, AMP, IDLE, SHOOTER_OUTTAKE -> superstructure.setState(state);
};
}
maya1414 marked this conversation as resolved.
Show resolved Hide resolved

}

maya1414 marked this conversation as resolved.
Show resolved Hide resolved
28 changes: 24 additions & 4 deletions src/main/java/frc/robot/superstructure/Superstructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,9 @@ public class Superstructure {

private RobotState currentState;

private boolean enableChangeStateAutomatically;

maya1414 marked this conversation as resolved.
Show resolved Hide resolved

public Superstructure(String logPath, Robot robot) {
this.logPath = logPath;

Expand All @@ -51,6 +54,8 @@ public Superstructure(String logPath, Robot robot) {
this.intakeStatesHandler = new IntakeStatesHandler(robot.getIntakeRoller());
this.pivotStateHandler = new PivotStateHandler(robot.getPivot());
this.elevatorStatesHandler = new ElevatorStatesHandler(robot.getElevator());

this.enableChangeStateAutomatically = true;
}

public RobotState getCurrentState() {
Expand All @@ -59,6 +64,11 @@ public RobotState getCurrentState() {

public void logStatus() {
Logger.recordOutput(logPath + "CurrentState", currentState);
Logger.recordOutput(logPath + "EnableChangeStateAutomatically", enableChangeStateAutomatically);
}

public boolean isEnableChangeStateAutomatically() {
return enableChangeStateAutomatically;
}

private boolean isNoteInShooter() {
Expand Down Expand Up @@ -100,6 +110,10 @@ private boolean isNoteInRobot(AtomicReference<Double> lastTimeDetectedNote) {
return false;
}

public Command enableChangeStateAutomatically(boolean enable) {
return new InstantCommand(() -> enableChangeStateAutomatically = enable);
}

private Command setCurrentStateValue(RobotState state) {
return new InstantCommand(() -> currentState = state);
}
Expand All @@ -122,6 +136,7 @@ public Command setState(RobotState state) {
//@formatter:off
public Command idle() {
return new ParallelCommandGroup(
enableChangeStateAutomatically(true),
setCurrentStateValue(RobotState.IDLE),
swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
Expand All @@ -130,12 +145,13 @@ public Command idle() {
intakeStatesHandler.setState(IntakeStates.STOP),
pivotStateHandler.setState(PivotState.UP),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command intake() {
return new ParallelCommandGroup(
setCurrentStateValue(RobotState.INTAKE),
enableChangeStateAutomatically(false),
new SequentialCommandGroup(
new ParallelCommandGroup(
funnelStateHandler.setState(FunnelState.NOTE_TO_SHOOTER),
Expand All @@ -147,6 +163,7 @@ public Command intake() {
intakeStatesHandler.setState(IntakeStates.INTAKE),
pivotStateHandler.setState(PivotState.UP)
).until(this::isNoteInShooter),
enableChangeStateAutomatically(true),
new ParallelCommandGroup(
funnelStateHandler.setState(FunnelState.STOP),
intakeStatesHandler.setState(IntakeStates.STOP)
Expand All @@ -156,37 +173,40 @@ public Command intake() {
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
flywheelStateHandler.setState(FlywheelState.DEFAULT),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command preSpeaker() {
return new ParallelCommandGroup(
setCurrentStateValue(RobotState.PRE_SPEAKER),
enableChangeStateAutomatically(true),
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE.withAimAssist(AimAssist.SPEAKER)),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
flywheelStateHandler.setState(FlywheelState.SHOOTING),
funnelStateHandler.setState(FunnelState.STOP),
intakeStatesHandler.setState(IntakeStates.STOP),
pivotStateHandler.setState(PivotState.UP),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command speaker() {
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
return new ParallelCommandGroup(
setCurrentStateValue(RobotState.SPEAKER),
enableChangeStateAutomatically(false),
new SequentialCommandGroup(
funnelStateHandler.setState(FunnelState.STOP).until(this::isReadyToShoot),
funnelStateHandler.setState(FunnelState.SPEAKER).until(() -> !isNoteInShooter()),
funnelStateHandler.setState(FunnelState.STOP)
),
enableChangeStateAutomatically(true),
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE.withAimAssist(AimAssist.SPEAKER)),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
flywheelStateHandler.setState(FlywheelState.SHOOTING),
intakeStatesHandler.setState(IntakeStates.STOP),
pivotStateHandler.setState(PivotState.UP),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command preAmp() {
Expand Down
Loading