diff --git a/src/main/java/team3647/frc2023/auto/AutoCommands.java b/src/main/java/team3647/frc2023/auto/AutoCommands.java index af97d03..927118e 100644 --- a/src/main/java/team3647/frc2023/auto/AutoCommands.java +++ b/src/main/java/team3647/frc2023/auto/AutoCommands.java @@ -363,7 +363,7 @@ private Command getSuperstrcutreSequenceConeBalance() { .goToStateParallel(SuperstructureState.pushDownStation) .withTimeout(2), Commands.waitSeconds(1), - superstructure.stow()); + superstructure.stowScore()); } private Command getSupeStructureSequenceConeTaxiBalance() { @@ -870,8 +870,8 @@ public Command coneBalance() { // Trajectories.Blue.coneCubeBalance.kFirstTrajectory, // color)) // .until(() -> (drive.getPitch() < -0.1)), - driveForward().until(() -> (drive.getPitch() < -13)), - driveForward().until(() -> (drive.getPitch() > -13)), + driveForwardSlwoly().until(() -> (drive.getPitch() < -13)), + driveForwardSlwoly().until(() -> (drive.getPitch() > -13)), // rotateAndBalance(SwerveDriveConstants.kAutoSteerHeadingController), lock()); return Commands.parallel(drivetrainSequence, getSuperstrcutreSequenceConeBalance()); diff --git a/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java b/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java index 04679d7..d0a6440 100644 --- a/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java +++ b/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java @@ -60,9 +60,12 @@ public Command driveVisionTeleop( var motionTurnComponent = -turnSpeedFunction.getAsDouble() * maxRotationRadpS * triggerSlow; - motionYComponent = lockY ? supplier.get().dy : motionYComponent; + motionYComponent = + lockY ? supplier.get().dy + 0.1 * motionYComponent : motionYComponent; motionTurnComponent = - lockRotation ? supplier.get().dtheta : motionTurnComponent; + lockRotation + ? supplier.get().dtheta + 0.1 * motionTurnComponent + : motionTurnComponent; boolean correct = shouldCorrect.getAsBoolean(); Translation2d translation = new Translation2d(motionXComponent, motionYComponent); diff --git a/src/main/java/team3647/frc2023/constants/WristConstants.java b/src/main/java/team3647/frc2023/constants/WristConstants.java index fa87ad9..be4a816 100644 --- a/src/main/java/team3647/frc2023/constants/WristConstants.java +++ b/src/main/java/team3647/frc2023/constants/WristConstants.java @@ -8,7 +8,7 @@ public final class WristConstants { public static final TalonFX kMaster = new TalonFX(GlobalConstants.WristIds.kMasterId); // public static final InvertType kMasterInvert = InvertType.InvertMotorOutput; - private static final double kGearBoxRatio = 1.0 / 200; + private static final double kGearBoxRatio = 1.0 / 70; private static final TalonFXConfiguration kMasterConfig = new TalonFXConfiguration(); public static final double kNativePosToDegrees = kGearBoxRatio * 360.0; diff --git a/src/main/java/team3647/frc2023/robot/RobotContainer.java b/src/main/java/team3647/frc2023/robot/RobotContainer.java index 5425003..4fbbfab 100644 --- a/src/main/java/team3647/frc2023/robot/RobotContainer.java +++ b/src/main/java/team3647/frc2023/robot/RobotContainer.java @@ -83,7 +83,7 @@ public RobotContainer() { extender.setEncoder(ExtenderConstants.kMinimumPositionTicks); wrist.setEncoder(WristConstants.kInitialDegree); cubeWrist.setEncoder(CubeWristConstants.kInitialDegree); - runningMode = autoCommands.redConeTaxiBalance; + runningMode = autoCommands.redConeCubeCubeBumpSideNoBump; LimelightHelpers.setPipelineIndex(LimelightConstant.kLimelightCenterHost, 1); swerve.setRobotPose(runningMode.getPathplannerPose2d()); } @@ -164,9 +164,16 @@ private void configureButtonBindings() { LEDS.setLEDState(LEDStates.IDLE); })); + mainController + .rightTrigger + .and(level23Cube) + .onTrue( + Commands.waitUntil(() -> autoDrive.doneRotatingGruond()) + .andThen(superstructure.shootAutomatic())); mainController .rightTrigger .and(superstructure::isBottomF) + .and(() -> !level23Cube.getAsBoolean()) .onTrue(superstructure.shootAutomatic()); mainController.buttonA.whileTrue(superstructure.intakeForCurrentGamePiece()); @@ -234,13 +241,20 @@ private void configureButtonBindings() { // coController.rightMidButton.onTrue(superstructure.enableAutoSteer()); // coController.leftMidButton.onTrue(superstructure.disableAutoSteer()); - goodForLockIntake.onTrue(autoDrive.lockRot(0)); - goodForLockIntake.onFalse(autoDrive.unlockRot().withTimeout(0.1)); + goodForLockIntake + .and(() -> superstructure.getWantedStation() == StationType.Double) + .onTrue(autoDrive.lockRot(0)); + goodForLockIntake + .and(() -> superstructure.getWantedStation() == StationType.Ground) + .onTrue(autoDrive.lockRot(180)); + goodForLockIntake.onFalse(autoDrive.unlockRot()); 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()); + level23Cube.onTrue(autoDrive.lockRot(90)); + level23Cube.onFalse(autoDrive.unlockRot()); coController.leftMidButton.onTrue(autoDrive.disable()); coController.rightMidButton.onTrue(autoDrive.enable()); @@ -547,6 +561,17 @@ public Command getAutonomousCommand() { new Trigger(limelightTriggers.isAligned) .or(() -> superstructure.getCurrentIntakePiece() == GamePiece.Cube); + private final Trigger level23Cube = + mainController + .rightTrigger + .and(superstructure::isBottomF) + .and( + () -> + superstructure.isBottomF() + && (superstructure.getWantedLevel() == Level.Two + || superstructure.getWantedLevel() + == Level.Three)); + private final BooleanSupplier goodForAutosteer = globalEnableAutosteer .and(mainController.rightTrigger) @@ -560,6 +585,7 @@ public Command getAutonomousCommand() { public final Trigger goodForLockScore = mainController .rightTrigger + .and(mainController.buttonY) .and(() -> superstructure.getGamePiece() == GamePiece.Cone) .and(() -> !superstructure.isBottomF()); diff --git a/src/main/java/team3647/frc2023/util/AutoDrive.java b/src/main/java/team3647/frc2023/util/AutoDrive.java index dfb5abb..80ba439 100644 --- a/src/main/java/team3647/frc2023/util/AutoDrive.java +++ b/src/main/java/team3647/frc2023/util/AutoDrive.java @@ -46,6 +46,11 @@ public Command lockY() { return Commands.runOnce(() -> this.lockY = true); } + public boolean doneRotatingGruond() { + return Math.abs((swerve.getHeading() % 360) - 90) < 1 + || Math.abs((swerve.getHeading() % 360) + 270) < 1; + } + public Command unlockY() { return Commands.runOnce(() -> this.lockY = false); } diff --git a/src/main/java/team3647/frc2023/util/SuperstructureState.java b/src/main/java/team3647/frc2023/util/SuperstructureState.java index 6e3f469..e6464fb 100644 --- a/src/main/java/team3647/frc2023/util/SuperstructureState.java +++ b/src/main/java/team3647/frc2023/util/SuperstructureState.java @@ -131,9 +131,9 @@ public SuperstructureState noExtend() { new SuperstructureState(184, 57500, 43, "ground intake long"); public static final SuperstructureState doubleStationCone = - new SuperstructureState(120, 17700, 103, "double station"); + new SuperstructureState(119, 18400, 103, "double station"); public static final SuperstructureState doubleStationConeLying = - new SuperstructureState(139, 3000, 51, "double station lying"); + new SuperstructureState(74, 3700, -58, "double station lying"); public static final SuperstructureState doubleStationCube = new SuperstructureState(120, 16000, 117, "double station");