post tidal

MillenniumFalcons · Oct 24, 2023 · ccc062d · ccc062d
1 parent 884e000
commit ccc062d
Show file tree

Hide file tree

Showing 6 changed files with 45 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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");