Add autoshoot

.vscode/settings.json

@@ -58,6 +58,6 @@
     "edu.wpi.first.math.**.struct.*",
   ],
   "java.dependency.enableDependencyCheckup": false,
-  "java.jdt.ls.vmargs": "-Xmx4G",
+  "java.jdt.ls.vmargs": "-Xmx8G",
   "java.compile.nullAnalysis.mode": "disabled"
 }

build.gradle

@@ -76,6 +76,10 @@ wpi.java.debugJni = false
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
+repositories {
+    mavenCentral()
+}
+
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
 dependencies {

src/main/deploy/all_shooter_hub_data.csv

@@ -0,0 +1,27 @@
+57.5,18,25
+57.5,23,23
+57.5,26,21
+85,18,26.5
+85,28,23.5
+85,23,24.5
+107,18,28
+107,28,24.3
+107,38,23.9
+107,33,23.5
+107,23,25.5
+124,18,28
+124,23,26
+124,28,24
+150,18,32
+150,28,27
+150,38,26.5
+214,18,36
+214,28,30
+214,38,29
+214,23,34
+214,33,29.75
+263,18,42
+263,28,35
+263,38,31.5
+263,23,38.5
+263,33,32.5

src/main/deploy/shooter_hub_data.csv

@@ -0,0 +1,9 @@
+83.75,18,22,0.868
+101.375,22,24,1.004
+114.25,24,24.5,1.012
+128.25,25,25,1.016
+144.25,26,26,1.029
+165.75,26.5,28,1.159
+189.25,27,29,1.212
+221.25,29,31.5,1.289
+235.75,29.5,32,1.318

src/main/deploy/sim_shooter_hub_data.csv

@@ -0,0 +1,5 @@
+50,18,15,0.5
+100,28,20,0.75
+150,38,25,1
+200,43,27,1.25
+250,48,30,1.5

src/main/java/com/team6962/lib/commands/CommandUtil.java

@@ -0,0 +1,22 @@
+package com.team6962.lib.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+/** Utility class for command-related helper functions. */
+public class CommandUtil {
+  /**
+   * Checks if it is okay to replace the currently active command on a subsystem, and have the given
+   * command take control instead. A subsystem is clear to override if it has no current command,
+   * has only its default command running, or is already running the specified command.
+   *
+   * @param subsystem The subsystem to check
+   * @param command The command that wants to take control
+   * @return True if the subsystem's active command can be safely overridden
+   */
+  public static boolean isClearToOverride(Subsystem subsystem, Command command) {
+    return subsystem.getCurrentCommand() == null
+        || subsystem.getCurrentCommand() == subsystem.getDefaultCommand()
+        || subsystem.getCurrentCommand() == command;
+  }
+}

src/main/java/com/team6962/lib/logging/LoggingUtil.java

@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.controls.ControlRequest;
 import dev.doglog.DogLog;
+import edu.wpi.first.wpilibj2.command.Command;
 import java.io.InputStream;
 import java.util.Map;
 import java.util.Optional;
@@ -90,4 +91,23 @@ public static void logGitProperties() {
       System.err.println("Failed to load git properties: " + e.getMessage());
     }
   }
+
+  /**
+   * Wraps the given command with logging that indicates when the command starts and ends. Logs are
+   * written under the path "Commands/{key}" with a boolean value indicating whether the command is
+   * currently running (true) or not (false).
+   *
+   * @param key A unique key to identify the command in the logs. This should be a descriptive name
+   *     for the command being logged (e.g., "Shoot", "DriveToClump", etc.).
+   * @param command The Command to wrap with logging.
+   * @return A new Command that behaves the same as the input command but also logs its execution
+   *     status under "Commands/{key}".
+   */
+  public static Command logCommand(String key, Command command) {
+    DogLog.log("Commands/" + key, false);
+
+    return command
+        .beforeStarting(() -> DogLog.log("Commands/" + key, true))
+        .finallyDo(() -> DogLog.log("Commands/" + key, false));
+  }
 }

src/main/java/com/team6962/lib/math/ConstantFunction.java

@@ -0,0 +1,22 @@
+package com.team6962.lib.math;
+
+import org.apache.commons.math3.analysis.MultivariateFunction;
+import org.apache.commons.math3.analysis.UnivariateFunction;
+
+public class ConstantFunction implements UnivariateFunction, MultivariateFunction {
+  private final double value;
+
+  public ConstantFunction(double value) {
+    this.value = value;
+  }
+
+  @Override
+  public double value(double x) {
+    return value;
+  }
+
+  @Override
+  public double value(double[] point) {
+    return value;
+  }
+}

src/main/java/com/team6962/lib/math/TranslationalVelocity.java

@@ -158,6 +158,19 @@ public TranslationalVelocity times(double scalar) {
         MetersPerSecond.of(this.y.in(MetersPerSecond) * scalar));
   }
 
+  /**
+   * Multiplies this TranslationalVelocity by a Time measure and returns the resulting
+   * Translation2d.
+   *
+   * @param time The Time measure to multiply by
+   * @return The resulting Translation2d
+   */
+  public Translation2d times(edu.wpi.first.units.measure.Time time) {
+    return new Translation2d(
+        this.x.in(MetersPerSecond) * time.in(edu.wpi.first.units.Units.Seconds),
+        this.y.in(MetersPerSecond) * time.in(edu.wpi.first.units.Units.Seconds));
+  }
+
   /**
    * Divides this TranslationalVelocity by a scalar and returns the result.
    *

src/main/java/com/team6962/lib/swerve/commands/TeleopSwerveCommand.java

@@ -2,11 +2,11 @@
 
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 
+import com.team6962.lib.commands.CommandUtil;
 import com.team6962.lib.math.TranslationalVelocity;
 import com.team6962.lib.swerve.CommandSwerveDrive;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 /**
@@ -43,12 +43,15 @@ public TeleopSwerveCommand(CommandSwerveDrive swerveDrive) {
 
     Trigger rotationTrigger =
         new Trigger(
-            () -> isScheduled() && isClearToOverride(swerveDrive.useRotation(), rotationCommand));
+            () ->
+                isScheduled()
+                    && CommandUtil.isClearToOverride(swerveDrive.useRotation(), rotationCommand));
     Trigger translationTrigger =
         new Trigger(
             () ->
                 isScheduled()
-                    && isClearToOverride(swerveDrive.useTranslation(), translationCommand));
+                    && CommandUtil.isClearToOverride(
+                        swerveDrive.useTranslation(), translationCommand));
 
     rotationTrigger.whileTrue(rotationCommand);
     translationTrigger.whileTrue(translationCommand);
@@ -70,19 +73,4 @@ protected CommandSwerveDrive getSwerveDrive() {
    * @return The desired field-relative chassis speeds
    */
   protected abstract ChassisSpeeds getDrivenVelocity();
-
-  /**
-   * Checks if it is okay to replace the currently active command on a subsystem, and have the given
-   * command take control instead. A subsystem is clear to override if it has no current command,
-   * has only its default command running, or is already running the specified command.
-   *
-   * @param subsystem The subsystem to check
-   * @param command The command that wants to take control
-   * @return True if the subsystem's active command can be safely overridden
-   */
-  public static boolean isClearToOverride(Subsystem subsystem, Command command) {
-    return subsystem.getCurrentCommand() == null
-        || subsystem.getCurrentCommand() == subsystem.getDefaultCommand()
-        || subsystem.getCurrentCommand() == command;
-  }
 }

src/main/java/com/team6962/lib/swerve/config/TimingConstants.java

@@ -31,6 +31,9 @@ public class TimingConstants implements Cloneable {
    */
   public boolean TimesyncControlRequests = false;
 
+  /** Whether to minimize logging to improve performance. */
+  public boolean MinimizeLogging = false;
+
   /** Constructs a TimingConstants object with default values. */
   public TimingConstants() {
     SignalUpdateRate = Hertz.of(100);
@@ -83,6 +86,17 @@ public TimingConstants withTimesyncControlRequests(boolean timesyncControlReques
     return this;
   }
 
+  /**
+   * Sets whether to minimize logging, and returns this TimingConstants for chaining.
+   *
+   * @param minimizeLogging True if logging should be minimized
+   * @return This TimingConstants object
+   */
+  public TimingConstants withMinimizeLogging(boolean minimizeLogging) {
+    MinimizeLogging = minimizeLogging;
+    return this;
+  }
+
   @Override
   public TimingConstants clone() {
     try {

src/main/java/com/team6962/lib/swerve/localization/Gyroscope.java

@@ -129,17 +129,20 @@ private boolean shouldUseGyroscope() {
   public void logTelemetry(String basePath) {
     basePath = LoggingUtil.ensureEndsWithSlash(basePath);
 
-    DogLog.log(basePath + "IsConnected", gyro.isConnected());
-    DogLog.log(basePath + "UsingGyroscope", shouldUseGyroscope());
-
-    DogLog.log(basePath + "Yaw", getYaw().in(Radians), Radians);
-    DogLog.log(basePath + "Pitch", getPitch().in(Radians), Radians);
-    DogLog.log(basePath + "Roll", getRoll().in(Radians), Radians);
-
-    DogLog.log(basePath + "YawVelocity", getYawVelocity().in(RadiansPerSecond), RadiansPerSecond);
-    DogLog.log(
-        basePath + "PitchVelocity", getPitchVelocity().in(RadiansPerSecond), RadiansPerSecond);
-    DogLog.log(basePath + "RollVelocity", getRollVelocity().in(RadiansPerSecond), RadiansPerSecond);
+    if (!constants.Timing.MinimizeLogging) {
+      DogLog.log(basePath + "IsConnected", gyro.isConnected());
+      DogLog.log(basePath + "UsingGyroscope", shouldUseGyroscope());
+
+      DogLog.log(basePath + "Yaw", getYaw().in(Radians), Radians);
+      DogLog.log(basePath + "Pitch", getPitch().in(Radians), Radians);
+      DogLog.log(basePath + "Roll", getRoll().in(Radians), Radians);
+
+      DogLog.log(basePath + "YawVelocity", getYawVelocity().in(RadiansPerSecond), RadiansPerSecond);
+      DogLog.log(
+          basePath + "PitchVelocity", getPitchVelocity().in(RadiansPerSecond), RadiansPerSecond);
+      DogLog.log(
+          basePath + "RollVelocity", getRollVelocity().in(RadiansPerSecond), RadiansPerSecond);
+    }
   }
 
   /**