Fixes

Mechanical-Advantage · Jun 13, 2024 · 840db8e · 840db8e
1 parent 529c9c6
commit 840db8e
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 53 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -326,7 +327,7 @@ public RobotContainer() {
 
     // Configure autos and buttons
     configureAutos();
-    configureButtonBindings(demoControls.get());
+    configureButtonBindings(false);
 
     // Alerts for constants
     if (Constants.tuningMode) {
@@ -449,11 +450,11 @@ private void configureAutos() {
         "Diagnose Arm", superstructure.setGoalCommand(Superstructure.Goal.DIAGNOSTIC_ARM));
 
     // Add options to demo mode
-    demoControls.addDefaultOption("NO", false);
     demoControls.addOption("YES", true);
+    demoControls.addDefaultOption("NO", false);
 
     // Set speed scalar chooser for demo
-    demoSpeedChooser.addDefaultOption("Competition (100%)", 1.0);
+    demoSpeedChooser.addDefaultOption("Full (100%)", 1.0);
     demoSpeedChooser.addOption("Fast (70%)", 0.7);
     demoSpeedChooser.addOption("Medium (30%)", 0.3);
     demoSpeedChooser.addOption("Slow (15%)", 0.15);
@@ -462,19 +463,19 @@ private void configureAutos() {
     demoShotChooser.addDefaultOption(
         "Short",
         new RobotState.DemoShotParameters(
-            Rotation2d.fromDegrees(20.0), new RobotState.FlywheelSpeeds(900, 1500)));
+            Rotation2d.fromDegrees(35.0), new RobotState.FlywheelSpeeds(1200, 1200)));
     demoShotChooser.addOption(
         "Medium",
         new RobotState.DemoShotParameters(
-            Rotation2d.fromDegrees(35.0), new RobotState.FlywheelSpeeds(2600, 4000)));
+            Rotation2d.fromDegrees(35.0), new RobotState.FlywheelSpeeds(3500, 4000)));
     demoShotChooser.addOption(
         "Long",
         new RobotState.DemoShotParameters(
-            Rotation2d.fromDegrees(40.0), new RobotState.FlywheelSpeeds(2400, 4000)));
+            Rotation2d.fromDegrees(40.0), new RobotState.FlywheelSpeeds(5000, 8000)));
     demoShotChooser.addOption(
         "Tall",
         new RobotState.DemoShotParameters(
-            Rotation2d.fromDegrees(65.0), new RobotState.FlywheelSpeeds(2400, 4000)));
+            Rotation2d.fromDegrees(80.0), new RobotState.FlywheelSpeeds(5000, 8000)));
   }
 
   /**
@@ -485,21 +486,6 @@ private void configureAutos() {
   private void configureButtonBindings(boolean demo) {
     CommandScheduler.getInstance().getActiveButtonLoop().clear();
 
-    // Drive with joysticks speed scalar
-    if (demo) {
-      Container<Double> previousDemoSpeed = new Container<>();
-      previousDemoSpeed.value = 1.0;
-      new Trigger(() -> demoSpeedChooser.get() != previousDemoSpeed.value)
-          .onTrue(
-              Commands.runOnce(
-                  () -> {
-                    TeleopDriveController.setVelocityScalar(demoSpeedChooser.get());
-                    previousDemoSpeed.value = demoSpeedChooser.get();
-                  }));
-    } else {
-      TeleopDriveController.setVelocityScalar(1.0);
-    }
-
     // ------------- Driver Controls -------------
     drive.setDefaultCommand(
         drive
@@ -592,6 +578,18 @@ private void configureButtonBindings(boolean demo) {
                   .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming));
 
       driver.a().and(readyToShoot).whileTrue(controllerRumbleCommand());
+
+      // Poop.
+      driver
+          .rightTrigger()
+          .and(driver.a().negate())
+          .and(driver.b().negate())
+          .whileTrue(
+              flywheels
+                  .poopCommand()
+                  .alongWith(
+                      Commands.waitUntil(flywheels::atGoal)
+                          .andThen(rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER))));
     } else {
       new Trigger(DriverStation::isEnabled)
           .whileTrueContinuous(
@@ -611,24 +609,13 @@ private void configureButtonBindings(boolean demo) {
           .onTrue(
               rollers
                   .setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER)
+                  .alongWith(
+                      superstructure.setGoalWithConstraintsCommand(
+                          Superstructure.Goal.DEMO_SHOT, Arm.smoothProfileConstraints.get()),
+                      flywheels.demoShootCommand())
                   .withTimeout(0.6)
-                  .deadlineWith(
-                      superstructure
-                          .setGoalWithConstraintsCommand(
-                              Superstructure.Goal.DEMO_SHOT, Arm.smoothProfileConstraints.get())
-                          .alongWith(flywheels.demoShootCommand())));
+                  .withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
     }
-    // Poop.
-    driver
-        .rightTrigger()
-        .and(driver.a().negate())
-        .and(driver.b().negate())
-        .whileTrue(
-            flywheels
-                .poopCommand()
-                .alongWith(
-                    Commands.waitUntil(flywheels::atGoal)
-                        .andThen(rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER))));
 
     // ------------- Intake Controls -------------
     // Intake Floor
@@ -670,7 +657,7 @@ private void configureButtonBindings(boolean demo) {
           .whileTrue(
               superstructure
                   .setGoalWithConstraintsCommand(
-                      Superstructure.Goal.STATION_INTAKE, Arm.smoothProfileConstraints.get())
+                      Superstructure.Goal.STOW, Arm.smoothProfileConstraints.get())
                   .alongWith(
                       rollers.setGoalCommand(Rollers.Goal.DEMO_STATION_INTAKE),
                       flywheels.demoIntakeCommand())
@@ -751,18 +738,20 @@ private void configureButtonBindings(boolean demo) {
                     ampAutoDrive,
 
                     // Drive while heading is being controlled
-                    drive
-                        .run(
-                            () ->
-                                drive.acceptTeleopInput(
-                                    -driver.getLeftY(),
-                                    -driver.getLeftX(),
-                                    0.0,
-                                    robotRelative.getAsBoolean()))
-                        .alongWith(
-                            Commands.startEnd(
-                                () -> drive.setHeadingGoal(() -> Rotation2d.fromDegrees(-90.0)),
-                                drive::clearHeadingGoal)),
+                    demo
+                        ? Commands.none()
+                        : drive
+                            .run(
+                                () ->
+                                    drive.acceptTeleopInput(
+                                        -driver.getLeftY(),
+                                        -driver.getLeftX(),
+                                        0.0,
+                                        robotRelative.getAsBoolean()))
+                            .alongWith(
+                                Commands.startEnd(
+                                    () -> drive.setHeadingGoal(() -> Rotation2d.fromDegrees(-90.0)),
+                                    drive::clearHeadingGoal)),
                     autoDriveEnable)
                 .alongWith(
                     Commands.waitUntil(
@@ -896,6 +885,9 @@ public void updateDemoControls() {
       lastWasDemoControls = demoControls.get();
     }
 
+    // Update teleop speed
+    TeleopDriveController.setVelocityScalar(demoControls.get() ? demoSpeedChooser.get() : 1.0);
+
     // Update alerts
     Leds.getInstance().demoMode = demoControls.get();
     demoControlsActivated.set(demoControls.get());

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java
@@ -42,7 +42,7 @@ public class Flywheels extends SubsystemBase {
   private static final LoggedTunableNumber intakingRpm =
       new LoggedTunableNumber("Flywheels/IntakingRpm", -3000.0);
   private static final LoggedTunableNumber demoIntakingRpm =
-      new LoggedTunableNumber("Flywheels/DemoIntakingRpm", -1000.0);
+      new LoggedTunableNumber("Flywheels/DemoIntakingRpm", -250.0);
   private static final LoggedTunableNumber ejectingRpm =
       new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0);
   private static final LoggedTunableNumber poopingRpm =

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java
@@ -180,6 +180,8 @@ public synchronized void periodic() {
         rainbow(rainbowCycleLength, rainbowDuration);
       } else if (hasNote) {
         solid(Color.kGreen);
+      } else if (demoMode) {
+        wave(allianceColor, secondaryDisabledColor, waveAllianceCycleLength, waveAllianceDuration);
       }
 
       if (endgameAlert) {

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java
@@ -33,7 +33,7 @@ public class Rollers extends SubsystemBase {
   private static final LoggedTunableNumber stationIntakeTime =
       new LoggedTunableNumber("Rollers/StationIntakeTime", 0.06);
   private static final LoggedTunableNumber demoStationIntakeTime =
-      new LoggedTunableNumber("Rollers/StationIntakeTime", 0.06);
+      new LoggedTunableNumber("Rollers/DemoStationIntakeTime", 0.1);
 
   private final Feeder feeder;
   private final Indexer indexer;