Merge branch 'dcmp-autos' into davis-unethical-auto

.gitignore

@@ -178,4 +178,7 @@ networktables.json
 src/main/java/org/littletonrobotics/frc2024/BuildConstants.java
 
 # Generated paths
-src/main/deploy/trajectories/*.pathblob
+src/main/deploy/trajectories/*.pathblob
+
+# Shot map temporary file
+~*.xlsx

build.gradle

@@ -39,7 +39,6 @@ sourceSets {
     }
 }
 
-
 java {
     sourceCompatibility = JavaVersion.VERSION_17
     targetCompatibility = JavaVersion.VERSION_17
@@ -69,6 +68,7 @@ deploy {
 
                 // Static files artifact
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    predeploy << { execute 'rm -rf /home/lvuser/deploy' }
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
                 }

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

@@ -73,6 +73,11 @@ public Command getCommand() {
     return lastRoutine.command();
   }
 
+  /** Returns the name of the selected routine. */
+  public String getSelectedName() {
+    return lastRoutine.name();
+  }
+
   /** Returns the selected question responses. */
   public List<AutoQuestionResponse> getResponses() {
     return lastResponses;

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

@@ -220,7 +220,7 @@ public void robotPeriodic() {
     // Robot container periodic methods
     robotContainer.checkControllers();
     robotContainer.updateDashboardOutputs();
-    robotContainer.updateAprilTagAlert();
+    robotContainer.updateAlerts();
 
     // Update NoteVisualizer
     NoteVisualizer.showHeldNotes();

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

@@ -113,6 +113,8 @@ public class RobotContainer {
   private final Trigger autoDriveDisable = overrides.operatorSwitch(3);
   private final Trigger autoFlywheelSpinupDisable = overrides.operatorSwitch(4);
   private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO);
+  private final Alert ridiculousAutoAlert =
+      new Alert("The selected auto is ridiculous! 😡", AlertType.WARNING);
   private final Alert driverDisconnected =
       new Alert("Driver controller disconnected (port 0).", AlertType.WARNING);
   private final Alert operatorDisconnected =
@@ -311,12 +313,7 @@ public RobotContainer() {
                                 FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d()))
                     < Units.feetToMeters(25.0)
                 && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED
-                && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.POST_PREPARE_TRAP_CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.TRAP
-                && superstructure.getCurrentGoal() != Superstructure.Goal.UNTRAP);
+                && !superstructure.getCurrentGoal().isClimbingGoal());
 
     // Configure autos and buttons
     configureAutos();
@@ -390,8 +387,6 @@ private void configureAutos() {
                 "End behavior?",
                 List.of(AutoQuestionResponse.SCORE_POOPED, AutoQuestionResponse.FOURTH_CENTER))),
         autoBuilder.davisSpeedyAuto());
-    autoSelector.addRoutine(
-        "Davis Alternative Speedy Auto", autoBuilder.davisAlternativeSpeedyAuto());
     autoSelector.addRoutine("Davis Ethical Auto", autoBuilder.davisEthicalAuto());
     autoSelector.addRoutine(
         "Davis Unethical Auto",
@@ -814,14 +809,20 @@ public void updateDashboardOutputs() {
     SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
   }
 
-  /** Updates the AprilTag alert. */
-  public void updateAprilTagAlert() {
-    boolean active = getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL;
-    aprilTagLayoutAlert.set(active);
-    if (active) {
+  /** Updates the alerts. */
+  public void updateAlerts() {
+    // AprilTag layout alert
+    boolean aprilTagAlertActive = getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL;
+    aprilTagLayoutAlert.set(aprilTagAlertActive);
+    if (aprilTagAlertActive) {
       aprilTagLayoutAlert.setText(
           "Non-official AprilTag layout in use (" + getAprilTagLayoutType().toString() + ").");
     }
+
+    // Ridiculous auto alert
+    ridiculousAutoAlert.set(
+        autoSelector.getSelectedName().equals("Davis Spiky Auto")
+            && autoSelector.getResponses().get(2) == autoSelector.getResponses().get(3));
   }
 
   /**

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

@@ -11,7 +11,6 @@
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.*;
-import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
@@ -31,6 +30,7 @@
 import org.littletonrobotics.frc2024.util.NoteVisualizer;
 import org.littletonrobotics.frc2024.util.swerve.ModuleLimits;
 import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
 
 @ExtensionMethod({GeomUtil.class})
 public class RobotState {
@@ -45,34 +45,16 @@ public record AimingParameters(
       double effectiveDistance,
       double driveFeedVelocity) {}
 
+  private static final LoggedTunableNumber autoLookahead =
+      new LoggedTunableNumber("RobotState/AutoLookahead", 0.5);
   private static final LoggedTunableNumber lookahead =
       new LoggedTunableNumber("RobotState/lookaheadS", 0.35);
   private static final double poseBufferSizeSeconds = 2.0;
 
-  /** Arm angle look up table key: meters, values: degrees */
-  private static final InterpolatingDoubleTreeMap armAngleMap = new InterpolatingDoubleTreeMap();
+  private static final double armAngleCoefficient = 57.254371165197;
+  private static final double armAngleExponent = -0.593140189605718;
 
   @AutoLogOutput @Getter @Setter private boolean flywheelAccelerating = false;
-
-  static {
-    armAngleMap.put(1.04, 55.0);
-    armAngleMap.put(1.25, 52.0);
-    armAngleMap.put(1.5, 46.0);
-    armAngleMap.put(1.75, 42.0);
-    armAngleMap.put(2.0, 40.0);
-    armAngleMap.put(2.25, 37.5);
-    armAngleMap.put(2.5, 35.5);
-    armAngleMap.put(2.75, 33.25);
-    armAngleMap.put(2.94, 32.15);
-    armAngleMap.put(3.15, 30.65);
-    armAngleMap.put(3.55, 28.75);
-    armAngleMap.put(3.75, 28.1);
-    armAngleMap.put(4.0, 27.75);
-    armAngleMap.put(4.25, 26.8);
-    armAngleMap.put(4.5, 25.6);
-    armAngleMap.put(8.0, 8.8); // Added in with slope of previous two points to make a best guess
-  }
-
   @AutoLogOutput @Getter @Setter private double shotCompensationDegrees = 0.0;
 
   public void adjustShotCompensationDegrees(double deltaDegrees) {
@@ -105,6 +87,7 @@ public static RobotState getInstance() {
           });
   private Rotation2d lastGyroAngle = new Rotation2d();
   private Twist2d robotVelocity = new Twist2d();
+  private Twist2d trajectoryVelocity = new Twist2d();
 
   /** Cached latest aiming parameters. Calculated in {@code getAimingParameters()} */
   private AimingParameters latestParameters = null;
@@ -205,6 +188,13 @@ public void addVelocityData(Twist2d robotVelocity) {
     this.robotVelocity = robotVelocity;
   }
 
+  public void addTrajectoryVelocityData(Twist2d robotVelocity) {
+    if (DriverStation.isAutonomousEnabled()) {
+      latestParameters = null;
+      trajectoryVelocity = robotVelocity;
+    }
+  }
+
   public AimingParameters getAimingParameters() {
     if (latestParameters != null) {
       // Cache previously calculated aiming parameters. Cache is invalidated whenever new
@@ -217,10 +207,18 @@ public AimingParameters getAimingParameters() {
             .toTranslation2d()
             .toTransform2d()
             .plus(FudgeFactors.speaker.getTransform());
-    Pose2d fieldToPredictedVehicle =
-        lookaheadDisable.getAsBoolean() || DriverStation.isAutonomousEnabled()
-            ? getEstimatedPose()
-            : getPredictedPose(lookahead.get(), lookahead.get());
+    Pose2d fieldToPredictedVehicle;
+    if (DriverStation.isAutonomousEnabled()) {
+      fieldToPredictedVehicle = getPredictedPose(autoLookahead.get(), autoLookahead.get());
+
+    } else {
+      fieldToPredictedVehicle =
+          lookaheadDisable.getAsBoolean()
+              ? getEstimatedPose()
+              : getPredictedPose(lookahead.get(), lookahead.get());
+    }
+    Logger.recordOutput("RobotState/AimingParameters/PredictedPose", fieldToPredictedVehicle);
+
     Pose2d fieldToPredictedVehicleFixed =
         new Pose2d(fieldToPredictedVehicle.getTranslation(), new Rotation2d());
 
@@ -238,10 +236,11 @@ public AimingParameters getAimingParameters() {
         robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance
             - robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance;
 
+    double armAngleDegrees = armAngleCoefficient * Math.pow(targetDistance, armAngleExponent);
     latestParameters =
         new AimingParameters(
             targetVehicleDirection,
-            Rotation2d.fromDegrees(armAngleMap.get(targetDistance) + shotCompensationDegrees),
+            Rotation2d.fromDegrees(armAngleDegrees + shotCompensationDegrees),
             targetDistance,
             feedVelocity);
     return latestParameters;
@@ -285,12 +284,13 @@ public Pose2d getEstimatedPose() {
    * @return The predicted pose.
    */
   public Pose2d getPredictedPose(double translationLookaheadS, double rotationLookaheadS) {
+    Twist2d velocity = DriverStation.isAutonomousEnabled() ? trajectoryVelocity : robotVelocity;
     return getEstimatedPose()
-        .exp(
-            new Twist2d(
-                robotVelocity.dx * translationLookaheadS,
-                robotVelocity.dy * translationLookaheadS,
-                robotVelocity.dtheta * rotationLookaheadS));
+        .transformBy(
+            new Transform2d(
+                velocity.dx * translationLookaheadS,
+                velocity.dy * translationLookaheadS,
+                Rotation2d.fromRadians(velocity.dtheta * rotationLookaheadS)));
   }
 
   @AutoLogOutput(key = "RobotState/OdometryPose")