From 276e05cf47008f7c9979ff368b51cb702b078dd3 Mon Sep 17 00:00:00 2001
From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
Date: Mon, 27 Jan 2025 15:46:06 -0500
Subject: [PATCH] Add velocity scalars for DT

---
 .../frc/robot/commands/DriveCommands.java     | 20 +++++++++---
 .../frc/robot/util/LoggedTunableNumber.java   | 31 +++++++++++++++++--
 2 files changed, 43 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
index bca47ba..26acb14 100644
--- a/src/main/java/frc/robot/commands/DriveCommands.java
+++ b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -15,6 +15,7 @@
 import frc.robot.subsystems.drive.DriveBase;
 import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.util.AllianceFlipUtil;
+import frc.robot.util.LoggedTunableNumber;
 import java.text.DecimalFormat;
 import java.text.NumberFormat;
 import java.util.LinkedList;
@@ -25,6 +26,12 @@
 public class DriveCommands {
   // Drive
   private static final double DEADBAND = 0.1;
+
+  private static final LoggedTunableNumber LINEAR_VELOCITY_SCALAR =
+      new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0, true);
+  private static final LoggedTunableNumber ANGULAR_VELOCITY_SCALAR =
+      new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0, true);
+
   private static final double ANGLE_KP = 5.0;
   private static final double ANGLE_KD = 0.4;
   private static final double ANGLE_MAX_VELOCITY = 8.0;
@@ -57,11 +64,13 @@ public static Command joystickDrive(
           omega = Math.copySign(Math.pow(omega, 2), omega);
 
           // Generate robot relative speeds
+          double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get();
+          double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get();
           var speeds =
               new ChassisSpeeds(
-                  x * DriveConstants.maxLinearVelocityMetersPerSec,
-                  y * DriveConstants.maxLinearVelocityMetersPerSec,
-                  omega * DriveConstants.maxAngularVelocityRadPerSec);
+                  x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar,
+                  y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar,
+                  omega * DriveConstants.maxAngularVelocityRadPerSec * angularVelocityScalar);
 
           // Convert to field relative
           Rotation2d rotation = RobotState.getInstance().getRotation();
@@ -109,10 +118,11 @@ public static Command joystickDriveAtAngle(
                       rotation.getRadians(), rotationSupplier.get().getRadians());
 
               // Generate robot relative speeds
+              double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get();
               var speeds =
                   new ChassisSpeeds(
-                      x * DriveConstants.maxLinearVelocityMetersPerSec,
-                      y * DriveConstants.maxLinearVelocityMetersPerSec,
+                      x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar,
+                      y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar,
                       omega);
 
               // Convert to field relative
diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java
index 71e4c88..5d7bfe7 100644
--- a/src/main/java/frc/robot/util/LoggedTunableNumber.java
+++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java
@@ -20,13 +20,38 @@ public class LoggedTunableNumber {
 
   private LoggedNetworkNumber dashboardNumber;
 
+  private final boolean ntPubEnabled;
+
   /**
    * Create a new LoggedTunableNumber
    *
    * @param dashboardKey Key on dashboard
+   * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode
    */
-  public LoggedTunableNumber(String dashboardKey) {
+  public LoggedTunableNumber(String dashboardKey, boolean alwaysEnabled) {
     this.key = tableKey + "/" + dashboardKey;
+    this.ntPubEnabled = alwaysEnabled;
+  }
+
+  /**
+   * Create a new LoggedTunableNumber
+   *
+   * @param dashboardKey Key on dashboard
+   */
+  public LoggedTunableNumber(String dashboardKey) {
+    this(dashboardKey, Constants.TUNING_MODE);
+  }
+
+  /**
+   * Create a new LoggedTunableNumber with the default value
+   *
+   * @param dashboardKey Key on dashboard
+   * @param defaultValue Default value
+   * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode
+   */
+  public LoggedTunableNumber(String dashboardKey, double defaultValue, boolean alwaysEnabled) {
+    this(dashboardKey, alwaysEnabled);
+    initDefault(defaultValue);
   }
 
   /**
@@ -55,7 +80,7 @@ public void initDefault(double defaultValue) {
     }
 
     this.defaultValue = defaultValue;
-    if (Constants.TUNING_MODE) {
+    if (ntPubEnabled) {
       dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
     }
   }
@@ -74,7 +99,7 @@ public double get() {
               key));
     }
 
-    return Constants.TUNING_MODE ? dashboardNumber.get() : defaultValue;
+    return ntPubEnabled ? dashboardNumber.get() : defaultValue;
   }
 
   /**