Skip to content

Commit

Permalink
miktake no setpoints untested
Browse files Browse the repository at this point in the history
  • Loading branch information
@eshen7
eshen7 committed Oct 30, 2023
1 parent ccc062d commit 9f86ee6
Show file tree
Hide file tree
Showing 4 changed files with 65 additions and 57 deletions.
59 changes: 31 additions & 28 deletions src/main/java/team3647/frc2023/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -249,7 +249,7 @@ private Command getSupestructureSequenceConeCubeFlat(
Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
superstructure
.scoreAndStowCube(0.2, -0.4, SuperstructureState.groundIntakeCube)
.scoreAndStowCube(0.2, 0.4, SuperstructureState.groundIntakeCube)
.raceWith(endRightAfterExtenderRetracted()),
Commands.waitSeconds(1),
Commands.deadline(
Expand Down Expand Up @@ -283,7 +283,7 @@ private Command getSupestructureSequenceConeCubeCubeMidFlat(
Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
superstructure
.scoreAndStowCube(0.2, -0.4, SuperstructureState.beforeLongTongueCube)
.scoreAndStowCube(0.2, 0.4, SuperstructureState.beforeLongTongueCube)
.raceWith(endRightAfterExtenderRetracted()),
Commands.waitSeconds(1),
Commands.deadline(
Expand All @@ -297,7 +297,7 @@ private Command getSupestructureSequenceConeCubeCubeMidFlat(
superstructure.goToStateParallel(SuperstructureState.cubeTwoReversedLoong),
Commands.waitSeconds(0.3),
superstructure
.scoreAndStowCube(0.3, -0.4, SuperstructureState.stowAll)
.scoreAndStowCube(0.3, 0.4, SuperstructureState.stowAll)
.raceWith(endRightAfterExtenderRetracted()),
superstructure.goToStateParallel(nextState));
}
Expand Down Expand Up @@ -333,24 +333,24 @@ private Command getSupestructureSequenceConeCubeBump() {
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(3),
superstructure.stow().withTimeout(1.3),
// Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
// Commands.waitSeconds(0.5),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
Commands.waitSeconds(0.5),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(2.35),
superstructure.stow().withTimeout(1.3),
superstructure.goToStateParallel(SuperstructureState.cubeTwoReversed),
Commands.waitSeconds(0.4),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore));
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore));
}

private Command getSuperstrcutreSequenceConeBalance() {
Expand Down Expand Up @@ -401,7 +401,7 @@ private Command getSupestructureSeqeunceConeCubeBalance() {
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(1.3),
// superstructure.stow().withTimeout(1),
superstructure.goToStateParallel(SuperstructureState.untipReverse).withTimeout(2),
Expand All @@ -410,7 +410,7 @@ private Command getSupestructureSeqeunceConeCubeBalance() {
superstructure.goToStateParallel(SuperstructureState.cubeTwoReversed),
// Commands.waitSeconds(1),
// Commands.waitSeconds(0.5),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
superstructure
.goToStateParallel(SuperstructureState.pushDownStation)
.withTimeout(2),
Expand All @@ -429,24 +429,24 @@ private Command getSupestructureSequenceConeCubeCubeBumpNoBump() {
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(2),
superstructure.stow().withTimeout(1),
// Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
// Commands.waitSeconds(0.5),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
// Commands.waitSeconds(0.5),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(3),
superstructure.stow().withTimeout(1),
superstructure.goToStateParallel(SuperstructureState.cubeTwoReversed),
// Commands.waitSeconds(0.2),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore));
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore));
}

private Command getSupestructureSequenceConeCubeCubeBumpNoBumpBalance() {
Expand All @@ -460,24 +460,24 @@ private Command getSupestructureSequenceConeCubeCubeBumpNoBumpBalance() {
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(2),
superstructure.stow().withTimeout(1),
// Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
// Commands.waitSeconds(0.5),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
// Commands.waitSeconds(0.5),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(1.5),
// superstructure.stow().withTimeout(1),
superstructure.goToStateParallel(SuperstructureState.cubeTwoReversed),
// Commands.waitSeconds(0.2),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
superstructure.goToStateParallel(SuperstructureState.pushDownStation),
Commands.waitSeconds(1),
superstructure.stowScore());
Expand All @@ -494,13 +494,13 @@ private Command getSupestructureSequenceConeCube2PieceBumpNoBumpBalance() {
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(2),
superstructure.stow().withTimeout(1),
// Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
// Commands.waitSeconds(0.5),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
// Commands.waitSeconds(0.5),
superstructure.goToStateParallel(SuperstructureState.pushDownStation),
Commands.waitSeconds(1),
Expand All @@ -518,19 +518,19 @@ private Command getSupestructureSequenceConeCubeCubeHoldBumpNoBumpBalance() {
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(2),
superstructure.stow().withTimeout(1),
// Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
// Commands.waitSeconds(0.5),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
// Commands.waitSeconds(0.5),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(1.5),
superstructure.goToStateParallel(SuperstructureState.untipReverse),
Commands.waitSeconds(1),
Expand All @@ -548,24 +548,24 @@ private Command getSupestructureSequenceConeCubeCubeFlat() {
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(2.2),
superstructure.stow().withTimeout(0.5),
// Commands.waitSeconds(0.2),
superstructure.goToStateParallel(SuperstructureState.cubeThreeReversed),
// Commands.waitSeconds(0.5),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore),
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore),
// Commands.waitSeconds(0.5),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
superstructure.rollersCommands.openloop(() -> -0.45))
.withTimeout(2.2),
superstructure.stow().withTimeout(1),
superstructure.goToStateParallel(SuperstructureState.cubeTwoReversed),
Commands.waitSeconds(0.2),
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore));
superstructure.scoreAndStowCube(0.5, 0.4, SuperstructureState.stowScore));
}

public Command drive() {
Expand Down Expand Up @@ -1026,8 +1026,11 @@ public Command justScore(SuperstructureState state, SuperstructureState nextStat
return Commands.sequence(
Commands.parallel(
superstructure.goToStateParallel(state),
superstructure.rollersCommands.outCone().withTimeout(1)),
superstructure.scoreAndStowConeReversed(nextState));
superstructure.rollersCommands.outCube().withTimeout(0.8)),
superstructure.goToStateParallel(state.addWristExtend(-60, 0)),
Commands.parallel(
superstructure.rollersCommands.openloop(() -> 0),
superstructure.goToStateParallel(nextState)));
}

public Command justScore(SuperstructureState state) {
Expand Down
7 changes: 2 additions & 5 deletions src/main/java/team3647/frc2023/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,8 +186,7 @@ private void configureButtonBindings() {
mainController
.rightBumper
.and(groundCubeIntake)
.whileTrue(superstructure.goToStateParallel(SuperstructureState.pushDownStation))
.onFalse(superstructure.stow());
.whileTrue(superstructure.intakeGroundCone());

// mainController
// .rightBumper
Expand Down Expand Up @@ -599,9 +598,7 @@ public Command getAutonomousCommand() {

public final Trigger goodForLockIntake =
mainController.rightBumper.and(
() ->
(superstructure.getWantedStation() == StationType.Double
|| superstructure.getWantedIntakePiece() == GamePiece.Cone));
() -> (superstructure.getWantedStation() == StationType.Double));

private final BooleanSupplier groundConeIntake =
() ->
Expand Down
Loading

0 comments on commit 9f86ee6

Please sign in to comment.