Skip to content

Commit

Permalink
should be comp ready
Browse files Browse the repository at this point in the history
  • Loading branch information
@eshen7
eshen7 committed Oct 19, 2023
1 parent b9a5c37 commit ee8cc6a
Show file tree
Hide file tree
Showing 4 changed files with 194 additions and 10 deletions.
127 changes: 122 additions & 5 deletions src/main/java/team3647/frc2023/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ public class AutoCommands {
public final AutonomousMode blueJustDrive;
public final AutonomousMode blueConeCubeCubeLowBalanceBumpSide;
public final AutonomousMode blueConeTaxiBalance;
public final AutonomousMode blueConeCubeCubeHoldBalance;

public final AutonomousMode redConeCubeCubeBumpSideNoBump;
public final AutonomousMode redConeCubeCubeFlatSideMode;
Expand All @@ -49,6 +50,7 @@ public class AutoCommands {
public final AutonomousMode redJustDrive;
public final AutonomousMode redConeCubeCubeLowBalanceBumpSide;
public final AutonomousMode redConeTaxiBalance;
public final AutonomousMode redConeCubeCubeHoldBalance;

public AutoCommands(
SwerveDrive drive,
Expand Down Expand Up @@ -104,6 +106,10 @@ public AutoCommands(
new AutonomousMode(
coneTaxiBalance(),
Trajectories.Blue.coneCubeCubeBalanceBumpSideNoBump.kFirstPathInitial);
blueConeCubeCubeHoldBalance =
new AutonomousMode(
coneCubeCubeHoldBalanceBumpSideNoBump(Alliance.Blue),
Trajectories.Blue.coneCubeCubeHoldBalanceBumpSideNoBump.kFirstPathInitial);

// red side modes
redConeCubeCubeBumpSideNoBump =
Expand Down Expand Up @@ -151,14 +157,22 @@ public AutoCommands(
flipForPP(Trajectories.Blue.justDrive.kFirstPathInitial));
redConeCubeCubeLowBalanceBumpSide =
new AutonomousMode(
coneCubeCubeLowBalanceBumpSideNoBump(Alliance.Blue),
Trajectories.Blue.coneCubeCubeBalanceBumpSideNoBump.kFirstPathInitial);
coneCubeCubeLowBalanceBumpSideNoBump(Alliance.Red),
flipForPP(
Trajectories.Blue.coneCubeCubeBalanceBumpSideNoBump
.kFirstPathInitial));
redConeTaxiBalance =
new AutonomousMode(
coneTaxiBalance(),
flipForPP(
Trajectories.Blue.coneCubeCubeBalanceBumpSideNoBump
.kFirstPathInitial));
redConeCubeCubeHoldBalance =
new AutonomousMode(
coneCubeCubeHoldBalanceBumpSideNoBump(Alliance.Red),
flipForPP(
Trajectories.Blue.coneCubeCubeHoldBalanceBumpSideNoBump
.kFirstPathInitial));
}

public static Pose2d getJustScore(Pose2d pose) {
Expand Down Expand Up @@ -421,6 +435,70 @@ private Command getSupestructureSequenceConeCubeCubeBumpNoBump() {
superstructure.scoreAndStowCube(0.5, -0.4, SuperstructureState.stowScore));
}

private Command getSupestructureSequenceConeCubeCubeBumpNoBumpBalance() {
return Commands.sequence(
justScore(
SuperstructureState.coneThreeReversed,
SuperstructureState.stowAfterConeThreeReversed)
.raceWith(endRightAfterExtenderRetracted()),
Commands.waitSeconds(0.2),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
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),
// Commands.waitSeconds(0.5),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
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.goToStateParallel(SuperstructureState.pushDownStation),
Commands.waitSeconds(1),
superstructure.stowScore());
}

private Command getSupestructureSequenceConeCubeCubeHoldBumpNoBumpBalance() {
return Commands.sequence(
justScore(
SuperstructureState.coneThreeReversed,
SuperstructureState.stowAfterConeThreeReversed)
.raceWith(endRightAfterExtenderRetracted()),
Commands.waitSeconds(0.2),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
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),
// Commands.waitSeconds(0.5),
Commands.deadline(
superstructure.waitForCurrentSpike(7),
superstructure.goToStateParallel(
SuperstructureState.longTongueCube),
superstructure.rollersCommands.openloop(() -> 0.45))
.withTimeout(1.5),
superstructure.goToStateParallel(SuperstructureState.untipReverse),
Commands.waitSeconds(1),
superstructure.goToStateParallel(SuperstructureState.backStow));
}

private Command getSupestructureSequenceConeCubeCubeFlat() {
return Commands.sequence(
justScore(
Expand Down Expand Up @@ -527,11 +605,50 @@ public Command coneCubeCubeLowBalanceBumpSideNoBump(Alliance color) {
Trajectories.Blue.coneCubeCubeBalanceBumpSideNoBump
.kFourthTrajectory,
color)),
driveForward().until(() -> (drive.getPitch() > 9)),
driveForward().until(() -> (drive.getPitch() < 9)));
driveForwardSlwoly().until(() -> (drive.getPitch() < -13)),
driveForwardSlwoly().until(() -> (drive.getPitch() > -13)),
lock());
// ,
return Commands.parallel(
drivetrainSequence, getSupestructureSequenceConeCubeCubeBumpNoBump());
drivetrainSequence, getSupestructureSequenceConeCubeCubeBumpNoBumpBalance());
}

public Command coneCubeCubeHoldBalanceBumpSideNoBump(Alliance color) {
Command drivetrainSequence =
Commands.sequence(
Commands.waitSeconds(1.8), // score cone
followTrajectory(
PathPlannerTrajectory.transformTrajectoryForAlliance(
Trajectories.Blue.coneCubeCubeHoldBalanceBumpSideNoBump
.kFirstTrajectory,
color)),
// rollers don't need waiting
followTrajectory(
PathPlannerTrajectory.transformTrajectoryForAlliance(
Trajectories.Blue.coneCubeCubeHoldBalanceBumpSideNoBump
.kSecondTrajectory,
color)),
followTrajectory(
PathPlannerTrajectory.transformTrajectoryForAlliance(
Trajectories.Blue.coneCubeCubeHoldBalanceBumpSideNoBump
.kThirdTrajectory,
color)),
Commands.waitSeconds(0.8),
// followTrajectory(
// PathPlannerTrajectory.transformTrajectoryForAlliance(
// Trajectories.Blue.ConeCubeBalanceBumpSide.kIntakeCube,
// color)),
followTrajectory(
PathPlannerTrajectory.transformTrajectoryForAlliance(
Trajectories.Blue.coneCubeCubeHoldBalanceBumpSideNoBump
.kFourthTrajectory,
color)),
driveBackwardSlowly().until(() -> (drive.getPitch() > 13)),
driveBackwardSlowly().until(() -> (drive.getPitch() < 13)),
lock());
// ,
return Commands.parallel(
drivetrainSequence, getSupestructureSequenceConeCubeCubeHoldBumpNoBumpBalance());
}

public Command coneCubeBalanceBumpSide(Alliance color) {
Expand Down
59 changes: 58 additions & 1 deletion src/main/java/team3647/frc2023/auto/Trajectories.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,8 @@ public static final class coneCubeCubeBalanceBumpSideNoBump {
private static final Pose2d kThirdPathFinal =
new Pose2d(6.25, 1.37, Rotation2d.fromDegrees(45));
private static final Pose2d kFourthPathInitial = kThirdPathFinal;
private static final Pose2d kFourthPathWaypoint1 =
new Pose2d(4.1, 0.6, FieldConstants.kZero);
// private static final Pose2d kFourthPathWaypoint1 = kThirdPathWaypoint2;
// private static final Pose2d kFourthPathWaypoint2 = kThirdPathWaypoint1;
private static final Pose2d kFourthPathFinal =
Expand Down Expand Up @@ -221,14 +223,69 @@ public static final class coneCubeCubeBalanceBumpSideNoBump {
fastConstraints,
List.of(
fromPose(kFourthPathInitial, Rotation2d.fromDegrees(-135)),
fromPose(kThirdPathWaypoint1, FieldConstants.kOneEighty),
fromPose(kFourthPathWaypoint1, FieldConstants.kOneEighty),
// fromPose(kFourthPathWaypoint1,
// Rotation2d.fromDegrees(155)),
// fromPose(kFourthPathWaypoint2,
// FieldConstants.kOneEighty),
fromPose(kFourthPathFinal, Rotation2d.fromDegrees(45))));
}

public static final class coneCubeCubeHoldBalanceBumpSideNoBump {
public static final Pose2d kFirstPathInitial =
new Pose2d(1.80, 0.5, FieldConstants.kZero);
private static final Pose2d kFirstPathFinal =
new Pose2d(5.7, 0.98, FieldConstants.kZero);
private static final Pose2d kSecondPathInitial = kFirstPathFinal;
private static final Pose2d kSecondPathFinal =
new Pose2d(1.90, 1.1, FieldConstants.kZero);
private static final Pose2d kThirdPathInitial = kSecondPathFinal;
private static final Pose2d kThirdPathWaypoint1 =
new Pose2d(4.1, 0.6, Rotation2d.fromDegrees(30));
private static final Pose2d kThirdPathWaypoint2 =
new Pose2d(5.80, 0.95, Rotation2d.fromDegrees(45));
private static final Pose2d kThirdPathFinal =
new Pose2d(6.25, 1.37, Rotation2d.fromDegrees(45));
private static final Pose2d kFourthPathInitial = kThirdPathFinal;
// private static final Pose2d kFourthPathWaypoint1 = kThirdPathWaypoint2;
// private static final Pose2d kFourthPathWaypoint2 = kThirdPathWaypoint1;
private static final Pose2d kFourthPathFinal =
new Pose2d(5.3, 2.25, FieldConstants.kZero);

public static final PathPlannerTrajectory kFirstTrajectory =
PathPlanner.generatePath(
fastConstraints,
List.of(
fromPose(kFirstPathInitial, FieldConstants.kZero),
// fromPose(kFirstPathWaypoint1, FieldConstants.kZero),
fromPose(kFirstPathFinal, FieldConstants.kZero)));
public static final PathPlannerTrajectory kSecondTrajectory =
PathPlanner.generatePath(
fastConstraints,
List.of(
fromPose(kSecondPathInitial, FieldConstants.kOneEighty),
// fromPose(kFirstPathWaypoint1, FieldConstants.kZero),
fromPose(kSecondPathFinal, FieldConstants.kOneEighty)));
public static final PathPlannerTrajectory kThirdTrajectory =
PathPlanner.generatePath(
defaultConstraints,
List.of(
fromPose(kThirdPathInitial, Rotation2d.fromDegrees(-25)),
fromPose(kThirdPathWaypoint1, FieldConstants.kZero),
fromPose(kThirdPathWaypoint2, Rotation2d.fromDegrees(25)),
fromPose(kThirdPathFinal, Rotation2d.fromDegrees(45))));
public static final PathPlannerTrajectory kFourthTrajectory =
PathPlanner.generatePath(
slowConstraints,
List.of(
fromPose(kFourthPathInitial, Rotation2d.fromDegrees(135)),
// fromPose(kFourthPathWaypoint1,
// Rotation2d.fromDegrees(155)),
// fromPose(kFourthPathWaypoint2,
// FieldConstants.kOneEighty),
fromPose(kFourthPathFinal, FieldConstants.kOneEighty)));
}

public static final class ConeCubeBalanceBumpSide {
public static final Pose2d kFirstPathInitial =
new Pose2d(1.80, 0.5, FieldConstants.kZero);
Expand Down
13 changes: 10 additions & 3 deletions src/main/java/team3647/frc2023/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ public RobotContainer() {
extender.setEncoder(ExtenderConstants.kMinimumPositionTicks);
wrist.setEncoder(WristConstants.kInitialDegree);
cubeWrist.setEncoder(CubeWristConstants.kInitialDegree);
runningMode = autoCommands.redConeTaxiBalance;
runningMode = autoCommands.redConeCubeCubeHoldBalance;
LimelightHelpers.setPipelineIndex(LimelightConstant.kLimelightCenterHost, 1);
swerve.setRobotPose(runningMode.getPathplannerPose2d());
}
Expand Down Expand Up @@ -239,6 +239,8 @@ private void configureButtonBindings() {
goodForLockScore.onTrue(autoDrive.lockRot(180));
goodForLockScore.and(() -> autoDrive.isAlmostDone()).onTrue(autoDrive.lockY());
goodForLockScore.onFalse(autoDrive.unlockRot()).onFalse(autoDrive.unlockY());
goodForLockCube.onTrue(autoDrive.lockRot(180));
goodForLockCube.onFalse(autoDrive.unlockRot());

mainController.leftMidButton.onTrue(autoDrive.disable());
mainController.rightMidButton.onTrue(autoDrive.enable());
Expand Down Expand Up @@ -557,10 +559,15 @@ public Command getAutonomousCommand() {

public final Trigger goodForLockScore =
mainController
.buttonY
.and(mainController.rightTrigger)
.rightTrigger
.and(() -> superstructure.getGamePiece() == GamePiece.Cone)
.and(() -> !superstructure.isBottomF());

public final Trigger goodForLockCube =
mainController
.rightTrigger
.and(() -> superstructure.getGamePiece() == GamePiece.Cube)
.and(() -> !superstructure.isBottomF());
// .and(() -> superstructure.getGamePiece() == GamePiece.Cone)
// .and(() -> superstructure.getWantedLevel() != Level.Ground);

Expand Down
5 changes: 4 additions & 1 deletion src/main/java/team3647/frc2023/util/SuperstructureState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,10 @@ public SuperstructureState noExtend() {
new SuperstructureState(25, 30000, "cube yeeter");

public static final SuperstructureState untipReverse =
new SuperstructureState(-5, 0, 30, "untip");
new SuperstructureState(-5, ExtenderConstants.kMinimumPositionTicks, 30, "untip");

public static final SuperstructureState backStow =
new SuperstructureState(20, 0, 30, "stow back");

public static final SuperstructureState lowCG = new SuperstructureState(160, 0, 104, "low cg");

Expand Down

0 comments on commit ee8cc6a

Please sign in to comment.