Add demo mode controls (#267)

* Add all demo mode controls * Fix half stow override and force TeleopDriveController max velocity to 1 when not in demo mode * Fixes --------- Co-authored-by: nharnwal <[email protected]> Co-authored-by: Jonah <[email protected]>
Mechanical-Advantage · Jun 13, 2024 · 7bbcb66 · 7bbcb66
1 parent 907ba57
commit 7bbcb66
Show file tree

Hide file tree

Showing 9 changed files with 278 additions and 118 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java
@@ -229,6 +229,7 @@ public void robotPeriodic() {
         superPoopParameters.flywheelSpeeds().rightSpeed());
 
     // Robot container periodic methods
+    robotContainer.updateDemoControls();
     robotContainer.checkControllers();
     robotContainer.updateDashboardOutputs();
     robotContainer.updateAlerts();

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -52,6 +52,8 @@ public record AimingParameters(
       double effectiveDistance,
       FlywheelSpeeds flywheelSpeeds) {}
 
+  public record DemoShotParameters(Rotation2d armAngle, FlywheelSpeeds flywheelSpeeds) {}
+
   private static final LoggedTunableNumber autoLookahead =
       new LoggedTunableNumber("RobotState/AutoLookahead", 0.5);
   private static final LoggedTunableNumber lookahead =
@@ -129,6 +131,10 @@ public static RobotState getInstance() {
 
   private AimingParameters latestSuperPoopParameters = null;
 
+  @Setter @Getter
+  private DemoShotParameters demoShotParameters =
+      new DemoShotParameters(Rotation2d.fromDegrees(0.0), new FlywheelSpeeds(0.0, 0.0));
+
   @Setter private BooleanSupplier lookaheadDisable = () -> false;
 
   private RobotState() {

diff --git a/...ava/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java b/...ava/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
+import lombok.Setter;
 import org.littletonrobotics.frc2024.RobotState;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 
@@ -26,6 +27,8 @@ public class TeleopDriveController {
   private static final LoggedTunableNumber maxAngularVelocityScalar =
       new LoggedTunableNumber("TeleopDrive/MaxAngularVelocityScalar", 0.65);
 
+  @Setter private static double velocityScalar = 1.0;
+
   private double controllerX = 0;
   private double controllerY = 0;
   private double controllerOmega = 0;
@@ -56,21 +59,22 @@ public ChassisSpeeds update() {
     double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get());
     omega = Math.copySign(omega * omega, omega);
 
+    final double maxLinearVelocity = driveConfig.maxLinearVelocity() * velocityScalar;
     final double maxAngularVelocity =
-        driveConfig.maxAngularVelocity() * maxAngularVelocityScalar.get();
+        driveConfig.maxAngularVelocity() * (maxAngularVelocityScalar.get() * velocityScalar);
     if (robotRelative) {
       return new ChassisSpeeds(
-          linearVelocity.getX() * driveConfig.maxLinearVelocity(),
-          linearVelocity.getY() * driveConfig.maxLinearVelocity(),
+          linearVelocity.getX() * maxLinearVelocity,
+          linearVelocity.getY() * maxLinearVelocity,
           omega * maxAngularVelocity);
     } else {
       if (DriverStation.getAlliance().isPresent()
           && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
         linearVelocity = linearVelocity.rotateBy(Rotation2d.fromRadians(Math.PI));
       }
       return ChassisSpeeds.fromFieldRelativeSpeeds(
-          linearVelocity.getX() * driveConfig.maxLinearVelocity(),
-          linearVelocity.getY() * driveConfig.maxLinearVelocity(),
+          linearVelocity.getX() * maxLinearVelocity,
+          linearVelocity.getY() * maxLinearVelocity,
           omega * maxAngularVelocity,
           RobotState.getInstance().getEstimatedPose().getRotation());
     }

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java
@@ -41,6 +41,8 @@ public class Flywheels extends SubsystemBase {
       new LoggedTunableNumber("Flywheels/PrepareShootMultiplier", 1.0);
   private static final LoggedTunableNumber intakingRpm =
       new LoggedTunableNumber("Flywheels/IntakingRpm", -3000.0);
+  private static final LoggedTunableNumber demoIntakingRpm =
+      new LoggedTunableNumber("Flywheels/DemoIntakingRpm", -250.0);
   private static final LoggedTunableNumber ejectingRpm =
       new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0);
   private static final LoggedTunableNumber poopingRpm =
@@ -70,12 +72,16 @@ public enum Goal {
     IDLE(() -> 0.0, () -> 0.0),
     SHOOT(shootingLeftRpm, shootingRightRpm),
     INTAKE(intakingRpm, intakingRpm),
+    DEMO_INTAKE(demoIntakingRpm, demoIntakingRpm),
     EJECT(ejectingRpm, ejectingRpm),
     POOP(poopingRpm, poopingRpm),
     SUPER_POOP(
         () -> RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().leftSpeed(),
         () ->
             RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().rightSpeed()),
+    DEMO_SHOT(
+        () -> RobotState.getInstance().getDemoShotParameters().flywheelSpeeds().leftSpeed(),
+        () -> RobotState.getInstance().getDemoShotParameters().flywheelSpeeds().rightSpeed()),
     CHARACTERIZING(() -> 0.0, () -> 0.0);
 
     private final DoubleSupplier leftGoal;
@@ -226,6 +232,11 @@ public Command intakeCommand() {
         .withName("Flywheels Intake");
   }
 
+  public Command demoIntakeCommand() {
+    return startEnd(() -> setGoal(Goal.DEMO_INTAKE), () -> setGoal(Goal.IDLE))
+        .withName("Flywheels Demo Intake");
+  }
+
   public Command ejectCommand() {
     return startEnd(() -> setGoal(Goal.EJECT), () -> setGoal(Goal.IDLE))
         .withName("Flywheels Eject");
@@ -239,4 +250,9 @@ public Command superPoopCommand() {
     return startEnd(() -> setGoal(Goal.SUPER_POOP), () -> setGoal(Goal.IDLE))
         .withName("Flywheels Super Poop");
   }
+
+  public Command demoShootCommand() {
+    return startEnd(() -> setGoal(Goal.DEMO_SHOT), () -> setGoal(Goal.IDLE))
+        .withName("Flywheels Demo Shoot");
+  }
 }
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
@@ -30,6 +30,10 @@
 public class Rollers extends SubsystemBase {
   private static final LoggedTunableNumber jackhammerTime =
       new LoggedTunableNumber("Rollers/JackhammerTime", 0.075);
+  private static final LoggedTunableNumber stationIntakeTime =
+      new LoggedTunableNumber("Rollers/StationIntakeTime", 0.06);
+  private static final LoggedTunableNumber demoStationIntakeTime =
+      new LoggedTunableNumber("Rollers/DemoStationIntakeTime", 0.1);
 
   private final Feeder feeder;
   private final Indexer indexer;
@@ -44,6 +48,7 @@ public enum Goal {
     IDLE,
     FLOOR_INTAKE,
     STATION_INTAKE,
+    DEMO_STATION_INTAKE,
     EJECT_TO_FLOOR,
     UNJAM_UNTACO,
     UNJAM_FEEDER,
@@ -131,7 +136,16 @@ public void periodic() {
         }
       }
       case STATION_INTAKE -> {
-        if (gamepieceState != GamepieceState.NONE && gamepieceStateTimer.hasElapsed(0.06)) {
+        if (gamepieceState != GamepieceState.NONE
+            && gamepieceStateTimer.hasElapsed(stationIntakeTime.get())) {
+          indexer.setGoal(Indexer.Goal.IDLING);
+        } else {
+          indexer.setGoal(Indexer.Goal.STATION_INTAKING);
+        }
+      }
+      case DEMO_STATION_INTAKE -> {
+        if (gamepieceState != GamepieceState.NONE
+            && gamepieceStateTimer.hasElapsed(demoStationIntakeTime.get())) {
           indexer.setGoal(Indexer.Goal.IDLING);
         } else {
           indexer.setGoal(Indexer.Goal.STATION_INTAKING);

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
@@ -31,6 +31,7 @@ public enum Goal {
     BACKPACK_OUT_UNJAM,
     AIM,
     SUPER_POOP,
+    DEMO_SHOT,
     UNJAM_FEEDER,
     STATION_INTAKE,
     AMP,
@@ -116,6 +117,11 @@ public void periodic() {
         climber.setGoal(Climber.Goal.IDLE);
         backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
       }
+      case DEMO_SHOT -> {
+        arm.setGoal(Arm.Goal.DEMO_SHOT);
+        climber.setGoal(Climber.Goal.IDLE);
+        backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
+      }
       case STATION_INTAKE -> {
         arm.setGoal(Arm.Goal.STATION_INTAKE);
         climber.setGoal(Climber.Goal.IDLE);

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
@@ -81,6 +81,7 @@ public enum Goal {
     AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()),
     SUPER_POOP(
         () -> RobotState.getInstance().getSuperPoopAimingParameters().armAngle().getDegrees()),
+    DEMO_SHOT(() -> RobotState.getInstance().getDemoShotParameters().armAngle().getDegrees()),
     AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)),
     SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)),
     PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 34.0)),
@@ -125,6 +126,7 @@ private double getRads() {
 
   private BooleanSupplier disableSupplier = DriverStation::isDisabled;
   private BooleanSupplier coastSupplier = () -> false;
+  private BooleanSupplier halfStowSupplier = () -> true;
   private boolean brakeModeEnabled = true;
 
   private boolean wasNotAuto = false;
@@ -146,13 +148,19 @@ public Arm(ArmIO io) {
     goalVisualizer = new ArmVisualizer("Goal", Color.kBlue);
   }
 
-  public void setOverrides(BooleanSupplier disableOverride, BooleanSupplier coastOverride) {
+  public void setOverrides(
+      BooleanSupplier disableOverride,
+      BooleanSupplier coastOverride,
+      BooleanSupplier halfStowOverride) {
     disableSupplier = () -> disableOverride.getAsBoolean() || DriverStation.isDisabled();
     coastSupplier = coastOverride;
+    halfStowSupplier = halfStowOverride;
   }
 
   private double getStowAngle() {
-    if (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()) {
+    if (DriverStation.isTeleopEnabled()
+        && RobotState.getInstance().inCloseShootingZone()
+        && !halfStowSupplier.getAsBoolean()) {
       return MathUtil.clamp(
           setpointState.position,
           minAngle.getRadians(),