Add demo autos

src/main/java/org/littletonrobotics/frc2024/Robot.java

@@ -227,6 +227,14 @@ public void robotPeriodic() {
     Logger.recordOutput(
         "RobotState/SuperPoopParameters/FlywheelsRightSpeed",
         superPoopParameters.flywheelSpeeds().rightSpeed());
+    var demoParameters = RobotState.getInstance().getDemoTagParameters();
+    demoParameters.ifPresent(
+        parameters -> {
+          Logger.recordOutput("RobotState/DemoParameters/TargetPose", parameters.targetPose());
+          Logger.recordOutput(
+              "RobotState/DemoParameters/TargetHeading", parameters.targetHeading());
+          Logger.recordOutput("RobotState/DemoParameters/ArmAngle", parameters.armAngle());
+        });
 
     // Robot container periodic methods
     robotContainer.checkControllers();

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -31,10 +31,7 @@
 import org.littletonrobotics.frc2024.AutoSelector.AutoQuestion;
 import org.littletonrobotics.frc2024.AutoSelector.AutoQuestionResponse;
 import org.littletonrobotics.frc2024.FieldConstants.AprilTagLayoutType;
-import org.littletonrobotics.frc2024.commands.ClimbingCommands;
-import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization;
-import org.littletonrobotics.frc2024.commands.StaticCharacterization;
-import org.littletonrobotics.frc2024.commands.WheelRadiusCharacterization;
+import org.littletonrobotics.frc2024.commands.*;
 import org.littletonrobotics.frc2024.commands.auto.AutoBuilder;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO;
@@ -402,6 +399,13 @@ private void configureAutos() {
                     AutoQuestionResponse.SIX_SECONDS,
                     AutoQuestionResponse.LAST_SECOND))),
         autoBuilder.davisInspirationalAuto());
+    DemoAutos demoAutos = new DemoAutos(drive, superstructure, autoSelector::getResponses);
+    autoSelector.addRoutine(
+        "Davis Demo Auto",
+        List.of(
+            new AutoQuestion(
+                "Follow Tag?", List.of(AutoQuestionResponse.YES, AutoQuestionResponse.NO))),
+        demoAutos.davisDemoAuto());
 
     // Set up feedforward characterization
     autoSelector.addRoutine(

src/main/java/org/littletonrobotics/frc2024/RobotState.java

@@ -18,11 +18,13 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.NoSuchElementException;
+import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import lombok.Getter;
 import lombok.Setter;
 import lombok.experimental.ExtensionMethod;
 import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
+import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants;
 import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
 import org.littletonrobotics.frc2024.util.GeomUtil;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
@@ -52,6 +54,9 @@ public record AimingParameters(
       double effectiveDistance,
       FlywheelSpeeds flywheelSpeeds) {}
 
+  public record DemoAimingParameters(
+      Pose2d targetPose, Rotation2d targetHeading, Rotation2d armAngle) {}
+
   private static final LoggedTunableNumber autoLookahead =
       new LoggedTunableNumber("RobotState/AutoLookahead", 0.5);
   private static final LoggedTunableNumber lookahead =
@@ -60,6 +65,7 @@ public record AimingParameters(
       new LoggedTunableNumber("RobotState/SuperPoopLookahead", 0.1);
   private static final LoggedTunableNumber closeShootingZoneFeet =
       new LoggedTunableNumber("RobotState/CloseShootingZoneFeet", 10.0);
+
   private static final double poseBufferSizeSeconds = 2.0;
 
   private static final double armAngleCoefficient = 57.254371165197;
@@ -128,6 +134,9 @@ public static RobotState getInstance() {
   private AimingParameters latestParameters = null;
 
   private AimingParameters latestSuperPoopParameters = null;
+  // Demo parameters
+  private Pose3d demoTagPose = null;
+  private DemoAimingParameters latestDemoParamters = null;
 
   @Setter private BooleanSupplier lookaheadDisable = () -> false;
 
@@ -320,6 +329,63 @@ public AimingParameters getSuperPoopAimingParameters() {
     return latestSuperPoopParameters;
   }
 
+  public void setDemoTagPose(Pose3d demoTagPose) {
+    this.demoTagPose = demoTagPose;
+    latestDemoParamters = null;
+  }
+
+  private static final LoggedTunableNumber demoTargetDistance =
+      new LoggedTunableNumber("RobotState/DemoTargetDistance", 2.0);
+  private static final Translation3d upDirection = new Translation3d(0.0, 0.0, 1.0);
+  private static final Translation3d leftDirection = new Translation3d(0.0, 1.0, 0.0);
+  private static final Translation3d downDirection = new Translation3d(0.0, 0.0, -1.0);
+  private static final Translation3d rightDirection = new Translation3d(0.0, -1.0, 0.0);
+
+  public Optional<DemoAimingParameters> getDemoTagParameters() {
+    if (latestDemoParamters != null) {
+      // Use cached demo parameters.
+      return Optional.of(latestDemoParamters);
+    }
+    // Return empty optional if no demo tag pose.
+    if (demoTagPose == null) return Optional.empty();
+
+    // Calculate target pose.
+    // Determine tag rotation
+    double maxZ = 0.0;
+    int maxIndex = 0;
+    int index = 0;
+    for (var tagDirection :
+        new Translation3d[] {upDirection, leftDirection, downDirection, rightDirection}) {
+      double z = demoTagPose.transformBy(new Transform3d(tagDirection, new Rotation3d())).getZ();
+      if (z > maxZ) {
+        maxZ = z;
+        maxIndex = index;
+      }
+      index++;
+    }
+    Rotation2d robotRotation = new Rotation2d(Math.PI + Math.PI / 2.0 * maxIndex);
+    Pose2d targetPose =
+        demoTagPose
+            .toPose2d()
+            .transformBy(
+                new Transform2d(new Translation2d(demoTargetDistance.get(), 0.0), robotRotation));
+
+    // Calculate heading without movement.
+    Translation2d demoTagFixed = demoTagPose.getTranslation().toTranslation2d();
+    Translation2d robotToDemoTagFixed = demoTagFixed.minus(getEstimatedPose().getTranslation());
+    Rotation2d targetHeading = robotToDemoTagFixed.getAngle();
+
+    // Calculate arm angle.
+    double z = demoTagPose.getZ();
+    Rotation2d armAngle =
+        new Rotation2d(
+            robotToDemoTagFixed.getNorm() - ArmConstants.armOrigin.getX(),
+            z - ArmConstants.armOrigin.getY());
+
+    latestDemoParamters = new DemoAimingParameters(targetPose, targetHeading, armAngle);
+    return Optional.of(latestDemoParamters);
+  }
+
   public ModuleLimits getModuleLimits() {
     return flywheelAccelerating && !DriverStation.isAutonomousEnabled()
         ? DriveConstants.moduleLimitsFlywheelSpinup

src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java

@@ -0,0 +1,69 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package org.littletonrobotics.frc2024.commands;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import java.util.List;
+import java.util.function.Supplier;
+import lombok.RequiredArgsConstructor;
+import org.littletonrobotics.frc2024.AutoSelector;
+import org.littletonrobotics.frc2024.RobotState;
+import org.littletonrobotics.frc2024.subsystems.drive.Drive;
+import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
+import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
+
+@RequiredArgsConstructor
+public class DemoAutos {
+  private final Drive drive;
+  private final Superstructure superstructure;
+  private final Supplier<List<AutoSelector.AutoQuestionResponse>> responses;
+
+  public Command davisDemoAuto() {
+    return Commands.either(
+        followTag(),
+        lookAtTag(),
+        () -> responses.get().get(0).equals(AutoSelector.AutoQuestionResponse.YES));
+  }
+
+  private Command followTag() {
+    return drive
+        .startEnd(
+            () ->
+                drive.setAutoAlignGoal(
+                    () ->
+                        RobotState.getInstance()
+                            .getDemoTagParameters()
+                            .map(RobotState.DemoAimingParameters::targetPose)
+                            .orElseGet(() -> RobotState.getInstance().getEstimatedPose()),
+                    Translation2d::new,
+                    false),
+            drive::clearAutoAlignGoal)
+        .alongWith(
+            superstructure.setGoalWithConstraintsCommand(
+                Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get()));
+  }
+
+  private Command lookAtTag() {
+    return drive
+        .startEnd(
+            () ->
+                drive.setHeadingGoal(
+                    () ->
+                        RobotState.getInstance()
+                            .getDemoTagParameters()
+                            .map(RobotState.DemoAimingParameters::targetHeading)
+                            .orElseGet(
+                                () -> RobotState.getInstance().getEstimatedPose().getRotation())),
+            drive::clearAutoAlignGoal)
+        .alongWith(
+            superstructure.setGoalWithConstraintsCommand(
+                Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get()));
+  }
+}