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 17 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, SPEAKER, AMP, TRANSFER_ELEVATOR_SHOOTER, TRANSFER_SHOOTER_ELEVATOR ->
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
new SequentialCommandGroup(
superstructure.enableChangeStateAutomatically(false),
superstructure.setState(state).until(superstructure::isEnableChangeStateAutomatically),
superstructure.setState(RobotState.IDLE)
);
case INTAKE_OUTTAKE, PRE_AMP, PRE_SPEAKER, SHOOTER_OUTTAKE, IDLE -> superstructure.setState(state);
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
};
}

}

maya1414 marked this conversation as resolved.
Show resolved Hide resolved
57 changes: 43 additions & 14 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 @@ -36,6 +36,8 @@ 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 @@ -47,6 +49,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 @@ -55,6 +59,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 @@ -85,6 +94,11 @@ private Command setCurrentStateName(RobotState state) {
return new InstantCommand(() -> currentState = state);
}

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


public Command setState(RobotState state) {
return switch (state) {
case IDLE -> idle();
Expand All @@ -103,6 +117,7 @@ public Command setState(RobotState state) {
//@formatter:off
public Command idle() {
return new ParallelCommandGroup(
enableChangeStateAutomatically(true),
setCurrentStateName(RobotState.IDLE),
// swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
Expand All @@ -111,18 +126,20 @@ 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(
setCurrentStateName(RobotState.INTAKE),
enableChangeStateAutomatically(false),
new SequentialCommandGroup(
new ParallelCommandGroup(
funnelStateHandler.setState(FunnelState.NOTE_TO_SHOOTER),
intakeStatesHandler.setState(IntakeStates.INTAKE),
pivotStateHandler.setState(PivotState.ON_FLOOR)
).until(this::isNoteInShooter),
enableChangeStateAutomatically(true),
new ParallelCommandGroup(
pivotStateHandler.setState(PivotState.UP),
funnelStateHandler.setState(FunnelState.STOP),
Expand All @@ -133,28 +150,31 @@ 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(
setCurrentStateName(RobotState.PRE_SPEAKER),
enableChangeStateAutomatically(true),
// 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(
setCurrentStateName(RobotState.SPEAKER),
enableChangeStateAutomatically(false),
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
new SequentialCommandGroup(
funnelStateHandler.setState(FunnelState.STOP).until(this::isReadyToShoot),
funnelStateHandler.setState(FunnelState.SPEAKER),//.until(() -> !isNoteInShooter()),
enableChangeStateAutomatically(true),
funnelStateHandler.setState(FunnelState.STOP)
),
// swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE.withAimAssist(AimAssist.SPEAKER)),
Expand All @@ -163,26 +183,28 @@ public Command speaker() {
intakeStatesHandler.setState(IntakeStates.STOP),
pivotStateHandler.setState(PivotState.UP),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command preAmp() {
return new ParallelCommandGroup(
setCurrentStateName(RobotState.PRE_AMP),
enableChangeStateAutomatically(true),
// swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE.withAimAssist(AimAssist.AMP)),
funnelStateHandler.setState(FunnelState.STOP),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
flywheelStateHandler.setState(FlywheelState.DEFAULT),
intakeStatesHandler.setState(IntakeStates.STOP),
pivotStateHandler.setState(PivotState.UP),
elevatorStatesHandler.setState(ElevatorStates.PRE_AMP)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command amp() {
return new ParallelCommandGroup(
setCurrentStateName(RobotState.AMP),
new SequentialCommandGroup(
enableChangeStateAutomatically(false),
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
new SequentialCommandGroup(
new ParallelCommandGroup(
// swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE.withAimAssist(AimAssist.AMP)),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
Expand All @@ -197,18 +219,20 @@ public Command amp() {
// swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
)
),
enableChangeStateAutomatically(true)
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
),
flywheelStateHandler.setState(FlywheelState.DEFAULT),
intakeStatesHandler.setState(IntakeStates.STOP),
pivotStateHandler.setState(PivotState.UP),
funnelStateHandler.setState(FunnelState.STOP)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command transferShooterElevator() {
return new ParallelCommandGroup(
setCurrentStateName(RobotState.TRANSFER_SHOOTER_ELEVATOR),
enableChangeStateAutomatically(false),
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
new SequentialCommandGroup(
new ParallelCommandGroup(
intakeStatesHandler.setState(IntakeStates.INTAKE),
Expand All @@ -218,27 +242,30 @@ public Command transferShooterElevator() {
new ParallelCommandGroup(
intakeStatesHandler.setState(IntakeStates.STOP),
funnelStateHandler.setState(FunnelState.STOP),
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP)
elevatorRollerStateHandler.setState(ElevatorRollerState.STOP),
enableChangeStateAutomatically(true)
)
),
// swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE),
flywheelStateHandler.setState(FlywheelState.DEFAULT),
elevatorStatesHandler.setState(ElevatorStates.IDLE),
pivotStateHandler.setState(PivotState.UP)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command transferElevatorShooter() {
return new ParallelCommandGroup(
setCurrentStateName(RobotState.TRANSFER_ELEVATOR_SHOOTER),
enableChangeStateAutomatically(false),
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
new SequentialCommandGroup(
new ParallelCommandGroup(
intakeStatesHandler.setState(IntakeStates.INTAKE),
funnelStateHandler.setState(FunnelState.NOTE_TO_SHOOTER)
),//.until(this::isNoteInShooter),
new ParallelCommandGroup(
intakeStatesHandler.setState(IntakeStates.STOP),
funnelStateHandler.setState(FunnelState.STOP)
funnelStateHandler.setState(FunnelState.STOP),
enableChangeStateAutomatically(true)
)
),
new SequentialCommandGroup(
Expand All @@ -249,11 +276,12 @@ public Command transferElevatorShooter() {
flywheelStateHandler.setState(FlywheelState.DEFAULT),
elevatorStatesHandler.setState(ElevatorStates.IDLE),
pivotStateHandler.setState(PivotState.UP)
);
).handleInterrupt(() -> enableChangeStateAutomatically(true).schedule());
}

public Command intakeOuttake() {
return new ParallelCommandGroup(
enableChangeStateAutomatically(true),
maya1414 marked this conversation as resolved.
Show resolved Hide resolved
setCurrentStateName(RobotState.INTAKE_OUTTAKE),
// swerve.getCommandsBuilder().saveState(SwerveState.DEFAULT_DRIVE),
elevatorRollerStateHandler.setState(ElevatorRollerState.TRANSFER_FROM_ELEVATOR),
Expand All @@ -262,11 +290,12 @@ public Command intakeOuttake() {
intakeStatesHandler.setState(IntakeStates.OUTTAKE),
pivotStateHandler.setState(PivotState.UP),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
);
).handleInterrupt(()->enableChangeStateAutomatically(true).schedule());
}

public Command shooterOuttake() {
return new ParallelCommandGroup(
enableChangeStateAutomatically(true),
setCurrentStateName(RobotState.SHOOTER_OUTTAKE),
new SequentialCommandGroup(
funnelStateHandler.setState(FunnelState.SHOOTER_OUTTAKE).until(() -> !isNoteInShooter()),
Expand All @@ -278,7 +307,7 @@ public Command shooterOuttake() {
intakeStatesHandler.setState(IntakeStates.STOP),
pivotStateHandler.setState(PivotState.UP),
elevatorStatesHandler.setState(ElevatorStates.IDLE)
);
).handleInterrupt(()-> enableChangeStateAutomatically(true).schedule());
}
//@formatter:on

Expand Down
Loading