co

MillenniumFalcons · Apr 8, 2024 · ab34cfe · ab34cfe
1 parent 63ee47a
commit ab34cfe
Show file tree

Hide file tree

Showing 6 changed files with 22 additions and 37 deletions.
diff --git a/src/main/java/team3647/frc2024/auto/AutoCommands.java b/src/main/java/team3647/frc2024/auto/AutoCommands.java
@@ -81,6 +81,8 @@ public class AutoCommands {
 
     private final BooleanSupplier hasTarget;
 
+    private final BooleanSupplier noteNotSeen;
+
     private final Supplier<DriveMode> getMode;
 
     //     public final AutonomousMode blueFive_S1N1F1N2N3;
@@ -113,8 +115,6 @@ public class AutoCommands {
 
     public final AutonomousMode redFullCenterS1;
 
-    private boolean hasNote = false;
-
     //     public final AutonomousMode yes;
 
     public AutoCommands(
@@ -131,7 +131,9 @@ public AutoCommands(
         this.hasTarget = hasTarget;
         this.getMode = getMode;
 
-        currentYes = new Trigger(() -> superstructure.currentYes()).debounce(0.04);
+        noteNotSeen = new Trigger(() -> !hasTarget.getAsBoolean()).debounce(1);
+
+        currentYes = new Trigger(() -> superstructure.currentYes()).debounce(0.08);
 
         // this.yes = new AutonomousMode(four_S3N5N4N3(Alliance.Blue), getInitial(s3_to_f5));
 
@@ -193,14 +195,6 @@ public AutoCommands(
 
     public void registerCommands() {}
 
-    public Command setHasNote() {
-        return Commands.runOnce(() -> this.hasNote = true);
-    }
-
-    public Command setHasNoNote() {
-        return Commands.runOnce(() -> this.hasNote = false);
-    }
-
     public AutonomousMode getSix_S1F1F2N1N2N3ByColor(Alliance color) {
         return new AutonomousMode(six_S1F1F2N1N2N3(color), getInitial(s1_to_n1_to_f1));
     }
@@ -288,13 +282,13 @@ public Command fullCenterS1(Alliance color) {
                 Commands.sequence(
                         followChoreoPathWithOverrideFast(s15_to_f1, color),
                         target().withTimeout(0.2),
-                        getScoringSequenceF1F2(() -> this.hasNote, color),
+                        getScoringSequenceF1F2(() -> !noteNotSeen.getAsBoolean(), color),
                         target().withTimeout(0.2),
-                        getScoringSequenceF2F3(() -> this.hasNote, color),
+                        getScoringSequenceF2F3(() -> !noteNotSeen.getAsBoolean(), color),
                         target().withTimeout(0.2),
-                        getScoringSequenceF3F4(() -> this.hasNote, color),
+                        getScoringSequenceF3F4(() -> !noteNotSeen.getAsBoolean(), color),
                         target().withTimeout(0.2),
-                        getScoringSequenceF4F5(() -> this.hasNote, color)));
+                        getScoringSequenceF4F5(() -> !noteNotSeen.getAsBoolean(), color)));
     }
 
     public Command six_S1F1F2N1N2N3(Alliance color) {
@@ -472,12 +466,10 @@ public Command continuouslyIntakeForShoot(Alliance color) {
                 superstructure
                         .intake()
                         .until(currentYes)
-                        .andThen(setHasNote())
                         .andThen(Commands.runOnce(() -> this.hasPiece = true))
                         .andThen(
                                 superstructure.passToShooterNoKicker(
-                                        new Trigger(() -> goodToGo(color))))
-                        .andThen(setHasNoNote()));
+                                        new Trigger(() -> goodToGo(color)))));
     }
 
     public Pose2d flipForPP(Pose2d pose) {

diff --git a/src/main/java/team3647/frc2024/constants/PivotConstants.java b/src/main/java/team3647/frc2024/constants/PivotConstants.java
@@ -33,7 +33,7 @@ public class PivotConstants {
     public static final double kMaxVelocityTicks = (400.0 / kNativeVelToDPS);
     public static final double kMaxAccelerationTicks = (200.0 / kNativeVelToDPS);
 
-    public static final double kMinDegree = 15;
+    public static final double kMinDegree = 12;
     public static final double kMaxDegree = 68.27;
     public static final double kMaxDegreeUnderStage = 30;
 

diff --git a/src/main/java/team3647/frc2024/constants/ShooterConstants.java b/src/main/java/team3647/frc2024/constants/ShooterConstants.java
@@ -31,13 +31,13 @@ public class ShooterConstants {
             kWheelRotationMeters / GlobalConstants.kFalconTicksPerRotation * kGearboxReduction;
 
     // tune ff
-    public static final double kS = 13.132; // 17.729; // 8.7167;
-    public static final double kV = 0.23336; // 0.28947; // 0.24226;
-    public static final double kA = 1.2516; // 0.88966; // 0.60231;
+    public static final double kS = 21.415 / 2; // 17.729; // 8.7167;
+    public static final double kV = 0.7985 / 2; // 0.28947; // 0.24226;
+    public static final double kA = 2.2601 / 2; // 0.88966; // 0.60231;
 
     public static final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA);
 
-    public static final double masterKP = 17.378;
+    public static final double masterKP = 15;
     public static final double masterKI = 0;
     public static final double masterKD = 0;
 

diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java
@@ -278,7 +278,7 @@ public void configureSmartDashboardLogging() {
         SmartDashboard.putNumber("pivot interp angle", 40);
         // SmartDashboard.putNumber("shoot speed", 15);
         // SmartDashboard.putNumber("differential", 1.1);
-        // printer.addDouble("shooter distance", targetingUtil::distance);
+        printer.addDouble("shooter distance", targetingUtil::distance);
         // printer.addBoolean("current sensing", () -> autoCommands.currentYes.getAsBoolean());
         // printer.addBoolean("wrist at stow", superstructure::wristAtStow);
         // printer.addDouble("climb len", () -> climb.getLength());

diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java
@@ -126,7 +126,8 @@ public boolean isClimbing() {
 
     public Command spinUp() {
         return shooterCommands.setVelocityIndep(
-                () -> feedShot.getAsBoolean() ? 15 : 28, () -> feedShot.getAsBoolean() ? 15 : 18);
+                () -> feedShot.getAsBoolean() ? 15 : shooterSpeedSupplier.getAsDouble(),
+                () -> feedShot.getAsBoolean() ? 15 : shooterSpeedSupplier.getAsDouble() * 18 / 28);
     }
 
     public Command spinUpPreload() {

diff --git a/src/main/java/team3647/frc2024/util/AutoDrive.java b/src/main/java/team3647/frc2024/util/AutoDrive.java
@@ -144,18 +144,10 @@ public DriveMode getMode() {
     }
 
     public double getShootSpeed() {
-        if (DriverStation.isAutonomous()) {
-            return targeting.shootAtSpeakerOnTheMove().shootSpeed;
-        }
-        switch (mode) {
-            case SHOOT_ON_THE_MOVE:
-                return targeting.shootAtSpeakerOnTheMove().shootSpeed;
-            case SHOOT_STATIONARY:
-                return targeting.shootAtSpeaker().shootSpeed;
-            case SHOOT_AT_AMP:
-                return targeting.shootAtAmp().shootSpeed;
-            default:
-                return 25;
+        if (targeting.distance() < 7) {
+            return 28;
+        } else {
+            return 36;
         }
     }